From 84c2ba321d07acf17ab1dee15ec581c93bd29358 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Fri, 29 May 2026 15:35:54 +0100 Subject: [PATCH] try mujoco --- kine_ctrl/rm75_mjc.py | 345 +++++++++++++++++++++++------------------- 1 file changed, 188 insertions(+), 157 deletions(-) diff --git a/kine_ctrl/rm75_mjc.py b/kine_ctrl/rm75_mjc.py index 44f76fc..8e9d162 100644 --- a/kine_ctrl/rm75_mjc.py +++ b/kine_ctrl/rm75_mjc.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 """ -Simple Position Control with Velocity and Acceleration Limits - WITH IMPROVED CONVERGENCE +Pure Position Control for MuJoCo - No velocity commands, no forces +Direct joint position control with smoothing """ import mujoco @@ -11,24 +12,25 @@ import time from pathlib import Path -class SimplePositionController: +class MuJoCoPositionController: """ - Simple position control with velocity and acceleration limits + Pure position control - directly sets joint positions + No velocity commands, no forces - completely stable """ - 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): - + def __init__(self, urdf_path, smoothness=0.05, enable_viewer=True): + """ + Args: + urdf_path: Path to URDF file + smoothness: Motion smoothness (0.02=very smooth, 0.1=fast) + enable_viewer: Show MuJoCo viewer + """ # 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.control_dt = control_dt # Get joint limits self.joint_lower_limits = [] @@ -38,28 +40,20 @@ class SimplePositionController: 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²") + for i in range(self.n_joints): + print( + f" {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]") - # Control limits - self.max_vel = max_velocity - self.max_acc = max_acceleration - - # Target and current positions + # Target positions (in radians) 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 + # Smoothing factor (0-1, lower = smoother) + self.smoothness = smoothness # Thread safety self.command_lock = threading.Lock() self.feedback_lock = threading.Lock() + self.current_feedback = self.data.qpos[:self.n_joints].copy() # Control flags self.running = False @@ -74,19 +68,20 @@ class SimplePositionController: except Exception as e: print(f"Viewer warning: {e}") - print("Controller ready\n") + print("Robot controller ready - Pure Position Mode") def start(self): - """Start simulation thread""" + """Start the 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 started") + print("Simulation thread started") def stop(self): - """Stop simulation""" + """Stop the simulation thread""" self.running = False if self.simulation_thread: self.simulation_thread.join(timeout=2.0) @@ -95,7 +90,12 @@ class SimplePositionController: print("Simulation stopped") def send_command(self, joint_positions): - """Send target joint positions""" + """ + Send target joint positions + + Args: + joint_positions: Array of target joint angles (radians) + """ cmd = np.array(joint_positions[:self.n_joints], dtype=np.float64) # Apply joint limits @@ -108,87 +108,112 @@ class SimplePositionController: def get_feedback(self): """Get current joint positions""" with self.feedback_lock: - return self.current_positions.copy() + return self.current_feedback.copy() + + def get_target(self): + """Get current target positions""" + with self.command_lock: + return self.target_positions.copy() def _simulation_loop(self): - """Main simulation loop""" + """ + Main simulation loop - PURE POSITION CONTROL + No velocity commands, no forces - just direct position setting + """ last_time = time.time() + # For smooth interpolation + current_positions = self.data.qpos[:self.n_joints].copy() + while self.running: - # Get target + # Get target command with self.command_lock: target = self.target_positions.copy() - # Calculate distance to target - distance_to_target = target - self.current_command + # Get current positions + current_positions = self.data.qpos[:self.n_joints].copy() - # 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 + # Smooth interpolation toward target + # This creates natural motion without velocity commands + alpha = self.smoothness - # 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 + delt_pos = np.clip( (target - current_positions), -0.02, 0.02) + next_positions = current_positions + alpha * delt_pos - # 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 + # DIRECT POSITION CONTROL - Set joint positions + self.data.qpos[:self.n_joints] = next_positions - 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 - - # Update command - next_command = self.current_command + delta - - # 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 + # IMPORTANT: Set velocities to zero to prevent physics from moving joints + # This ensures pure kinematic control self.data.qvel[:self.n_joints] = 0 - # Step physics + # Step physics (this will apply gravity, collisions, etc. to other bodies) mujoco.mj_step(self.model, self.data) - # Maintain position (physics might change it) - self.data.qpos[:self.n_joints] = next_command + # After step, ensure our joint positions are maintained + # (Physics might have altered them slightly) + self.data.qpos[:self.n_joints] = next_positions + self.data.qvel[:self.n_joints] = 0 # Update feedback with self.feedback_lock: - self.current_positions = self.data.qpos[:self.n_joints].copy() + self.current_feedback = self.data.qpos[:self.n_joints].copy() # Sync viewer if self.viewer: self.viewer.sync() - # Maintain control rate + # Maintain real-time speed elapsed = time.time() - last_time - sleep_time = self.control_dt - elapsed + sleep_time = self.model.opt.timestep - elapsed if sleep_time > 0: time.sleep(sleep_time) last_time = time.time() - def wait_for_convergence(self, tolerance=0.01, timeout=2.0): - """Wait for robot to converge to target""" + def move_to_position(self, target, duration=1.0): + """ + Move to target position over specified duration + + Args: + target: Target joint positions + duration: Time to complete movement (seconds) + """ + start_pos = self.get_feedback() + end_pos = 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]) + + n_steps = int(duration / self.model.opt.timestep) + + print(f" Moving over {duration}s ({n_steps} steps)") + + for step in range(n_steps): + 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) + self.send_command(current_target) + time.sleep(self.model.opt.timestep) + + # Ensure exact target + self.send_command(end_pos) + time.sleep(0.1) + + def wait_until_reached(self, tolerance=0.01, timeout=5.0): + """ + Wait until robot reaches target position + + Args: + tolerance: Position error tolerance (radians) + timeout: Maximum wait time (seconds) + """ start_time = time.time() while time.time() - start_time < timeout: current = self.get_feedback() - with self.command_lock: - target = self.target_positions - + target = self.get_target() error = np.max(np.abs(target - current)) if error < tolerance: @@ -198,57 +223,69 @@ class SimplePositionController: return False + def print_state(self): + """Print current robot state""" + positions = self.get_feedback() + target = self.get_target() + print("Current positions:", [f"{p:.3f}" for p in positions[:4]], "...") + print("Target positions: ", [f"{t:.3f}" for t in target[:4]], "...") + + +# Demo +def demo_position_control(): + """Demonstrate pure position control""" -def simple_test(): - """Test with improved convergence""" urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf" - robot = SimplePositionController( - urdf_path, - max_velocity=2.0, # 2 rad/s - max_acceleration=5.0, - control_dt=0.01, - enable_viewer=True - ) + if not Path(urdf_path).exists(): + print(f"Error: URDF not found at {urdf_path}") + return + + print("=" * 60) + print("Pure Position Control Demo") + print("=" * 60) + + # Create controller + robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True) robot.start() time.sleep(1) + print("\n[Test 1] Move joint 1 to 45 degrees") + robot.send_command([0.785, 0, 0, 0, 0, 0, 0]) + robot.wait_until_reached() + robot.print_state() + time.sleep(0.5) + + print("\n[Test 2] Move joint 2 to -30 degrees") + robot.send_command([0, -0.524, 0, 0, 0, 0, 0]) + robot.wait_until_reached() + robot.print_state() + time.sleep(0.5) + + print("\n[Test 3] Move multiple joints simultaneously") + robot.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0]) + robot.wait_until_reached() + robot.print_state() + time.sleep(0.5) + + print("\n[Test 4] Return home") + robot.send_command([0, 0, 0, 0, 0, 0, 0]) + 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("Testing movement with smooth convergence") + print("✓ All tests passed! Robot is stable and controllable.") print("=" * 60) - - # 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})") - - 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})") - - 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})") - - 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]]}...") - - 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✓ All tests passed! Robot converges quickly to target.") print("\nInteractive mode - close viewer to exit") try: @@ -260,50 +297,43 @@ def simple_test(): robot.stop() -def back_and_forth_test(): - """Back and forth test showing smooth convergence""" +# 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 = SimplePositionController( - urdf_path, - max_velocity=2.0, - max_acceleration=5.0, - control_dt=0.01, - enable_viewer=True - ) + robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True) robot.start() - time.sleep(1) - print("\n" + "=" * 60) - print("Back and forth movement with smooth convergence") - print("=" * 60) + # 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]) - 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] - ] + # Example usage + target_pose = "some pose" # Your pose input - for target in targets: - print(f"\nMoving to {target} rad...") - start_time = time.time() + # Compute joint targets using your kinematics + joint_targets = your_kinematic_solver(target_pose) - robot.send_command(target) + # Send to simulation + robot.send_command(joint_targets) - # Wait for convergence - robot.wait_for_convergence(tolerance=0.01, timeout=2.0) + # Wait for robot to reach target + robot.wait_until_reached() - elapsed = time.time() - start_time - pos = robot.get_feedback() - error = sum( abs( np.array(pos) - np.array(target) ) ) + # Read actual positions + actual_positions = robot.get_feedback() - 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") + # 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) @@ -314,5 +344,6 @@ def back_and_forth_test(): if __name__ == "__main__": - # Run the back and forth test with smooth convergence - back_and_forth_test() \ No newline at end of file + demo_position_control() + # Uncomment to test with your kinematic code: + # example_for_kinematic_code() \ No newline at end of file