From 31f64949e20a9f3ceecd196c4ac3c8cf2921c834 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Mon, 1 Jun 2026 22:04:49 +0100 Subject: [PATCH] test pin for ik, and mujoco --- kine_ctrl/main.py | 65 +++++++++- kine_ctrl/rm75_mjc.py | 73 ++---------- kine_ctrl/rm75_mujoco.py | 250 --------------------------------------- 3 files changed, 73 insertions(+), 315 deletions(-) delete mode 100644 kine_ctrl/rm75_mujoco.py diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index b779403..36bd754 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -1,13 +1,72 @@ from test_pin import KinematicsSolver as controller +from rm75_mjc import MuJoCoPositionController +import time +from pathlib import Path + +def demo_position_control(): + """Demonstrate pure position control""" + + urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf" + + 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" + "=" * 60) + print("✓ All tests passed! Robot is stable and controllable.") + print("=" * 60) + print("\nInteractive mode - close viewer to exit") + + try: + while robot.viewer and robot.viewer.is_running(): + time.sleep(0.1) + except KeyboardInterrupt: + pass + + robot.stop() def main(): - kine_node = controller() - kine_node.loop_run() - print("main get returned kine_node") + demo_position_control() + + # kine_node = controller() + # kine_node.loop_run() + # print("main get returned kine_node") if __name__ == "__main__": diff --git a/kine_ctrl/rm75_mjc.py b/kine_ctrl/rm75_mjc.py index 8e9d162..0cc3c7d 100644 --- a/kine_ctrl/rm75_mjc.py +++ b/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() \ No newline at end of file diff --git a/kine_ctrl/rm75_mujoco.py b/kine_ctrl/rm75_mujoco.py deleted file mode 100644 index 4a29204..0000000 --- a/kine_ctrl/rm75_mujoco.py +++ /dev/null @@ -1,250 +0,0 @@ -#!/usr/bin/env python3 -""" -RM75 Robot Controller for URDF without actuators -Direct joint position control (kinematic mode) -""" - -import mujoco -import mujoco.viewer -import numpy as np -import time -from pathlib import Path - - -class RM75Controller: - def __init__(self, urdf_path: str ="/home/zl/Downloads/urdf_rm75/RM75-B.urdf", enable_viewer: bool = True): - """ - Initialize RM75 robot simulation from URDF - - Args: - urdf_path: Path to RM75-B.urdf file - enable_viewer: Show visualization window - """ - # 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.n_actuators = self.model.nu - - print(f"✓ Loaded RM75 robot") - print(f" - Joints: {self.n_joints}") - print(f" - Actuators: {self.n_actuators} (using direct joint control)") - print(f" - Bodies: {self.model.nbody}") - - # Get joint names for reference - self.joint_names = [] - for i in range(self.n_joints): - self.joint_names.append(self.model.joint(i).name) - print(f" - Joints: {', '.join(self.joint_names)}") - - # Store home position (current joint angles) - self.home_position = self.data.qpos[:self.n_joints].copy() - print(f" - Home position: {self.home_position}") - - # For position control without actuators, we'll use qpos directly - self.use_actuators = self.n_actuators > 0 - - # Viewer - self.viewer = None - if enable_viewer: - try: - self.viewer = mujoco.viewer.launch_passive(self.model, self.data) - print("✓ Viewer launched successfully") - except Exception as e: - print(f"Warning: Could not launch viewer: {e}") - self.viewer = None - - def get_joint_positions(self): - """Get current joint angles (radians)""" - return self.data.qpos[:self.n_joints].copy() - - def get_joint_velocities(self): - """Get current joint velocities (rad/s)""" - return self.data.qvel[:self.n_joints].copy() - - def get_end_effector_pose(self): - """Get end-effector position and orientation""" - # Last body is usually end-effector - end_effector_id = self.model.nbody - 1 - position = self.data.xpos[end_effector_id].copy() - orientation = self.data.xmat[end_effector_id].copy().reshape(3, 3) - return position, orientation - - def set_joint_positions(self, positions): - """ - Set joint positions directly (kinematic control) - - Args: - positions: Target joint angles in radians (length should match n_joints) - """ - if len(positions) != self.n_joints: - print(f"Warning: Expected {self.n_joints} joints, got {len(positions)}") - positions = positions[:self.n_joints] - - # Directly set joint positions (kinematic control) - self.data.qpos[:self.n_joints] = positions - - # Also set velocities to zero to avoid unwanted motion - self.data.qvel[:self.n_joints] = 0 - - def step(self): - """Advance simulation one step""" - mujoco.mj_step(self.model, self.data) - if self.viewer: - self.viewer.sync() - - def step_n(self, n_steps: int): - """Advance simulation by N steps""" - for _ in range(n_steps): - self.step() - - def move_to_position(self, target_positions, steps=500): - """ - Smoothly move to target joint positions - - Args: - target_positions: Target joint angles - steps: Number of simulation steps for the movement - """ - current = self.get_joint_positions() - target = np.array(target_positions[:self.n_joints]) - - for i in range(steps): - # Linear interpolation - alpha = (i + 1) / steps - positions = current + alpha * (target - current) - self.set_joint_positions(positions) - self.step() - - # Ensure exact target - self.set_joint_positions(target) - self.step_n(10) - - def run_trajectory(self, trajectory_points, steps_between_points=500): - """ - Execute a joint trajectory - - Args: - trajectory_points: List of joint position arrays - steps_between_points: Steps between each point - """ - print(f"Executing trajectory with {len(trajectory_points)} points...") - - for i, target in enumerate(trajectory_points): - print(f" Moving to point {i + 1}/{len(trajectory_points)}") - self.move_to_position(target, steps_between_points) - - print("✓ Trajectory complete") - - def run_forever(self, dt=0.01): - """Run simulation with real-time control loop""" - print("\n✓ Simulation running. Close viewer window to exit.\n") - - try: - while self.viewer and self.viewer.is_running(): - self.step() - time.sleep(dt) - except KeyboardInterrupt: - print("\n✓ Stopped by user") - finally: - if self.viewer: - self.viewer.close() - - def print_state(self): - """Print current robot state""" - positions = self.get_joint_positions() - print(f"Joint positions (rad): {[f'{p:.3f}' for p in positions]}") - - -# Test trajectories for RM75 -def create_sine_wave_trajectory(controller, duration_seconds=5, frequency=0.5): - """Create a sine wave trajectory for testing""" - steps = int(duration_seconds / controller.model.opt.timestep) - trajectory = [] - - for i in range(steps): - t = i * controller.model.opt.timestep - positions = controller.home_position.copy() - - # Create sine wave motion on first 3 joints (shoulder, elbow, wrist) - positions[0] = controller.home_position[0] + 0.5 * np.sin(2 * np.pi * frequency * t) - positions[1] = controller.home_position[1] + 0.3 * np.sin(2 * np.pi * frequency * t + 1.0) - positions[2] = controller.home_position[2] + 0.2 * np.sin(2 * np.pi * frequency * t + 2.0) - - trajectory.append(positions) - - return trajectory - - -if __name__ == "__main__": - # Path to your URDF - urdf_file = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf" - - # Check if file exists - if not Path(urdf_file).exists(): - print(f"Error: URDF file not found at {urdf_file}") - exit(1) - - # Create robot controller - print("Initializing RM75 Controller...") - robot = RM75Controller(urdf_file, enable_viewer=True) - - if robot.viewer is None: - print("Error: Could not initialize viewer. Running without visualization.") - - # Give time for viewer to initialize - time.sleep(1) - - # Test 1: Print current state - print("\n>>> Current robot state:") - robot.print_state() - - # Test 2: Move to home position - print("\n>>> Moving to home position...") - robot.move_to_position(robot.home_position, steps=300) - robot.print_state() - - # Test 3: Create and execute a pose sequence - print("\n>>> Testing different poses...") - - # Pose 1: Slightly raised arm - pose1 = robot.home_position.copy() - pose1[0] = 0.5 # Joint 1 - pose1[1] = -0.3 # Joint 2 - pose1[2] = 0.2 # Joint 3 - - # Pose 2: Extended arm - pose2 = robot.home_position.copy() - pose2[0] = 0.8 - pose2[1] = -0.5 - pose2[2] = 0.4 - pose2[3] = 0.3 # Joint 4 - - # Pose 3: Folded position - pose3 = robot.home_position.copy() - pose3[0] = -0.5 - pose3[1] = 0.3 - pose3[2] = -0.2 - - trajectory = [pose1, pose2, pose3, robot.home_position] - robot.run_trajectory(trajectory, steps_between_points=400) - - # Test 4: Continuous sine wave motion (5 seconds) - print("\n>>> Starting sine wave motion (5 seconds)...") - sine_trajectory = create_sine_wave_trajectory(robot, duration_seconds=5, frequency=0.8) - robot.run_trajectory(sine_trajectory, steps_between_points=1) - - # Return to home - print("\n>>> Returning to home position...") - robot.move_to_position(robot.home_position, steps=300) - - print("\n✓ Demo complete!") - - # Keep viewer open until user closes - if robot.viewer: - print("\nPress Ctrl+C in terminal to exit, or close the viewer window.") - robot.run_forever() - else: - print("\nSimulation completed.") \ No newline at end of file