From c23b5d0d6de5ed538845871cf7f8ac4536c7299b Mon Sep 17 00:00:00 2001
From: LiuzhengSJ <zhengliu@sjtu.edu.cn>
Date: Fri, 29 May 2026 14:21:03 +0100
Subject: [PATCH] try mujoco2

---
 kine_ctrl/rm75_mjc.py | 459 +++++++++++++++---------------------------
 1 file changed, 161 insertions(+), 298 deletions(-)

diff --git a/kine_ctrl/rm75_mjc.py b/kine_ctrl/rm75_mjc.py
index 575fc7c..fb0ae06 100644
--- a/kine_ctrl/rm75_mjc.py
+++ b/kine_ctrl/rm75_mjc.py
@@ -1,29 +1,29 @@
 #!/usr/bin/env python3
 """
-RM75 Robot Controller with True Dynamics for URDF without Actuators
-Run this in your coppeliasim conda environment
+Thread-based MuJoCo controller for kinematic verification
+No ROS dependency - pure Python threading
 """
 
 import mujoco
 import mujoco.viewer
 import numpy as np
+import threading
 import time
 from pathlib import Path
-from scipy.signal import butter, lfilter
 
 
-class RM75Controller:
-    def __init__(self, urdf_path: str, enable_viewer: bool = True,
-                 control_mode='torque',  # 'torque' or 'position'
-                 low_pass_cutoff_hz=10.0):
+class MuJoCoRobotController:
+    """
+    Thread-based robot controller for kinematic verification
+    Command and feedback via thread-safe queues
+    """
+
+    def __init__(self, urdf_path, smoothness=0.1, enable_viewer=True):
         """
-        Initialize RM75 robot simulation with dynamic control
-
         Args:
-            urdf_path: Path to RM75-B.urdf file
-            enable_viewer: Show visualization window
-            control_mode: 'torque' (realistic dynamics) or 'position' (kinematic)
-            low_pass_cutoff_hz: Cutoff frequency for command filtering (Hz)
+            urdf_path: Path to URDF file
+            smoothness: Motion smoothness (0.05-0.2)
+            enable_viewer: Show MuJoCo viewer
         """
         # Load model
         self.model = mujoco.MjModel.from_xml_path(urdf_path)
@@ -31,335 +31,198 @@ class RM75Controller:
 
         # Robot info
         self.n_joints = self.model.njnt
-        self.n_actuators = self.model.nu
+        self.joint_names = [self.model.joint(i).name for i in range(self.n_joints)]
 
-        print(f"✓ Loaded RM75 robot")
-        print(f"  - Joints: {self.n_joints}")
-        print(f"  - Actuators in URDF: {self.n_actuators}")
-        print(f"  - Bodies: {self.model.nbody}")
+        # 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 names and limits
-        self.joint_names = []
-        self.joint_lower_limits = []
-        self.joint_upper_limits = []
-
-        for i in range(self.n_joints):
-            self.joint_names.append(self.model.joint(i).name)
-            # Get joint limits from URDF (range is [lower, upper])
-            self.joint_lower_limits.append(self.model.jnt_range[i, 0])
-            self.joint_upper_limits.append(self.model.jnt_range[i, 1])
-
-        print(f"  - Joint limits: {self.joint_lower_limits[0]:.2f} to {self.joint_upper_limits[0]:.2f} rad for joint_1")
-
-        # If no actuators, we need to add them programmatically
-        if self.n_actuators == 0:
-            print("  No actuators in URDF. Adding torque actuators for dynamic control...")
-            self._add_actuators_programmatically()
+        print(f"Loaded robot: {self.n_joints} joints")
 
         # Control parameters
-        self.control_mode = control_mode
+        self.smoothness = smoothness
 
-        # PD gains for torque control (tuned for realistic motion)
-        self.kp = np.array([200.0, 200.0, 150.0, 100.0, 80.0, 60.0, 50.0])  # Proportional gains
-        self.kd = np.array([20.0, 20.0, 15.0, 10.0, 8.0, 6.0, 5.0])  # Derivative gains
+        self.j_cmd = self.data.qpos[:self.n_joints].copy()
+        self.j_fbk = self.data.qvel[:self.n_joints].copy()
 
-        # State variables
-        self.desired_positions = self.get_joint_positions().copy()
+        # Control flags
+        self.running = False
+        self.simulation_thread = None
 
