Implement dual-arm teleoperation with RM75 QP controller and MuJoCo backend
This commit is contained in:
@ -1,297 +1,81 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Pure Position Control for MuJoCo - No velocity commands, no forces
|
||||
Direct joint position control with smoothing
|
||||
"""
|
||||
"""Compatibility adapter for the original MuJoCo controller import path."""
|
||||
|
||||
from time import sleep
|
||||
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
from rm75_ik.mujoco_backend import DualArmMuJoCo
|
||||
|
||||
|
||||
class MuJoCoPositionController:
|
||||
"""
|
||||
Pure position control - directly sets joint positions
|
||||
No velocity commands, no forces - completely stable
|
||||
"""
|
||||
"""Legacy facade backed by the threadless normalized dual-arm scene."""
|
||||
|
||||
def __init__(self, urdf_path="./urdf_rm75/RM75-B.urdf", 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)
|
||||
|
||||
self.time_interval = 0.02
|
||||
|
||||
print(f'time interval: {self.model.opt.timestep}')
|
||||
|
||||
# Robot info
|
||||
self.n_joints = self.model.njnt
|
||||
|
||||
# Get joint limits
|
||||
self.joint_lower_limits = []
|
||||
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")
|
||||
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}]")
|
||||
|
||||
# Target joint angles (in radians)
|
||||
self.target_joints = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Smoothing factor (0-1, lower = smoother)
|
||||
self.smoothness = smoothness
|
||||
|
||||
# Thread safety
|
||||
self.command_lock = threading.Lock()
|
||||
self.feedback_lock = threading.Lock()
|
||||
self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
self.max_ang_inc = 0.02
|
||||
|
||||
# Control flags
|
||||
self.running = False
|
||||
self.simulation_thread = None
|
||||
|
||||
# Viewer
|
||||
self.viewer = None
|
||||
if enable_viewer:
|
||||
try:
|
||||
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
|
||||
print("Viewer launched")
|
||||
except Exception as e:
|
||||
print(f"Viewer warning: {e}")
|
||||
self.start()
|
||||
def __init__(
|
||||
self,
|
||||
urdf_path=None,
|
||||
smoothness=0.05,
|
||||
enable_viewer=True,
|
||||
controlled_arm="left",
|
||||
):
|
||||
del urdf_path, smoothness
|
||||
self.backend = DualArmMuJoCo(controlled_arm=controlled_arm)
|
||||
self.controlled_arm = controlled_arm
|
||||
self.viewer = (
|
||||
mujoco.viewer.launch_passive(self.backend.model, self.backend.data)
|
||||
if enable_viewer
|
||||
else None
|
||||
)
|
||||
|
||||
def start(self):
|
||||
"""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 thread started")
|
||||
return None
|
||||
|
||||
def stop(self):
|
||||
"""Stop the simulation thread"""
|
||||
self.running = False
|
||||
if self.simulation_thread:
|
||||
self.simulation_thread.join(timeout=2.0)
|
||||
if self.viewer:
|
||||
if self.viewer is not None:
|
||||
self.viewer.close()
|
||||
print("Simulation stopped")
|
||||
sleep(0.2)
|
||||
self.viewer = None
|
||||
|
||||
def send_command(self, 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
|
||||
for i in range(self.n_joints):
|
||||
cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
|
||||
|
||||
with self.command_lock:
|
||||
self.target_joints = cmd
|
||||
self.backend.set_arm_configuration(
|
||||
self.controlled_arm, np.asarray(joint_positions, dtype=float)
|
||||
)
|
||||
if self.viewer is not None:
|
||||
self.viewer.sync()
|
||||
|
||||
def get_feedback(self):
|
||||
"""Get current joint positions"""
|
||||
with self.feedback_lock:
|
||||
return self.current_feedback_joint.copy()
|
||||
return self.backend.get_arm_configuration(self.controlled_arm)
|
||||
|
||||
def get_target(self):
|
||||
"""Get current target positions"""
|
||||
with self.command_lock:
|
||||
return self.target_joints.copy()
|
||||
|
||||
def _simulation_loop(self):
|
||||
"""
|
||||
Main simulation loop - PURE POSITION CONTROL
|
||||
No velocity commands, no forces - just direct position setting
|
||||
"""
|
||||
last_time = time.time()
|
||||
|
||||
# For smooth interpolation
|
||||
current_joints = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
while self.running:
|
||||
# Get target command
|
||||
with self.command_lock:
|
||||
target = self.target_joints.copy()
|
||||
|
||||
# Get current positions
|
||||
current_joints = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Smooth interpolation toward target
|
||||
# This creates natural motion without velocity commands
|
||||
alpha = self.smoothness
|
||||
|
||||
next_joints = current_joints + np.clip(alpha * (target - current_joints) , -self.max_ang_inc, self.max_ang_inc)
|
||||
|
||||
# DIRECT POSITION CONTROL - Set joint positions
|
||||
self.data.qpos[:self.n_joints] = next_joints
|
||||
|
||||
# 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 (this will apply gravity, collisions, etc. to other bodies)
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
|
||||
# After step, ensure our joint positions are maintained
|
||||
# (Physics might have altered them slightly)
|
||||
self.data.qpos[:self.n_joints] = next_joints
|
||||
self.data.qvel[:self.n_joints] = 0
|
||||
|
||||
# Update feedback
|
||||
with self.feedback_lock:
|
||||
self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Sync viewer
|
||||
if self.viewer:
|
||||
self.viewer.sync()
|
||||
|
||||
# Maintain real-time speed
|
||||
elapsed = time.time() - last_time
|
||||
sleep_time = self.time_interval - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
last_time = time.time()
|
||||
return self.get_feedback()
|
||||
|
||||
def move_to_joints(self, target, duration=1.0):
|
||||
"""
|
||||
Move to target joints over specified duration
|
||||
|
||||
Args:
|
||||
target: Target joint joints
|
||||
duration: Time to complete movement (seconds)
|
||||
"""
|
||||
start_js = self.get_feedback()
|
||||
end_js = np.array(target[:self.n_joints])
|
||||
|
||||
# Apply limits
|
||||
for i in range(self.n_joints):
|
||||
end_js[i] = np.clip(end_js[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
|
||||
|
||||
n_steps = int(duration / self.time_interval)
|
||||
|
||||
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_js + ease_alpha * (end_js - start_js)
|
||||
self.send_command(current_target)
|
||||
time.sleep(self.time_interval)
|
||||
|
||||
# Ensure exact target
|
||||
self.send_command(end_js)
|
||||
time.sleep(0.1)
|
||||
start = self.get_feedback()
|
||||
target_q = np.asarray(target, dtype=float)
|
||||
points = max(2, int(round(duration * 90.0)))
|
||||
blend = 0.5 - 0.5 * np.cos(np.linspace(0.0, np.pi, points))
|
||||
trajectory = start[None, :] + blend[:, None] * (target_q - start)[None, :]
|
||||
self.backend.play_trajectory(
|
||||
trajectory,
|
||||
dt=duration / points,
|
||||
realtime=True,
|
||||
viewer=self.viewer,
|
||||
)
|
||||
|
||||
def wait_until_reached(self, tolerance=0.01, timeout=10.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()
|
||||
target = self.get_target()
|
||||
error = np.max(np.abs(target - current))
|
||||
|
||||
if error < tolerance:
|
||||
return True
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
return False
|
||||
del tolerance, timeout
|
||||
return True
|
||||
|
||||
def print_state(self):
|
||||
"""Print current robot state"""
|
||||
joints = self.get_feedback()
|
||||
target = self.get_target()
|
||||
print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...")
|
||||
print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...")
|
||||
print("Current joints (rad):", self.get_feedback().tolist())
|
||||
|
||||
|
||||
# Demo
|
||||
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()
|
||||
|
||||
from rm75_ik.stage2_demo import main
|
||||
|
||||
return main([])
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
demo_position_control()
|
||||
raise SystemExit(demo_position_control())
|
||||
|
||||
Reference in New Issue
Block a user