forked from ZhengLiu-cart/IK_qp
try mujoco
This commit is contained in:
LiuzhengSJ
@ -1,6 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Simple Position Control with Velocity and Acceleration Limits - WITH IMPROVED CONVERGENCE
|
||||
Pure Position Control for MuJoCo - No velocity commands, no forces
|
||||
Direct joint position control with smoothing
|
||||
"""
|
||||
|
||||
import mujoco
|
||||
@ -11,24 +12,25 @@ import time
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
class SimplePositionController:
|
||||
class MuJoCoPositionController:
|
||||
"""
|
||||
Simple position control with velocity and acceleration limits
|
||||
Pure position control - directly sets joint positions
|
||||
No velocity commands, no forces - completely stable
|
||||
"""
|
||||
|
||||
def __init__(self, urdf_path,
|
||||
max_velocity=2.0, # rad/s
|
||||
max_acceleration=5.0, # rad/s²
|
||||
control_dt=0.01, # seconds
|
||||
enable_viewer=True):
|
||||
|
||||
def __init__(self, urdf_path, 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)
|
||||
|
||||
# Robot info
|
||||
self.n_joints = self.model.njnt
|
||||
self.control_dt = control_dt
|
||||
|
||||
# Get joint limits
|
||||
self.joint_lower_limits = []
|
||||
@ -38,28 +40,20 @@ class SimplePositionController:
|
||||
self.joint_upper_limits.append(self.model.jnt_range[i, 1])
|
||||
|
||||
print(f"Loaded robot: {self.n_joints} joints")
|
||||
print(f"Velocity limit: {max_velocity} rad/s")
|
||||
print(f"Acceleration limit: {max_acceleration} rad/s²")
|
||||
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}]")
|
||||
|
||||
# Control limits
|
||||
self.max_vel = max_velocity
|
||||
self.max_acc = max_acceleration
|
||||
|
||||
# Target and current positions
|
||||
# Target positions (in radians)
|
||||
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
|
||||
# Smoothing factor (0-1, lower = smoother)
|
||||
self.smoothness = smoothness
|
||||
|
||||
# Thread safety
|
||||
self.command_lock = threading.Lock()
|
||||
self.feedback_lock = threading.Lock()
|
||||
self.current_feedback = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Control flags
|
||||
self.running = False
|
||||
@ -74,19 +68,20 @@ class SimplePositionController:
|
||||
except Exception as e:
|
||||
print(f"Viewer warning: {e}")
|
||||
|
||||
print("Controller ready\n")
|
||||
print("Robot controller ready - Pure Position Mode")
|
||||
|
||||
def start(self):
|
||||
"""Start simulation thread"""
|
||||
"""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 started")
|
||||
print("Simulation thread started")
|
||||
|
||||
def stop(self):
|
||||
"""Stop simulation"""
|
||||
"""Stop the simulation thread"""
|
||||
self.running = False
|
||||
if self.simulation_thread:
|
||||
self.simulation_thread.join(timeout=2.0)
|
||||
@ -95,7 +90,12 @@ class SimplePositionController:
|
||||
print("Simulation stopped")
|
||||
|
||||
def send_command(self, joint_positions):
|
||||
"""Send target 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
|
||||
@ -108,87 +108,112 @@ class SimplePositionController:
|
||||
def get_feedback(self):
|
||||
"""Get current joint positions"""
|
||||
with self.feedback_lock:
|
||||
return self.current_positions.copy()
|
||||
return self.current_feedback.copy()
|
||||
|
||||
def get_target(self):
|
||||
"""Get current target positions"""
|
||||
with self.command_lock:
|
||||
return self.target_positions.copy()
|
||||
|
||||
def _simulation_loop(self):
|
||||
"""Main simulation loop"""
|
||||
"""
|
||||
Main simulation loop - PURE POSITION CONTROL
|
||||
No velocity commands, no forces - just direct position setting
|
||||
"""
|
||||
last_time = time.time()
|
||||
|
||||
# For smooth interpolation
|
||||
current_positions = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
while self.running:
|
||||
# Get target
|
||||
# Get target command
|
||||
with self.command_lock:
|
||||
target = self.target_positions.copy()
|
||||
|
||||
# Calculate distance to target
|
||||
distance_to_target = target - self.current_command
|
||||
# Get current positions
|
||||
current_positions = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Adaptive velocity based on distance (slow down when close)
|
||||
for i in range(self.n_joints):
|
||||
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
|
||||
# Smooth interpolation toward target
|
||||
# This creates natural motion without velocity commands
|
||||
alpha = self.smoothness
|
||||
|
||||
# Limit the step size
|
||||
max_step = max_vel_for_joint * self.control_dt
|
||||
if abs(distance_to_target[i]) > max_step:
|
||||
distance_to_target[i] = np.sign(distance_to_target[i]) * max_step
|
||||
delt_pos = np.clip( (target - current_positions), -0.02, 0.02)
|
||||
next_positions = current_positions + alpha * delt_pos
|
||||
|
||||
# Apply acceleration limit
|
||||
delta = distance_to_target
|
||||
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
|
||||
# DIRECT POSITION CONTROL - Set joint positions
|
||||
self.data.qpos[:self.n_joints] = next_positions
|
||||
|
||||
vel_diff = desired_vel - current_vel
|
||||
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
|
||||
|
||||
# Update command
|
||||
next_command = self.current_command + delta
|
||||
|
||||
# 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
|
||||
# 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
|
||||
# Step physics (this will apply gravity, collisions, etc. to other bodies)
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
|
||||
# Maintain position (physics might change it)
|
||||
self.data.qpos[:self.n_joints] = next_command
|
||||
# After step, ensure our joint positions are maintained
|
||||
# (Physics might have altered them slightly)
|
||||
self.data.qpos[:self.n_joints] = next_positions
|
||||
self.data.qvel[:self.n_joints] = 0
|
||||
|
||||
# Update feedback
|
||||
with self.feedback_lock:
|
||||
self.current_positions = self.data.qpos[:self.n_joints].copy()
|
||||
self.current_feedback = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Sync viewer
|
||||
if self.viewer:
|
||||
self.viewer.sync()
|
||||
|
||||
# Maintain control rate
|
||||
# Maintain real-time speed
|
||||
elapsed = time.time() - last_time
|
||||
sleep_time = self.control_dt - elapsed
|
||||
sleep_time = self.model.opt.timestep - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
last_time = time.time()
|
||||
|
||||
def wait_for_convergence(self, tolerance=0.01, timeout=2.0):
|
||||
"""Wait for robot to converge to target"""
|
||||
def move_to_position(self, target, duration=1.0):
|
||||
"""
|
||||
Move to target position over specified duration
|
||||
|
||||
Args:
|
||||
target: Target joint positions
|
||||
duration: Time to complete movement (seconds)
|
||||
"""
|
||||
start_pos = self.get_feedback()
|
||||
end_pos = np.array(target[:self.n_joints])
|
||||
|
||||
# Apply limits
|
||||
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)
|
||||
|
||||
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_pos + ease_alpha * (end_pos - start_pos)
|
||||
self.send_command(current_target)
|
||||
time.sleep(self.model.opt.timestep)
|
||||
|
||||
# Ensure exact target
|
||||
self.send_command(end_pos)
|
||||
time.sleep(0.1)
|
||||
|
||||
def wait_until_reached(self, tolerance=0.01, timeout=5.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()
|
||||
with self.command_lock:
|
||||
target = self.target_positions
|
||||
|
||||
target = self.get_target()
|
||||
error = np.max(np.abs(target - current))
|
||||
|
||||
if error < tolerance:
|
||||
@ -198,57 +223,69 @@ class SimplePositionController:
|
||||
|
||||
return False
|
||||
|
||||
def print_state(self):
|
||||
"""Print current robot state"""
|
||||
positions = self.get_feedback()
|
||||
target = self.get_target()
|
||||
print("Current positions:", [f"{p:.3f}" for p in positions[:4]], "...")
|
||||
print("Target positions: ", [f"{t:.3f}" for t in target[:4]], "...")
|
||||
|
||||
|
||||
# Demo
|
||||
def demo_position_control():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
def simple_test():
|
||||
"""Test with improved convergence"""
|
||||
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
|
||||
|
||||
robot = SimplePositionController(
|
||||
urdf_path,
|
||||
max_velocity=2.0, # 2 rad/s
|
||||
max_acceleration=5.0,
|
||||
control_dt=0.01,
|
||||
enable_viewer=True
|
||||
)
|
||||
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[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("Testing movement with smooth convergence")
|
||||
print("✓ All tests passed! Robot is stable and controllable.")
|
||||
print("=" * 60)
|
||||
|
||||
# Test 1: Move to 0.8 rad and back
|
||||
print("\n[Test 1] Moving to +0.8 rad")
|
||||
robot.send_command([0.8, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_for_convergence(tolerance=0.01)
|
||||
pos = robot.get_feedback()
|
||||
print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0] - 0.8):.4f})")
|
||||
|
||||
print("\n[Test 2] Moving to -0.8 rad")
|
||||
robot.send_command([-0.8, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_for_convergence(tolerance=0.01)
|
||||
pos = robot.get_feedback()
|
||||
print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0] + 0.8):.4f})")
|
||||
|
||||
print("\n[Test 3] Moving to home (0 rad)")
|
||||
robot.send_command([0, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_for_convergence(tolerance=0.01)
|
||||
pos = robot.get_feedback()
|
||||
print(f" Converged to: {pos[0]:.4f} rad (error: {abs(pos[0]):.4f})")
|
||||
|
||||
print("\n[Test 4] Multi-joint movement")
|
||||
robot.send_command([0.5, -0.3, 0.2, 0.1, 0, 0, 0])
|
||||
robot.wait_for_convergence(tolerance=0.01)
|
||||
pos = robot.get_feedback()
|
||||
print(f" Final positions: {[f'{p:.3f}' for p in pos[:4]]}...")
|
||||
|
||||
print("\n[Test 5] Return home")
|
||||
robot.send_command([0, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_for_convergence(tolerance=0.01)
|
||||
pos = robot.get_feedback()
|
||||
print(f" Home positions: {[f'{p:.3f}' for p in pos[:4]]}...")
|
||||
|
||||
print("\n✓ All tests passed! Robot converges quickly to target.")
|
||||
print("\nInteractive mode - close viewer to exit")
|
||||
|
||||
try:
|
||||
@ -260,50 +297,43 @@ def simple_test():
|
||||
robot.stop()
|
||||
|
||||
|
||||
def back_and_forth_test():
|
||||
"""Back and forth test showing smooth convergence"""
|
||||
# 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 = SimplePositionController(
|
||||
urdf_path,
|
||||
max_velocity=2.0,
|
||||
max_acceleration=5.0,
|
||||
control_dt=0.01,
|
||||
enable_viewer=True
|
||||
)
|
||||
robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
|
||||
robot.start()
|
||||
time.sleep(1)
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print("Back and forth movement with smooth convergence")
|
||||
print("=" * 60)
|
||||
# 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])
|
||||
|
||||
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]
|
||||
]
|
||||
# Example usage
|
||||
target_pose = "some pose" # Your pose input
|
||||
|
||||
for target in targets:
|
||||
print(f"\nMoving to {target} rad...")
|
||||
start_time = time.time()
|
||||
# Compute joint targets using your kinematics
|
||||
joint_targets = your_kinematic_solver(target_pose)
|
||||
|
||||
robot.send_command(target)
|
||||
# Send to simulation
|
||||
robot.send_command(joint_targets)
|
||||
|
||||
# Wait for convergence
|
||||
robot.wait_for_convergence(tolerance=0.01, timeout=2.0)
|
||||
# Wait for robot to reach target
|
||||
robot.wait_until_reached()
|
||||
|
||||
elapsed = time.time() - start_time
|
||||
pos = robot.get_feedback()
|
||||
error = sum( abs( np.array(pos) - np.array(target) ) )
|
||||
# Read actual positions
|
||||
actual_positions = robot.get_feedback()
|
||||
|
||||
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")
|
||||
# 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)
|
||||
@ -314,5 +344,6 @@ def back_and_forth_test():
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# Run the back and forth test with smooth convergence
|
||||
back_and_forth_test()
|
||||
demo_position_control()
|
||||
# Uncomment to test with your kinematic code:
|
||||
# example_for_kinematic_code()
|
||||
Reference in New Issue
Block a user