-        # Low-pass filter for smooth commands
-        self.lp_cutoff = low_pass_cutoff_hz
-        self.filtered_commands = self.get_joint_positions().copy()
-
-        # Home position
-        self.home_position = self.get_joint_positions().copy()
-        print(f"  - Home position: {self.home_position}")
-
-        # Setup viewer
+        # 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
+                print(f"Viewer warning: {e}")
 
-    def _add_actuators_programmatically(self):
-        """Add torque actuators programmatically to enable dynamic control"""
-        # Save current model state
-        n_new_actuators = self.n_joints
+        print("Robot controller ready")
 
-        # We need to create a new model with actuators
-        # For now, we'll use direct qpos control with custom dynamics
-        # This is a workaround by setting control_mode to 'position' for kinematic control
-        # and using manual velocity/acceleration limits
+    def start(self):
+        """Start the simulation thread"""
+        if self.running:
+            return
 
-        print(f"  Adding {n_new_actuators} virtual torque actuators")
+        self.running = True
+        self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True)
+        self.simulation_thread.start()
+        print("Simulation thread started")
 
-        # Alternative approach: Use qfrc_applied to apply forces directly
-        # This bypasses the need for actuators in the URDF
-        self.use_qfrc_applied = True
-        self.n_actuators = n_new_actuators
+    def stop(self):
+        """Stop the simulation thread"""
+        self.running = False
+        if self.simulation_thread:
+            self.simulation_thread.join(timeout=2.0)
+        if self.viewer:
+            self.viewer.close()
+        print("Simulation stopped")
 
-        # Create virtual control array
-        self.virtual_ctrl = np.zeros(self.n_joints)
-
-    def _low_pass_filter(self, new_target):
-        """Apply simple first-order low-pass filter to target positions"""
-        dt = self.model.opt.timestep
-        alpha = 2 * np.pi * self.lp_cutoff * dt
-        alpha = min(alpha, 1.0)  # Clamp for stability
-
-        for i in range(self.n_joints):
-            self.filtered_commands[i] = alpha * new_target[i] + (1 - alpha) * self.filtered_commands[i]
-
-        return self.filtered_commands.copy()
-
-    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 set_desired_positions(self, target_positions, apply_filter=True):
+    def send_command(self, joint_positions):
         """
-        Set desired joint positions
+        Send joint command to robot (non-blocking)
 
         Args:
-            target_positions: Target joint angles in radians
-            apply_filter: Apply low-pass filter for smooth motion
+            joint_positions: Array of target joint angles
         """
-        target = np.array(target_positions[:self.n_joints])
+        cmd = np.array(joint_positions[:self.n_joints])
 
         # Apply joint limits
         for i in range(self.n_joints):
