forked from ZhengLiu-cart/IK_qp
try mujoco2
This commit is contained in:
LiuzhengSJ
@ -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()
|
||||
# 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()
|
||||
Reference in New Issue
Block a user