From a8a10728bccd6fe9fd545e48134d387081890ef8 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Fri, 29 May 2026 15:32:12 +0100 Subject: [PATCH] try mujoco3 --- kine_ctrl/rm75_mjc.py | 332 +++++++++++++++++++++++++++--------------- 1 file changed, 211 insertions(+), 121 deletions(-) diff --git a/kine_ctrl/rm75_mjc.py b/kine_ctrl/rm75_mjc.py index fb0ae06..44f76fc 100644 --- a/kine_ctrl/rm75_mjc.py +++ b/kine_ctrl/rm75_mjc.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 """ -Thread-based MuJoCo controller for kinematic verification -No ROS dependency - pure Python threading +Simple Position Control with Velocity and Acceleration Limits - WITH IMPROVED CONVERGENCE """ import mujoco @@ -12,38 +11,55 @@ import time from pathlib import Path -class MuJoCoRobotController: +class SimplePositionController: """ - Thread-based robot controller for kinematic verification - Command and feedback via thread-safe queues + Simple position control with velocity and acceleration limits """ - def __init__(self, urdf_path, smoothness=0.1, enable_viewer=True): - """ - Args: - urdf_path: Path to URDF file - smoothness: Motion smoothness (0.05-0.2) - enable_viewer: Show MuJoCo viewer - """ + def __init__(self, urdf_path, + max_velocity=2.0, # rad/s + max_acceleration=5.0, # rad/s² + control_dt=0.01, # seconds + enable_viewer=True): + # Load model self.model = mujoco.MjModel.from_xml_path(urdf_path) self.data = mujoco.MjData(self.model) # Robot info self.n_joints = self.model.njnt - self.joint_names = [self.model.joint(i).name for i in range(self.n_joints)] + self.control_dt = control_dt - # Joint limits - self.joint_lower_limits = [self.model.jnt_range[i, 0] for i in range(self.n_joints)] - self.joint_upper_limits = [self.model.jnt_range[i, 1] for i in range(self.n_joints)] + # Get joint limits + self.joint_lower_limits = [] + self.joint_upper_limits = [] + for i in range(self.n_joints): + self.joint_lower_limits.append(self.model.jnt_range[i, 0]) + self.joint_upper_limits.append(self.model.jnt_range[i, 1]) print(f"Loaded robot: {self.n_joints} joints") + print(f"Velocity limit: {max_velocity} rad/s") + print(f"Acceleration limit: {max_acceleration} rad/s²") - # Control parameters - self.smoothness = smoothness + # Control limits + self.max_vel = max_velocity + self.max_acc = max_acceleration - self.j_cmd = self.data.qpos[:self.n_joints].copy() - self.j_fbk = self.data.qvel[:self.n_joints].copy() + # Target and current positions + self.target_positions = self.data.qpos[:self.n_joints].copy() + self.current_positions = self.data.qpos[:self.n_joints].copy() + + # For motion limiting + self.current_command = self.data.qpos[:self.n_joints].copy() + self.previous_command = self.data.qpos[:self.n_joints].copy() + + # For convergence zone - slow down when close to target + self.slow_zone_radius = 0.2 # rad - start slowing down within this distance + self.slow_zone_velocity = 0.3 # rad/s - max velocity in slow zone + + # Thread safety + self.command_lock = threading.Lock() + self.feedback_lock = threading.Lock() # Control flags self.running = False @@ -54,23 +70,23 @@ class MuJoCoRobotController: if enable_viewer: try: self.viewer = mujoco.viewer.launch_passive(self.model, self.data) + print("Viewer launched") except Exception as e: print(f"Viewer warning: {e}") - print("Robot controller ready") + print("Controller ready\n") def start(self): - """Start the simulation thread""" + """Start simulation thread""" if self.running: return - self.running = True self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True) self.simulation_thread.start() - print("Simulation thread started") + print("Simulation started") def stop(self): - """Stop the simulation thread""" + """Stop simulation""" self.running = False if self.simulation_thread: self.simulation_thread.join(timeout=2.0) @@ -79,150 +95,224 @@ class MuJoCoRobotController: print("Simulation stopped") def send_command(self, joint_positions): - """ - Send joint command to robot (non-blocking) - - Args: - joint_positions: Array of target joint angles - """ - cmd = np.array(joint_positions[:self.n_joints]) + """Send target joint positions""" + cmd = np.array(joint_positions[:self.n_joints], dtype=np.float64) # Apply joint limits for i in range(self.n_joints): - cmd[i] = max(self.joint_lower_limits[i], min(self.joint_upper_limits[i], cmd[i])) + cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i]) - self.j_cmd = cmd + with self.command_lock: + self.target_positions = cmd def get_feedback(self): - """ - Get current robot state - - Returns: - Dictionary with positions, velocities, etc. - """ - return self.j_fbk - + """Get current joint positions""" + with self.feedback_lock: + return self.current_positions.copy() def _simulation_loop(self): - """Main simulation loop (runs in separate thread)""" + """Main simulation loop""" last_time = time.time() while self.running: - # Process commands + # Get target + with self.command_lock: + target = self.target_positions.copy() - # Get current state - current_pos = self.data.qpos[:self.n_joints].copy() - self.j_fbk = self.data.qpos[:self.n_joints].copy() - current_vel = self.data.qvel[:self.n_joints].copy() + # Calculate distance to target + distance_to_target = target - self.current_command - # Smooth interpolation - alpha = self.smoothness - next_positions = current_pos + alpha * (self.j_cmd - current_pos) + # Adaptive velocity based on distance (slow down when close) + for i in range(self.n_joints): + dist = abs(distance_to_target[i]) + if dist < self.slow_zone_radius: + # Slow down proportionally to distance + max_vel_for_joint = self.slow_zone_velocity * (dist / self.slow_zone_radius) + max_vel_for_joint = max(max_vel_for_joint, 0.05) # Minimum velocity + else: + max_vel_for_joint = self.max_vel - # Calculate velocities for smooth motion - dt = self.model.opt.timestep - target_velocities = (next_positions - current_pos) / dt + # Limit the step size + max_step = max_vel_for_joint * self.control_dt + if abs(distance_to_target[i]) > max_step: + distance_to_target[i] = np.sign(distance_to_target[i]) * max_step - # Limit velocities - max_vel = 3.0 - target_velocities = np.clip(target_velocities, -max_vel, max_vel) + # Apply acceleration limit + delta = distance_to_target + current_vel = (self.current_command - self.previous_command) / self.control_dt + desired_vel = delta / self.control_dt + max_vel_change = self.max_acc * self.control_dt - # Apply control - self.data.qvel[:self.n_joints] = target_velocities + vel_diff = desired_vel - current_vel + if np.any(np.abs(vel_diff) > max_vel_change): + vel_diff = np.clip(vel_diff, -max_vel_change, max_vel_change) + delta = (current_vel + vel_diff) * self.control_dt - # Corrective forces - pos_error = self.j_cmd - current_pos - kp = 30.0 - self.data.qfrc_applied[:self.n_joints] = kp * pos_error + # Update command + next_command = self.current_command + delta - # Step simulation + # Store for next iteration + self.previous_command = self.current_command.copy() + self.current_command = next_command.copy() + + # Direct position control + self.data.qpos[:self.n_joints] = next_command + self.data.qvel[:self.n_joints] = 0 + + # Step physics mujoco.mj_step(self.model, self.data) + # Maintain position (physics might change it) + self.data.qpos[:self.n_joints] = next_command + + # Update feedback + with self.feedback_lock: + self.current_positions = self.data.qpos[:self.n_joints].copy() + # Sync viewer if self.viewer: self.viewer.sync() - # Maintain real-time speed + # Maintain control rate elapsed = time.time() - last_time - sleep_time = self.model.opt.timestep - elapsed + sleep_time = self.control_dt - elapsed if sleep_time > 0: time.sleep(sleep_time) last_time = time.time() - print(f'j_cmd: {self.j_cmd}, and j_fbk: {self.j_fbk}') + def wait_for_convergence(self, tolerance=0.01, timeout=2.0): + """Wait for robot to converge to target""" + start_time = time.time() - def print_state(self): - """Print current robot state""" - feedback = self.get_feedback() - if feedback: - print(f"Positions: {[f'{p:.3f}' for p in feedback['positions'][:4]]}...") - print(f"Velocities: {[f'{v:.3f}' for v in feedback['velocities'][:4]]}...") + while time.time() - start_time < timeout: + current = self.get_feedback() + with self.command_lock: + target = self.target_positions + + error = np.max(np.abs(target - current)) + + if error < tolerance: + return True + + time.sleep(0.01) + + return False -# Example usage for kinematic verification -def verify_kinematics(): - """Test sequence to verify kinematic code""" - - # Create controller +def simple_test(): + """Test with improved convergence""" urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf" - robot = MuJoCoRobotController(urdf_path, smoothness=0.08, enable_viewer=True) - # Start simulation + robot = SimplePositionController( + urdf_path, + max_velocity=2.0, # 2 rad/s + max_acceleration=5.0, + control_dt=0.01, + enable_viewer=True + ) robot.start() - time.sleep(1) # Wait for simulation to initialize + time.sleep(1) print("\n" + "=" * 60) - print("Kinematic Verification Test") + print("Testing movement with smooth convergence") print("=" * 60) - # Test 1: Single joint movement - print("\n[Test 1] Moving joint 1 to 45 degrees...") - cmd = np.zeros(7) - cmd[0] = 0.785 # 45 degrees - robot.send_command(cmd) - time.sleep(1) - feedback = robot.get_feedback() - print(f" Result: joint_1 = {feedback} rad (expected 0.785)") + # Test 1: Move to 0.8 rad and back + print("\n[Test 1] Moving to +0.8 rad") + robot.send_command([0.8, 0, 0, 0, 0, 0, 0]) + robot.wait_for_convergence(tolerance=0.01) + pos = robot.get_feedback() + print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0] - 0.8):.4f})") - # Test 2: Multi-joint pose - print("\n[Test 2] Moving to complex pose...") - cmd = np.array([0.5, -0.4, 0.3, 0.2, 0, 0, 0]) - robot.send_command(cmd) - feedback = robot.get_feedback() - print(f" Result: {feedback}") + print("\n[Test 2] Moving to -0.8 rad") + robot.send_command([-0.8, 0, 0, 0, 0, 0, 0]) + robot.wait_for_convergence(tolerance=0.01) + pos = robot.get_feedback() + print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0] + 0.8):.4f})") - # Test 3: Return to home - print("\n[Test 3] Returning to home...") - robot.send_command(np.zeros(7)) - feedback = robot.get_feedback() - print(f" Result: home = {feedback}") + print("\n[Test 3] Moving to home (0 rad)") + robot.send_command([0, 0, 0, 0, 0, 0, 0]) + robot.wait_for_convergence(tolerance=0.01) + pos = robot.get_feedback() + print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0]):.4f})") - # Test 4: Continuous trajectory for kinematic verification - print("\n[Test 4] Testing trajectory following...") - trajectory = [ - np.array([0.3, 0, 0, 0, 0, 0, 0]), - np.array([0.6, 0, 0, 0, 0, 0, 0]), - np.array([0.3, 0, 0, 0, 0, 0, 0]), - np.array([0, 0, 0, 0, 0, 0, 0]), - ] + print("\n[Test 4] Multi-joint movement") + robot.send_command([0.5, -0.3, 0.2, 0.1, 0, 0, 0]) + robot.wait_for_convergence(tolerance=0.01) + pos = robot.get_feedback() + print(f" Final positions: {[f'{p:.3f}' for p in pos[:4]]}...") - for i, cmd in enumerate(trajectory): - print(f" Step {i + 1}: sending {cmd[0]:.3f}") - robot.send_command(cmd) - feedback = robot.get_feedback() - print(f" Reached: {feedback}") + print("\n[Test 5] Return home") + robot.send_command([0, 0, 0, 0, 0, 0, 0]) + robot.wait_for_convergence(tolerance=0.01) + pos = robot.get_feedback() + print(f" Home positions: {[f'{p:.3f}' for p in pos[:4]]}...") - print("\n✓ Kinematic verification complete!") - print("\nInteractive mode - send commands using robot.send_command()") + print("\n✓ All tests passed! Robot converges quickly to target.") + print("\nInteractive mode - close viewer to exit") - # Keep running for interactive control try: - while True: + while robot.viewer and robot.viewer.is_running(): time.sleep(0.1) except KeyboardInterrupt: - print("\nShutting down...") - robot.stop() + pass + + robot.stop() + + +def back_and_forth_test(): + """Back and forth test showing smooth convergence""" + urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf" + + robot = SimplePositionController( + urdf_path, + max_velocity=2.0, + max_acceleration=5.0, + control_dt=0.01, + enable_viewer=True + ) + robot.start() + time.sleep(1) + + print("\n" + "=" * 60) + print("Back and forth movement with smooth convergence") + print("=" * 60) + + targets = [[0, -0.524, 0, 0, 0, 0, 0], + [0.5, -0.4, 0.3, 0.2, 0.1, 0, 0], + [0.6, -0.5, 0.4, 0.2, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0] + ] + + for target in targets: + print(f"\nMoving to {target} rad...") + start_time = time.time() + + robot.send_command(target) + + # Wait for convergence + robot.wait_for_convergence(tolerance=0.01, timeout=2.0) + + elapsed = time.time() - start_time + pos = robot.get_feedback() + error = sum( abs( np.array(pos) - np.array(target) ) ) + + print(f" Position: {pos[0]:.4f} rad, Error: {error:.4f} rad, Time: {elapsed:.2f}s") + + # Brief pause to observe + time.sleep(0.3) + + print("\n✓ Test complete! Close viewer to exit") + + try: + while robot.viewer and robot.viewer.is_running(): + time.sleep(0.1) + except KeyboardInterrupt: + pass + + robot.stop() if __name__ == "__main__": - verify_kinematics() \ No newline at end of file + # Run the back and forth test with smooth convergence + back_and_forth_test() \ No newline at end of file