-            target[i] = np.clip(target[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
+            cmd[i] = max(self.joint_lower_limits[i], min(self.joint_upper_limits[i], cmd[i]))
 
-        # Apply low-pass filter if enabled
-        if apply_filter:
-            target = self._low_pass_filter(target)
+        self.j_cmd = cmd
 
-        self.desired_positions = target
-
-    def compute_torques(self):
-        """Compute torques using PD control with velocity damping"""
-        current_pos = self.get_joint_positions()
-        current_vel = self.get_joint_velocities()
-
-        # Position error
-        pos_error = self.desired_positions - current_pos
-
-        # PD control law (negative feedback)
-        torques = self.kp[:self.n_joints] * pos_error - self.kd[:self.n_joints] * current_vel
-
-        # Apply torque limits (safety)
-        max_torque = 50.0  # Nm limit
-        torques = np.clip(torques, -max_torque, max_torque)
-
-        return torques
-
-    def step(self):
-        """Step the simulation with control applied"""
-        if self.control_mode == 'torque':
-            # Compute torques
-            torques = self.compute_torques()
-
-            # Apply torques directly to joints using qfrc_applied
-            # This bypasses the need for actuators in the URDF
-            self.data.qfrc_applied[:self.n_joints] = torques
-
-        elif self.control_mode == 'position':
-            # Direct position control (kinematic)
-            # Add velocity damping for smoother motion
-            current_pos = self.get_joint_positions()
-            pos_error = self.desired_positions - current_pos
-
-            # Simple proportional velocity control
-            kp_vel = 50.0
-            target_vel = kp_vel * pos_error
-
-            # Limit velocity
-            max_vel = 3.0  # rad/s
-            target_vel = np.clip(target_vel, -max_vel, max_vel)
-
-            # Apply velocity
-            self.data.qvel[:self.n_joints] = target_vel
-
-            # For position mode, we also apply small corrective torques
-            kp_correct = 100.0
-            kd_correct = 10.0
-            correction = kp_correct * pos_error - kd_correct * self.get_joint_velocities()
-            self.data.qfrc_applied[:self.n_joints] = correction
-
-        # Step physics
-        mujoco.mj_step(self.model, self.data)
-
-        # If using direct qpos control (position mode extreme)
-        if self.control_mode == 'position_direct':
-            self.data.qpos[:self.n_joints] = self.desired_positions
-            self.data.qvel[:self.n_joints] = 0
-            mujoco.mj_step(self.model, self.data)
-
-        # Sync viewer if active
-        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, duration=1.0, apply_filter=True):
+    def get_feedback(self):
         """
-        Smoothly move to target position with dynamics
+        Get current robot state
 
-        Args:
-            target_positions: Target joint angles
-            duration: Movement duration (seconds)
-            apply_filter: Apply low-pass filtering
+        Returns:
+            Dictionary with positions, velocities, etc.
         """
-        # Calculate number of steps for smooth interpolation
-        n_steps = int(duration / self.model.opt.timestep)
-        start_positions = self.get_joint_positions()
-        target = np.array(target_positions[:self.n_joints])
+        return self.j_fbk
 
-        print(f"  Moving with dynamics: {n_steps} steps over {duration}s")
 
-        for i in range(n_steps):
-            # Linear interpolation for desired positions
-            alpha = (i + 1) / n_steps
-            desired = start_positions + alpha * (target - start_positions)
+    def _simulation_loop(self):
+        """Main simulation loop (runs in separate thread)"""
+        last_time = time.time()
 
-            # Set desired position (filter will apply smoothing)
-            self.set_desired_positions(desired, apply_filter=apply_filter)
+        while self.running:
+            # Process commands
+
+            # 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()
+
+            # Smooth interpolation
+            alpha = self.smoothness
+            next_positions = current_pos + alpha * (self.j_cmd - current_pos)
+
+            # Calculate velocities for smooth motion
+            dt = self.model.opt.timestep
+            target_velocities = (next_positions - current_pos) / dt
+
+            # Limit velocities
+            max_vel = 3.0
+            target_velocities = np.clip(target_velocities, -max_vel, max_vel)
+
+            # Apply control
+            self.data.qvel[:self.n_joints] = target_velocities
+
+            # Corrective forces
+            pos_error = self.j_cmd - current_pos
+            kp = 30.0
+            self.data.qfrc_applied[:self.n_joints] = kp * pos_error
 
             # Step simulation
-            self.step()
+            mujoco.mj_step(self.model, self.data)
 
-            # Optional: print progress
-            if (i + 1) % 200 == 0:
-                progress = (i + 1) / n_steps * 100
-                print(f"    Progress: {progress:.0f}%")
-
-    def run_trajectory(self, trajectory_points, duration_per_point=1.0, apply_filter=True):
-        """
-        Execute a trajectory with smooth dynamics
-
-        Args:
-            trajectory_points: List of joint position arrays
-            duration_per_point: Time per trajectory point (seconds)
-            apply_filter: Apply low-pass filtering
-        """
-        print(f"\nExecuting 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, duration_per_point, apply_filter)
-
-        print("✓ Trajectory complete")
-
-    def run_interactive(self):
-        """Run simulation with real-time control"""
-        print("\n✓ Simulation running with dynamic control")
-        print(f"  Control mode: {self.control_mode}")
-        print(f"  Low-pass cutoff: {self.lp_cutoff} Hz")
-        print("  Close viewer window to exit.\n")
-
-        try:
-            last_time = time.time()
-            while self.viewer and self.viewer.is_running():
-                self.step()
-
-                # Maintain real-time speed
-                elapsed = time.time() - last_time
-                sleep_time = self.model.opt.timestep - elapsed
-                if sleep_time > 0:
-                    time.sleep(sleep_time)
-                last_time = time.time()
-
-        except KeyboardInterrupt:
-            print("\n✓ Stopped by user")
-        finally:
+            # Sync viewer
             if self.viewer:
-                self.viewer.close()
+                self.viewer.sync()
+
+            # Maintain real-time speed
+            elapsed = time.time() - last_time
+            sleep_time = self.model.opt.timestep - 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 print_state(self):
         """Print current robot state"""
-        positions = self.get_joint_positions()
-        velocities = self.get_joint_velocities()
-        print(f"Positions (rad): {[f'{p:.3f}' for p in positions[:4]]}...")
-        print(f"Velocities (rad/s): {[f'{v:.3f}' for v in velocities[:4]]}...")
+        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]]}...")
 
 
-# Demo with different dynamic behaviors
-if __name__ == "__main__":
-    # Path to your URDF
-    urdf_file = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
+# Example usage for kinematic verification
+def verify_kinematics():
+    """Test sequence to verify kinematic code"""
 
