diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index 372615f..6057566 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -109,7 +109,7 @@ def demo_position_control(): solve_sum = 0 - for i in range(2000): + for i in range(10): print(f'\n-------------- in i = {i} ----------------') joint_rand = np.random.uniform(ub/180*pi, lb/180*pi) 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}') if d_p_ik < 0.01: result[0][1] += 1 + + robot_mjk.send_command(joint_solution) + robot_mjk.wait_until_reached() + robot_mjk.print_state() else: 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) diff --git a/kine_ctrl/rm75_mjc.py b/kine_ctrl/rm75_mjc.py index 0cc3c7d..77598e8 100644 --- a/kine_ctrl/rm75_mjc.py +++ b/kine_ctrl/rm75_mjc.py @@ -48,8 +48,8 @@ class MuJoCoPositionController: print( f" {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]") - # Target positions (in radians) - self.target_positions = self.data.qpos[:self.n_joints].copy() + # Target joint angles (in radians) + self.target_joints = self.data.qpos[:self.n_joints].copy() # Smoothing factor (0-1, lower = smoother) self.smoothness = smoothness @@ -57,9 +57,9 @@ class MuJoCoPositionController: # Thread safety self.command_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 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]) with self.command_lock: - self.target_positions = cmd + self.target_joints = cmd def get_feedback(self): """Get current joint positions""" with self.feedback_lock: - return self.current_feedback.copy() + return self.current_feedback_joint.copy() def get_target(self): """Get current target positions""" with self.command_lock: - return self.target_positions.copy() + return self.target_joints.copy() def _simulation_loop(self): """ @@ -129,24 +129,24 @@ class MuJoCoPositionController: last_time = time.time() # For smooth interpolation - current_positions = self.data.qpos[:self.n_joints].copy() + current_joints = self.data.qpos[:self.n_joints].copy() while self.running: # Get target command with self.command_lock: - target = self.target_positions.copy() + target = self.target_joints.copy() # 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 # This creates natural motion without velocity commands 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 - 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 # This ensures pure kinematic control @@ -157,12 +157,12 @@ class MuJoCoPositionController: # After step, ensure our joint positions are maintained # (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 # Update feedback 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 if self.viewer: @@ -175,20 +175,20 @@ class MuJoCoPositionController: time.sleep(sleep_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: - target: Target joint positions + target: Target joint joints duration: Time to complete movement (seconds) """ - start_pos = self.get_feedback() - end_pos = np.array(target[:self.n_joints]) + start_js = self.get_feedback() + end_js = 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]) + 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) @@ -198,15 +198,15 @@ class MuJoCoPositionController: 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) + 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_pos) + self.send_command(end_js) 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 @@ -230,10 +230,10 @@ class MuJoCoPositionController: def print_state(self): """Print current robot state""" - positions = self.get_feedback() + joints = 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]], "...") + print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...") + print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...") # Demo