forked from ZhengLiu-cart/IK_qp
try mujoco3
This commit is contained in:
LiuzhengSJ
@ -1,7 +1,6 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""
|
"""
|
||||||
Thread-based MuJoCo controller for kinematic verification
|
Simple Position Control with Velocity and Acceleration Limits - WITH IMPROVED CONVERGENCE
|
||||||
No ROS dependency - pure Python threading
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import mujoco
|
import mujoco
|
||||||
@ -12,38 +11,55 @@ import time
|
|||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
||||||
class MuJoCoRobotController:
|
class SimplePositionController:
|
||||||
"""
|
"""
|
||||||
Thread-based robot controller for kinematic verification
|
Simple position control with velocity and acceleration limits
|
||||||
Command and feedback via thread-safe queues
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
def __init__(self, urdf_path, smoothness=0.1, enable_viewer=True):
|
def __init__(self, urdf_path,
|
||||||
"""
|
max_velocity=2.0, # rad/s
|
||||||
Args:
|
max_acceleration=5.0, # rad/s²
|
||||||
urdf_path: Path to URDF file
|
control_dt=0.01, # seconds
|
||||||
smoothness: Motion smoothness (0.05-0.2)
|
enable_viewer=True):
|
||||||
enable_viewer: Show MuJoCo viewer
|
|
||||||
"""
|
|
||||||
# Load model
|
# Load model
|
||||||
self.model = mujoco.MjModel.from_xml_path(urdf_path)
|
self.model = mujoco.MjModel.from_xml_path(urdf_path)
|
||||||
self.data = mujoco.MjData(self.model)
|
self.data = mujoco.MjData(self.model)
|
||||||
|
|
||||||
# Robot info
|
# Robot info
|
||||||
self.n_joints = self.model.njnt
|
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
|
# Get joint limits
|
||||||
self.joint_lower_limits = [self.model.jnt_range[i, 0] for i in range(self.n_joints)]
|
self.joint_lower_limits = []
|
||||||
self.joint_upper_limits = [self.model.jnt_range[i, 1] for i in range(self.n_joints)]
|
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"Loaded robot: {self.n_joints} joints")
|
||||||
|
print(f"Velocity limit: {max_velocity} rad/s")
|
||||||
|
print(f"Acceleration limit: {max_acceleration} rad/s²")
|
||||||
|
|
||||||
# Control parameters
|
# Control limits
|
||||||
self.smoothness = smoothness
|
self.max_vel = max_velocity
|
||||||
|
self.max_acc = max_acceleration
|
||||||
|
|
||||||
self.j_cmd = self.data.qpos[:self.n_joints].copy()
|
# Target and current positions
|
||||||
self.j_fbk = self.data.qvel[:self.n_joints].copy()
|
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
|
# Control flags
|
||||||
self.running = False
|
self.running = False
|
||||||
@ -54,23 +70,23 @@ class MuJoCoRobotController:
|
|||||||
if enable_viewer:
|
if enable_viewer:
|
||||||
try:
|
try:
|
||||||
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
|
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
|
||||||
|
print("Viewer launched")
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(f"Viewer warning: {e}")
|
print(f"Viewer warning: {e}")
|
||||||
|
|
||||||
print("Robot controller ready")
|
print("Controller ready\n")
|
||||||
|
|
||||||
def start(self):
|
def start(self):
|
||||||
"""Start the simulation thread"""
|
"""Start simulation thread"""
|
||||||
if self.running:
|
if self.running:
|
||||||
return
|
return
|
||||||
|
|
||||||
self.running = True
|
self.running = True
|
||||||
self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True)
|
self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True)
|
||||||
self.simulation_thread.start()
|
self.simulation_thread.start()
|
||||||
print("Simulation thread started")
|
print("Simulation started")
|
||||||
|
|
||||||
def stop(self):
|
def stop(self):
|
||||||
"""Stop the simulation thread"""
|
"""Stop simulation"""
|
||||||
self.running = False
|
self.running = False
|
||||||
if self.simulation_thread:
|
if self.simulation_thread:
|
||||||
self.simulation_thread.join(timeout=2.0)
|
self.simulation_thread.join(timeout=2.0)
|
||||||
@ -79,150 +95,224 @@ class MuJoCoRobotController:
|
|||||||
print("Simulation stopped")
|
print("Simulation stopped")
|
||||||
|
|
||||||
def send_command(self, joint_positions):
|
def send_command(self, joint_positions):
|
||||||
"""
|
"""Send target joint positions"""
|
||||||
Send joint command to robot (non-blocking)
|
cmd = np.array(joint_positions[:self.n_joints], dtype=np.float64)
|
||||||
|
|
||||||
Args:
|
|
||||||
joint_positions: Array of target joint angles
|
|
||||||
"""
|
|
||||||
cmd = np.array(joint_positions[:self.n_joints])
|
|
||||||
|
|
||||||
# Apply joint limits
|
# Apply joint limits
|
||||||
for i in range(self.n_joints):
|
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):
|
def get_feedback(self):
|
||||||
"""
|
"""Get current joint positions"""
|
||||||
Get current robot state
|
with self.feedback_lock:
|
||||||
|
return self.current_positions.copy()
|
||||||
Returns:
|
|
||||||
Dictionary with positions, velocities, etc.
|
|
||||||
"""
|
|
||||||
return self.j_fbk
|
|
||||||
|
|
||||||
|
|
||||||
def _simulation_loop(self):
|
def _simulation_loop(self):
|
||||||
"""Main simulation loop (runs in separate thread)"""
|
"""Main simulation loop"""
|
||||||
last_time = time.time()
|
last_time = time.time()
|
||||||
|
|
||||||
while self.running:
|
while self.running:
|
||||||
# Process commands
|
# Get target
|
||||||
|
with self.command_lock:
|
||||||
|
target = self.target_positions.copy()
|
||||||
|
|
||||||
# Get current state
|
# Calculate distance to target
|
||||||
current_pos = self.data.qpos[:self.n_joints].copy()
|
distance_to_target = target - self.current_command
|
||||||
self.j_fbk = self.data.qpos[:self.n_joints].copy()
|
|
||||||
current_vel = self.data.qvel[:self.n_joints].copy()
|
|
||||||
|
|
||||||
# Smooth interpolation
|
# Adaptive velocity based on distance (slow down when close)
|
||||||
alpha = self.smoothness
|
for i in range(self.n_joints):
|
||||||
next_positions = current_pos + alpha * (self.j_cmd - current_pos)
|
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
|
# Limit the step size
|
||||||
dt = self.model.opt.timestep
|
max_step = max_vel_for_joint * self.control_dt
|
||||||
target_velocities = (next_positions - current_pos) / dt
|
if abs(distance_to_target[i]) > max_step:
|
||||||
|
distance_to_target[i] = np.sign(distance_to_target[i]) * max_step
|
||||||
|
|
||||||
# Limit velocities
|
# Apply acceleration limit
|
||||||
max_vel = 3.0
|
delta = distance_to_target
|
||||||
target_velocities = np.clip(target_velocities, -max_vel, max_vel)
|
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
|
vel_diff = desired_vel - current_vel
|
||||||
self.data.qvel[:self.n_joints] = target_velocities
|
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
|
# Update command
|
||||||
pos_error = self.j_cmd - current_pos
|
next_command = self.current_command + delta
|
||||||
kp = 30.0
|
|
||||||
self.data.qfrc_applied[:self.n_joints] = kp * pos_error
|
|
||||||
|
|
||||||
# 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)
|
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
|
# Sync viewer
|
||||||
if self.viewer:
|
if self.viewer:
|
||||||
self.viewer.sync()
|
self.viewer.sync()
|
||||||
|
|
||||||
# Maintain real-time speed
|
# Maintain control rate
|
||||||
elapsed = time.time() - last_time
|
elapsed = time.time() - last_time
|
||||||
sleep_time = self.model.opt.timestep - elapsed
|
sleep_time = self.control_dt - elapsed
|
||||||
if sleep_time > 0:
|
if sleep_time > 0:
|
||||||
time.sleep(sleep_time)
|
time.sleep(sleep_time)
|
||||||
last_time = time.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):
|
while time.time() - start_time < timeout:
|
||||||
"""Print current robot state"""
|
current = self.get_feedback()
|
||||||
feedback = self.get_feedback()
|
with self.command_lock:
|
||||||
if feedback:
|
target = self.target_positions
|
||||||
print(f"Positions: {[f'{p:.3f}' for p in feedback['positions'][:4]]}...")
|
|
||||||
print(f"Velocities: {[f'{v:.3f}' for v in feedback['velocities'][:4]]}...")
|
error = np.max(np.abs(target - current))
|
||||||
|
|
||||||
|
if error < tolerance:
|
||||||
|
return True
|
||||||
|
|
||||||
|
time.sleep(0.01)
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
# Example usage for kinematic verification
|
def simple_test():
|
||||||
def verify_kinematics():
|
"""Test with improved convergence"""
|
||||||
"""Test sequence to verify kinematic code"""
|
|
||||||
|
|
||||||
# Create controller
|
|
||||||
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
|
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()
|
robot.start()
|
||||||
time.sleep(1) # Wait for simulation to initialize
|
time.sleep(1)
|
||||||
|
|
||||||
print("\n" + "=" * 60)
|
print("\n" + "=" * 60)
|
||||||
print("Kinematic Verification Test")
|
print("Testing movement with smooth convergence")
|
||||||
print("=" * 60)
|
print("=" * 60)
|
||||||
|
|
||||||
# Test 1: Single joint movement
|
# Test 1: Move to 0.8 rad and back
|
||||||
print("\n[Test 1] Moving joint 1 to 45 degrees...")
|
print("\n[Test 1] Moving to +0.8 rad")
|
||||||
cmd = np.zeros(7)
|
robot.send_command([0.8, 0, 0, 0, 0, 0, 0])
|
||||||
cmd[0] = 0.785 # 45 degrees
|
robot.wait_for_convergence(tolerance=0.01)
|
||||||
robot.send_command(cmd)
|
pos = robot.get_feedback()
|
||||||
time.sleep(1)
|
print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0] - 0.8):.4f})")
|
||||||
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 -0.8 rad")
|
||||||
print("\n[Test 2] Moving to complex pose...")
|
robot.send_command([-0.8, 0, 0, 0, 0, 0, 0])
|
||||||
cmd = np.array([0.5, -0.4, 0.3, 0.2, 0, 0, 0])
|
robot.wait_for_convergence(tolerance=0.01)
|
||||||
robot.send_command(cmd)
|
pos = robot.get_feedback()
|
||||||
feedback = robot.get_feedback()
|
print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0] + 0.8):.4f})")
|
||||||
print(f" Result: {feedback}")
|
|
||||||
|
|
||||||
# Test 3: Return to home
|
print("\n[Test 3] Moving to home (0 rad)")
|
||||||
print("\n[Test 3] Returning to home...")
|
robot.send_command([0, 0, 0, 0, 0, 0, 0])
|
||||||
robot.send_command(np.zeros(7))
|
robot.wait_for_convergence(tolerance=0.01)
|
||||||
feedback = robot.get_feedback()
|
pos = robot.get_feedback()
|
||||||
print(f" Result: home = {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] Multi-joint movement")
|
||||||
print("\n[Test 4] Testing trajectory following...")
|
robot.send_command([0.5, -0.3, 0.2, 0.1, 0, 0, 0])
|
||||||
trajectory = [
|
robot.wait_for_convergence(tolerance=0.01)
|
||||||
np.array([0.3, 0, 0, 0, 0, 0, 0]),
|
pos = robot.get_feedback()
|
||||||
np.array([0.6, 0, 0, 0, 0, 0, 0]),
|
print(f" Final positions: {[f'{p:.3f}' for p in pos[:4]]}...")
|
||||||
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("\n[Test 5] Return home")
|
||||||
print(f" Step {i + 1}: sending {cmd[0]:.3f}")
|
robot.send_command([0, 0, 0, 0, 0, 0, 0])
|
||||||
robot.send_command(cmd)
|
robot.wait_for_convergence(tolerance=0.01)
|
||||||
feedback = robot.get_feedback()
|
pos = robot.get_feedback()
|
||||||
print(f" Reached: {feedback}")
|
print(f" Home positions: {[f'{p:.3f}' for p in pos[:4]]}...")
|
||||||
|
|
||||||
print("\n✓ Kinematic verification complete!")
|
print("\n✓ All tests passed! Robot converges quickly to target.")
|
||||||
print("\nInteractive mode - send commands using robot.send_command()")
|
print("\nInteractive mode - close viewer to exit")
|
||||||
|
|
||||||
# Keep running for interactive control
|
|
||||||
try:
|
try:
|
||||||
while True:
|
while robot.viewer and robot.viewer.is_running():
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
print("\nShutting down...")
|
pass
|
||||||
robot.stop()
|
|
||||||
|
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__":
|
if __name__ == "__main__":
|
||||||
verify_kinematics()
|
# Run the back and forth test with smooth convergence
|
||||||
|
back_and_forth_test()
|
||||||
Reference in New Issue
Block a user