-    if not Path(urdf_file).exists():
-        print(f"Error: URDF file not found at {urdf_file}")
-        exit(1)
+    # Create controller
+    urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
+    robot = MuJoCoRobotController(urdf_path, smoothness=0.08, enable_viewer=True)
 
-    print("=" * 60)
-    print("RM75 Robot with Realistic Dynamics")
-    print("=" * 60)
-
-    # Test 1: Torque control with low-pass filter (most realistic)
-    print("\n>>> Test 1: Torque Control with Low-Pass Filter (5 Hz)")
-    robot1 = RM75Controller(urdf_file, enable_viewer=True,
-                            control_mode='torque',
-                            low_pass_cutoff_hz=5.0)  # Smooth, natural response
-
-    # Wait a moment for viewer to initialize
-    time.sleep(1)
-
-    # Move to a pose with dynamics
-    target_pose = robot1.home_position.copy()
-    target_pose[0] = 0.8  # Joint 1 (base rotation)
-    target_pose[1] = -0.5  # Joint 2 (shoulder)
-    target_pose[2] = 0.4  # Joint 3 (elbow)
-    target_pose[3] = 0.3  # Joint 4 (wrist 1)
-
-    print("\nMoving to target pose with realistic dynamics...")
-    robot1.move_to_position(target_pose, duration=2.0, apply_filter=True)
-
-    print("\nRobot state after movement:")
-    robot1.print_state()
-
-    # Test 2: Return to home with different filter
-    print("\n>>> Returning to home with softer filter (2 Hz)")
-    robot1.lp_cutoff = 2.0  # Change to slower response
-    robot1.move_to_position(robot1.home_position, duration=2.0, apply_filter=True)
-
-    # Test 3: Demonstrate different control modes
-    print("\n>>> Testing position control mode (faster response)")
-    robot2 = RM75Controller(urdf_file, enable_viewer=False,
-                            control_mode='position',
-                            low_pass_cutoff_hz=15.0)
-
-    test_pose = robot2.home_position.copy()
-    test_pose[0] = 0.5
-    test_pose[1] = -0.3
-
-    print("Moving in position control mode...")
-    robot2.move_to_position(test_pose, duration=1.0, apply_filter=True)
-    robot2.print_state()
+    # Start simulation
+    robot.start()
+    time.sleep(1)  # Wait for simulation to initialize
 
     print("\n" + "=" * 60)
-    print("Dynamic Behavior Summary")
+    print("Kinematic Verification Test")
     print("=" * 60)
-    print("✓ Torque Control: Realistic physics with inertia and damping")
-    print("✓ Low-pass filter: Adjustable smoothing (lower = smoother)")
-    print("✓ Joint limits: Automatically enforced from URDF")
-    print("✓ Programmatic actuation: No URDF modification needed")
-    print("\nTips for tuning:")
-    print("  - Lower cutoff (2-5 Hz): Smooth, natural motion")
-    print("  - Higher cutoff (10-20 Hz): More responsive")
-    print("  - Adjust kp/kd gains for stiffness/damping")
 
-    # Run interactive simulation
-    print("\nStarting interactive simulation...")
-    print("Press Ctrl+C in terminal to exit, or close the viewer window.")
-    robot1.run_interactive()
\ No newline at end of file
+    # 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 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}")
+
+    # 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}")
+
+    # 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]),
+    ]
+
+    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✓ Kinematic verification complete!")
+    print("\nInteractive mode - send commands using robot.send_command()")
+
+    # Keep running for interactive control
+    try:
+        while True:
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        print("\nShutting down...")
+        robot.stop()
+
+
+if __name__ == "__main__":
+    verify_kinematics()
\ No newline at end of file