ZhongyuWang-cart/IK_qp
1
0
Fork 0
forked from ZhengLiu-cart/IK_qp
Code Pull Requests Activity

update the mjc function names

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-05 10:23:02 +01:00
parent ddbbb1746e
commit aefc7bacd5
2 changed files with 31 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
kine_ctrl/main.py
View File

@ -109,7 +109,7 @@ def demo_position_control():
solve_sum = 0 solve_sum = 0
for i in range(2000): for i in range(10):
print(f'\n-------------- in i = {i} ----------------') print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(ub/180*pi, lb/180*pi) joint_rand = np.random.uniform(ub/180*pi, lb/180*pi)
print(f'the predefined joints are {joint_rand}') print(f'the predefined joints are {joint_rand}')
@ -146,6 +146,10 @@ def demo_position_control():
print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}') print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
if d_p_ik < 0.01: if d_p_ik < 0.01:
result[0][1] += 1 result[0][1] += 1
robot_mjk.send_command(joint_solution)
robot_mjk.wait_until_reached()
robot_mjk.print_state()
else: else:
fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name) fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)
fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0) fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0)

52
kine_ctrl/rm75_mjc.py
View File

@ -48,8 +48,8 @@ class MuJoCoPositionController:
print( print(
f" {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]") f" {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]")
# Target positions (in radians) # Target joint angles (in radians)
self.target_positions = self.data.qpos[:self.n_joints].copy() self.target_joints = self.data.qpos[:self.n_joints].copy()
# Smoothing factor (0-1, lower = smoother) # Smoothing factor (0-1, lower = smoother)
self.smoothness = smoothness self.smoothness = smoothness
@ -57,9 +57,9 @@ class MuJoCoPositionController:
# Thread safety # Thread safety
self.command_lock = threading.Lock() self.command_lock = threading.Lock()
self.feedback_lock = threading.Lock() self.feedback_lock = threading.Lock()
self.current_feedback = self.data.qpos[:self.n_joints].copy() self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
self.max_pos_inc = 0.02 self.max_ang_inc = 0.02
# Control flags # Control flags
self.running = False self.running = False
@ -109,17 +109,17 @@ class MuJoCoPositionController:
cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i]) cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
with self.command_lock: with self.command_lock:
self.target_positions = cmd self.target_joints = cmd
def get_feedback(self): def get_feedback(self):
"""Get current joint positions""" """Get current joint positions"""
with self.feedback_lock: with self.feedback_lock:
return self.current_feedback.copy() return self.current_feedback_joint.copy()
def get_target(self): def get_target(self):
"""Get current target positions""" """Get current target positions"""
with self.command_lock: with self.command_lock:
return self.target_positions.copy() return self.target_joints.copy()
def _simulation_loop(self): def _simulation_loop(self):
""" """
@ -129,24 +129,24 @@ class MuJoCoPositionController:
last_time = time.time() last_time = time.time()
# For smooth interpolation # For smooth interpolation
current_positions = self.data.qpos[:self.n_joints].copy() current_joints = self.data.qpos[:self.n_joints].copy()
while self.running: while self.running:
# Get target command # Get target command
with self.command_lock: with self.command_lock:
target = self.target_positions.copy() target = self.target_joints.copy()
# Get current positions # Get current positions
current_positions = self.data.qpos[:self.n_joints].copy() current_joints = self.data.qpos[:self.n_joints].copy()
# Smooth interpolation toward target # Smooth interpolation toward target
# This creates natural motion without velocity commands # This creates natural motion without velocity commands
alpha = self.smoothness alpha = self.smoothness
next_positions = current_positions + np.clip(alpha * (target - current_positions) , -self.max_pos_inc, self.max_pos_inc) next_joints = current_joints + np.clip(alpha * (target - current_joints) , -self.max_ang_inc, self.max_ang_inc)
# DIRECT POSITION CONTROL - Set joint positions # DIRECT POSITION CONTROL - Set joint positions
self.data.qpos[:self.n_joints] = next_positions self.data.qpos[:self.n_joints] = next_joints
# IMPORTANT: Set velocities to zero to prevent physics from moving joints # IMPORTANT: Set velocities to zero to prevent physics from moving joints
# This ensures pure kinematic control # This ensures pure kinematic control
@ -157,12 +157,12 @@ class MuJoCoPositionController:
# After step, ensure our joint positions are maintained # After step, ensure our joint positions are maintained
# (Physics might have altered them slightly) # (Physics might have altered them slightly)
self.data.qpos[:self.n_joints] = next_positions self.data.qpos[:self.n_joints] = next_joints
self.data.qvel[:self.n_joints] = 0 self.data.qvel[:self.n_joints] = 0
# Update feedback # Update feedback
with self.feedback_lock: with self.feedback_lock:
self.current_feedback = self.data.qpos[:self.n_joints].copy() self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
# Sync viewer # Sync viewer
if self.viewer: if self.viewer:
@ -175,20 +175,20 @@ class MuJoCoPositionController:
time.sleep(sleep_time) time.sleep(sleep_time)
last_time = time.time() last_time = time.time()
def move_to_position(self, target, duration=1.0): def move_to_joints(self, target, duration=1.0):
""" """
Move to target position over specified duration Move to target joints over specified duration
Args: Args:
target: Target joint positions target: Target joint joints
duration: Time to complete movement (seconds) duration: Time to complete movement (seconds)
""" """
start_pos = self.get_feedback() start_js = self.get_feedback()
end_pos = np.array(target[:self.n_joints]) end_js = np.array(target[:self.n_joints])
# Apply limits # Apply limits
for i in range(self.n_joints): 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]) 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) n_steps = int(duration / self.time_interval)
@ -198,15 +198,15 @@ class MuJoCoPositionController:
alpha = (step + 1) / n_steps alpha = (step + 1) / n_steps
# Use easing for smoother motion # Use easing for smoother motion
ease_alpha = 1 - (1 - alpha) ** 2 # Quadratic ease-out ease_alpha = 1 - (1 - alpha) ** 2 # Quadratic ease-out
current_target = start_pos + ease_alpha * (end_pos - start_pos) current_target = start_js + ease_alpha * (end_js - start_js)
self.send_command(current_target) self.send_command(current_target)
time.sleep(self.time_interval) time.sleep(self.time_interval)
# Ensure exact target # Ensure exact target
self.send_command(end_pos) self.send_command(end_js)
time.sleep(0.1) time.sleep(0.1)
def wait_until_reached(self, tolerance=0.01, timeout=5.0): def wait_until_reached(self, tolerance=0.01, timeout=10.0):
""" """
Wait until robot reaches target position Wait until robot reaches target position
@ -230,10 +230,10 @@ class MuJoCoPositionController:
def print_state(self): def print_state(self):
"""Print current robot state""" """Print current robot state"""
positions = self.get_feedback() joints = self.get_feedback()
target = self.get_target() target = self.get_target()
print("Current positions:", [f"{p:.3f}" for p in positions[:4]], "...") print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...")
print("Target positions: ", [f"{t:.3f}" for t in target[:4]], "...") print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...")
# Demo # Demo