test pin for ik, and mujoco

kine_ctrl/rm75_mjc.py

@@ -18,7 +18,7 @@ class MuJoCoPositionController:
     No velocity commands, no forces - completely stable
     """
 
-    def __init__(self, urdf_path, smoothness=0.05, enable_viewer=True):
+    def __init__(self, urdf_path, smoothness=0.2, enable_viewer=True):
         """
         Args:
             urdf_path: Path to URDF file
@@ -29,6 +29,10 @@ class MuJoCoPositionController:
         self.model = mujoco.MjModel.from_xml_path(urdf_path)
         self.data = mujoco.MjData(self.model)
 
+        self.time_interval = 0.02
+
+        print(f'time interval: {self.model.opt.timestep}')
+
         # Robot info
         self.n_joints = self.model.njnt
 
@@ -55,6 +59,8 @@ class MuJoCoPositionController:
         self.feedback_lock = threading.Lock()
         self.current_feedback = self.data.qpos[:self.n_joints].copy()
 
+        self.max_pos_inc = 0.02
+
         # Control flags
         self.running = False
         self.simulation_thread = None
@@ -137,8 +143,7 @@ class MuJoCoPositionController:
             # This creates natural motion without velocity commands
             alpha = self.smoothness
 
-            delt_pos = np.clip( (target - current_positions), -0.02, 0.02)
-            next_positions = current_positions + alpha * delt_pos
+            next_positions = current_positions + np.clip(alpha * (target - current_positions) , -self.max_pos_inc, self.max_pos_inc)
 
             # DIRECT POSITION CONTROL - Set joint positions
             self.data.qpos[:self.n_joints] = next_positions
@@ -165,7 +170,7 @@ class MuJoCoPositionController:
 
             # Maintain real-time speed
             elapsed = time.time() - last_time
-            sleep_time = self.model.opt.timestep - elapsed
+            sleep_time = self.time_interval - elapsed
             if sleep_time > 0:
                 time.sleep(sleep_time)
             last_time = time.time()
@@ -185,7 +190,7 @@ class MuJoCoPositionController:
         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])
 
-        n_steps = int(duration / self.model.opt.timestep)
+        n_steps = int(duration / self.time_interval)
 
         print(f"  Moving over {duration}s ({n_steps} steps)")
 
@@ -195,7 +200,7 @@ class MuJoCoPositionController:
             ease_alpha = 1 - (1 - alpha) ** 2  # Quadratic ease-out
             current_target = start_pos + ease_alpha * (end_pos - start_pos)
             self.send_command(current_target)
-            time.sleep(self.model.opt.timestep)
+            time.sleep(self.time_interval)
 
         # Ensure exact target
         self.send_command(end_pos)
@@ -273,16 +278,6 @@ def demo_position_control():
     robot.wait_until_reached()
     robot.print_state()
 
-    print("\n[Test 5] Smooth trajectory using move_to_position")
-    robot.move_to_position([0.6, -0.5, 0.4, 0.2, 0, 0, 0], duration=2.0)
-    robot.wait_until_reached()
-    robot.print_state()
-
-    print("\n[Test 6] Back home with smooth motion")
-    robot.move_to_position([0, 0, 0, 0, 0, 0, 0], duration=2.0)
-    robot.wait_until_reached()
-    robot.print_state()
-
     print("\n" + "=" * 60)
     print("✓ All tests passed! Robot is stable and controllable.")
     print("=" * 60)
@@ -297,53 +292,7 @@ def demo_position_control():
     robot.stop()
 
 
-# Simple usage for your kinematic code
-def example_for_kinematic_code():
-    """Example of how to use with your kinematic solver"""
-
-    urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
-    robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
-    robot.start()
-
-    # Your kinematic solver would compute joint targets like this:
-    def your_kinematic_solver(target_pose):
-        """
-        Your kinematic code here
-        Returns joint positions array of length 7
-        """
-        # Example output - replace with your actual kinematics
-        return np.array([0.5, -0.3, 0.2, 0.1, 0, 0, 0])
-
-    # Example usage
-    target_pose = "some pose"  # Your pose input
-
-    # Compute joint targets using your kinematics
-    joint_targets = your_kinematic_solver(target_pose)
-
-    # Send to simulation
-    robot.send_command(joint_targets)
-
-    # Wait for robot to reach target
-    robot.wait_until_reached()
-
-    # Read actual positions
-    actual_positions = robot.get_feedback()
-
-    # Verify your kinematics
-    error = np.max(np.abs(joint_targets - actual_positions))
-    print(f"Kinematic verification error: {error:.6f} rad")
-
-    # Keep running
-    try:
-        while robot.viewer and robot.viewer.is_running():
-            time.sleep(0.1)
-    except KeyboardInterrupt:
-        pass
-
-    robot.stop()
 
 
 if __name__ == "__main__":
     demo_position_control()
-    # Uncomment to test with your kinematic code:
-    # example_for_kinematic_code()