update the mjc function names

kine_ctrl/main.py

@@ -109,7 +109,7 @@ def demo_position_control():
 
         solve_sum = 0
 
-        for i in range(2000):
+        for i in range(10):
             print(f'\n-------------- in i = {i} ----------------')
             joint_rand = np.random.uniform(ub/180*pi, lb/180*pi)
             print(f'the predefined joints are {joint_rand}')
@@ -146,6 +146,10 @@ def demo_position_control():
                 print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
                 if d_p_ik < 0.01:
                     result[0][1] += 1
+
+                robot_mjk.send_command(joint_solution)
+                robot_mjk.wait_until_reached()
+                robot_mjk.print_state()
             else:
                 fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)
                 fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0)

kine_ctrl/rm75_mjc.py

@@ -48,8 +48,8 @@ class MuJoCoPositionController:
             print(
                 f"  {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]")
 
-        # Target positions (in radians)
+        # Target joint angles (in radians)
-        self.target_positions = self.data.qpos[:self.n_joints].copy()
+        self.target_joints = self.data.qpos[:self.n_joints].copy()
 
         # Smoothing factor (0-1, lower = smoother)
         self.smoothness = smoothness
@@ -57,9 +57,9 @@ class MuJoCoPositionController:
         # Thread safety
         self.command_lock = threading.Lock()
         self.feedback_lock = threading.Lock()
-        self.current_feedback = self.data.qpos[:self.n_joints].copy()
+        self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
 
-        self.max_pos_inc = 0.02
+        self.max_ang_inc = 0.02
 
         # Control flags
         self.running = False
@@ -109,17 +109,17 @@ class MuJoCoPositionController:
             cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
 
         with self.command_lock:
-            self.target_positions = cmd
+            self.target_joints = cmd
 
     def get_feedback(self):
         """Get current joint positions"""
         with self.feedback_lock:
-            return self.current_feedback.copy()
+            return self.current_feedback_joint.copy()
 
     def get_target(self):
         """Get current target positions"""
         with self.command_lock:
-            return self.target_positions.copy()
+            return self.target_joints.copy()
 
     def _simulation_loop(self):
         """
@@ -129,24 +129,24 @@ class MuJoCoPositionController:
         last_time = time.time()
 
         # For smooth interpolation
-        current_positions = self.data.qpos[:self.n_joints].copy()
+        current_joints = self.data.qpos[:self.n_joints].copy()
 
         while self.running:
             # Get target command
             with self.command_lock:
-                target = self.target_positions.copy()
+                target = self.target_joints.copy()
 
             # Get current positions
-            current_positions = self.data.qpos[:self.n_joints].copy()
+            current_joints = self.data.qpos[:self.n_joints].copy()
 
             # Smooth interpolation toward target
             # This creates natural motion without velocity commands
             alpha = self.smoothness
 
-            next_positions = current_positions + np.clip(alpha * (target - current_positions) , -self.max_pos_inc, self.max_pos_inc)
+            next_joints = current_joints + np.clip(alpha * (target - current_joints) , -self.max_ang_inc, self.max_ang_inc)
 
             # DIRECT POSITION CONTROL - Set joint positions
-            self.data.qpos[:self.n_joints] = next_positions
+            self.data.qpos[:self.n_joints] = next_joints
 
             # IMPORTANT: Set velocities to zero to prevent physics from moving joints
             # This ensures pure kinematic control
@@ -157,12 +157,12 @@ class MuJoCoPositionController:
 
             # After step, ensure our joint positions are maintained
             # (Physics might have altered them slightly)
-            self.data.qpos[:self.n_joints] = next_positions
+            self.data.qpos[:self.n_joints] = next_joints
             self.data.qvel[:self.n_joints] = 0
 
             # Update feedback
             with self.feedback_lock:
-                self.current_feedback = self.data.qpos[:self.n_joints].copy()
+                self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
 
             # Sync viewer
             if self.viewer:
@@ -175,20 +175,20 @@ class MuJoCoPositionController:
                 time.sleep(sleep_time)
             last_time = time.time()
 
-    def move_to_position(self, target, duration=1.0):
+    def move_to_joints(self, target, duration=1.0):
         """
-        Move to target position over specified duration
+        Move to target joints over specified duration
 
         Args:
-            target: Target joint positions
+            target: Target joint joints
             duration: Time to complete movement (seconds)
         """
-        start_pos = self.get_feedback()
+        start_js = self.get_feedback()
-        end_pos = np.array(target[:self.n_joints])
+        end_js = np.array(target[:self.n_joints])
 
         # Apply limits
         for i in range(self.n_joints):
-            end_pos[i] = np.clip(end_pos[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
+            end_js[i] = np.clip(end_js[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
 
         n_steps = int(duration / self.time_interval)
 
@@ -198,15 +198,15 @@ class MuJoCoPositionController:
             alpha = (step + 1) / n_steps
             # Use easing for smoother motion
             ease_alpha = 1 - (1 - alpha) ** 2  # Quadratic ease-out
-            current_target = start_pos + ease_alpha * (end_pos - start_pos)
+            current_target = start_js + ease_alpha * (end_js - start_js)
             self.send_command(current_target)
             time.sleep(self.time_interval)
 
         # Ensure exact target
-        self.send_command(end_pos)
+        self.send_command(end_js)
         time.sleep(0.1)
 
-    def wait_until_reached(self, tolerance=0.01, timeout=5.0):
+    def wait_until_reached(self, tolerance=0.01, timeout=10.0):
         """
         Wait until robot reaches target position
 
@@ -230,10 +230,10 @@ class MuJoCoPositionController:
 
     def print_state(self):
         """Print current robot state"""
-        positions = self.get_feedback()
+        joints = self.get_feedback()
         target = self.get_target()
-        print("Current positions:", [f"{p:.3f}" for p in positions[:4]], "...")
+        print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...")
-        print("Target positions: ", [f"{t:.3f}" for t in target[:4]], "...")
+        print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...")
 
 
 # Demo