diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index 2702c1c..697088d 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -66,8 +66,6 @@ def main(): print(f'pose = {pose}') print('-'*100) - time.sleep(5) - @@ -110,12 +108,9 @@ def main(): if d_p_ik < 0.01: result[0][1] += 1 - # while 1: - # time.sleep(1) - - robot_mjk.send_command(q) - robot_mjk.wait_until_reached() - robot_mjk.print_state() + # robot_mjk.send_command(q) + # robot_mjk.wait_until_reached() + # robot_mjk.print_state() else: fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name) d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2) @@ -136,6 +131,7 @@ def main(): print(f'results with qp and rm for ik are {result}') print(f'solve_sum is {solve_sum}') + robot_mjk.stop() def cal_pose_deviation(pose1, pose2): diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index 03c11c8..b771d6f 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/kine_ctrl/rm75_kine_qp.py @@ -26,48 +26,116 @@ class KinematicsSolver(): self.geom_model.addAllCollisionPairs() self.remove_adjacent_collision_pairs(verbose=True) self.geom_data = pin.GeometryData(self.geom_model) + self.data = self.model.createData() self.cfg_j_limit() - - q_range = ( self.model.upperPositionLimit[:7] - self.model.lowerPositionLimit[:7] ) + self.nv = 7 + q_range = ( self.model.upperPositionLimit[:self.nv] - self.model.lowerPositionLimit[:self.nv] ) self.w_q_limit = np.diag(1.0 / (q_range ** 2)) - self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7]) + self.q_mid = 0.5 * (self.model.lowerPositionLimit[:self.nv] + self.model.upperPositionLimit[:self.nv]) - # ---------- for reused qp_solver ------------------ - self.nv = 7 + # --------------------------------------------------------- + # Primary IK solver + # + # Optimization variable: + # dq in R^7 + # + # Constraints: + # lb <= dq <= ub + # + # Therefore: + # A = I, shape = 7 x 7 # Full dense symmetric matrix structure # P_template = np.triu(np.ones((7, 7))) - self.P_pattern = sparse.triu(np.ones((7,7))).tocsc() - - P_sparse = sparse.csc_matrix(self.P_pattern) - - A_sparse = sparse.eye(7, format='csc') - + self.ik_P_pattern = sparse.triu( np.ones((self.nv, self.nv)) ).tocsc() self.osqp_solver = osqp.OSQP() self.osqp_solver.setup( - P=P_sparse, - q=np.zeros(7), - A=A_sparse, - l=-np.ones(7), - u=np.ones(7), + P=self.ik_P_pattern, + q=np.zeros(self.nv), + A=sparse.eye(self.nv, format='csc'), + l=-np.ones(self.nv), + u=np.ones(self.nv), verbose=False, warm_start=True, polish=False ) + # End-effector task weight: self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4]) + # Smaller value => joint moves more actively - # Larger value => joint moves less / more lazy + # Larger value => joint moves more lazy self.joint_motion_weight = np.diag([ 1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2 ]) + # --------------------------------------------------------- + # Refinement solver + # + # Constraints: + # + # J_eff * dq = error_vec 6 constraints + # dq_lower <= dq <= dq_upper 7 constraints + # + # A_refine = [J_eff] + # [ I ] + # + # Shape: + # 13 x 7 + # + # The upper 6 x 7 block must have a dense sparsity + # pattern because every Jacobian entry can change. + # --------------------------------------------------------- + + # The refinement Hessian used below is diagonal: + # + # H = w_last * W_last + # + w_mid * W_mid + # + damping * I + # + # Therefore a diagonal P pattern is sufficient. + self.refine_P_pattern = sparse.eye( + self.nv, + format="csc", + ) + + # Dense structural pattern for the 6x7 Jacobian block. + refine_J_pattern = sparse.csc_matrix( + np.ones((6, self.nv)) + ) + + # Identity pattern for joint-step bounds. + refine_I_pattern = sparse.eye( + self.nv, + format="csc", + ) + + self.refine_A_pattern = sparse.vstack( + [ + refine_J_pattern, + refine_I_pattern, + ], + format="csc", + ) + + self.refine_osqp_solver = osqp.OSQP() + + self.refine_osqp_solver.setup( + P=self.refine_P_pattern, + q=np.zeros(self.nv), + A=self.refine_A_pattern, + l=-np.ones(6 + self.nv), + u=np.ones(6 + self.nv), + verbose=False, + warm_start=True, + polish=False, + ) if tcps is None: pass @@ -174,7 +242,7 @@ class KinematicsSolver(): debug: Print debug information tool: the frame name ('scissor', 'camera', 'ee') Returns: - tuple: (joint_angles, success, error) + sts, q_solved """ # Build target SE3 placement if target_quat is not None: @@ -241,9 +309,6 @@ class KinematicsSolver(): error_SE3 = current_placement.actInv(target_placement) error_vec = pin.log(error_SE3).vector - # print("\n initial error =", np.linalg.norm(error_vec)) - # print(error_vec) - while iter_count < max_iter: # Compute forward kinematics @@ -357,6 +422,360 @@ class KinematicsSolver(): # return q[:7].copy(), False, error_norm, iter_count return -1, q[:7].copy().tolist() + def refine_ik_solution( + self, + q_valid, + q_last, + tool="ee", + max_iter=30, + pose_tolerance=5e-4, + joint_tolerance=1e-5, + step_limit=None, + w_last=1.0, + w_mid=0.0001, + damping=1e-5, + debug=False, + ): + """ + Refine an already-valid IK solution. + + The initial configuration q_valid already reaches the desired + end-effector pose. The function searches for another configuration + that: + + 1. maintains the end-effector pose generated by q_valid; + 2. is closer to q_last; + 3. optionally remains away from joint limits. + + Optimization at each iteration: + + minimize over dq: + + 0.5 * w_last * + ||q + dq - q_last||^2_{W_last} + + + 0.5 * w_mid * + ||q + dq - q_mid||^2_{W_mid} + + + 0.5 * damping * ||dq||^2 + + subject to: + + J_eff(q) dq = error_vec(q) + + dq_lower <= dq <= dq_upper + + The pose error is defined relative to the pose generated by q_valid. + + Returns: + status, q_refined + + Status: + 0: refinement completed successfully + -1: invalid input or tool + -2: refinement QP failed + -3: final pose error exceeds tolerance + -4: refined configuration is in collision + """ + + # --------------------------------------------------------- + # Validate input + # --------------------------------------------------------- + q = np.asarray(q_valid, dtype=np.float64).reshape(-1) + q_last = np.asarray(q_last, dtype=np.float64).reshape(-1) + + if q.size != self.nv: + if debug: + print(f"q_valid must contain {self.nv} values, " f"got {q.size}" ) + return -1, q.tolist() + + if q_last.size != self.nv: + if debug: + print( f"q_last must contain {self.nv} values, " f"got {q_last.size}" ) + return -1, q.tolist() + + q_lower = self.model.lowerPositionLimit[:self.nv] + q_upper = self.model.upperPositionLimit[:self.nv] + + q = np.clip(q, q_lower, q_upper) + q_last = np.clip(q_last, q_lower, q_upper) + + # --------------------------------------------------------- + # Resolve end-effector frame + # --------------------------------------------------------- + if tool in self.tcps: + tcp_index = self.tcps.index(tool) + ee_frame_id = self.tcp_ids[tcp_index] + else: + try: + ee_frame_id = self.tool_frames[tool] + except: + print(f'{tool} definition not found') + + + # --------------------------------------------------------- + # Set per-joint refinement step limits + # --------------------------------------------------------- + if step_limit is None: + dq_limit = np.array([ 0.02, 0.02, 0.02, 0.02, 0.03, 0.03, 0.04, ]) + else: + dq_limit = np.asarray( step_limit, dtype=np.float64, ) + + if dq_limit.ndim == 0: + dq_limit = np.full( self.nv, float(dq_limit),) + else: + dq_limit = dq_limit.reshape(-1) + + if dq_limit.size != self.nv: + if debug: + print( f"step_limit must be scalar or contain " f"{self.nv} values") + return -1, q.tolist() + + if np.any(dq_limit <= 0.0): + if debug: + print("All step limits must be positive") + return -1, q.tolist() + + # --------------------------------------------------------- + # Save the end-effector pose produced by q_valid + # --------------------------------------------------------- + pin.forwardKinematics( self.model, self.data, q, ) + pin.updateFramePlacements( self.model, self.data, ) + + target_placement = self.data.oMf[ee_frame_id].copy() + + # --------------------------------------------------------- + # Objective weight matrices + # --------------------------------------------------------- + + # Temporal continuity: + # Larger weight for a joint means that joint is more strongly + # encouraged to remain close to q_last. + W_last = np.eye(self.nv) + + # Joint-limit-centering weight. + W_mid = self.w_q_limit + + # Since all objective matrices are diagonal, H is diagonal. + H_diag = ( w_last * np.diag(W_last) + w_mid * np.diag(W_mid) + damping * np.ones(self.nv) ) + + # OSQP uses: + # 0.5 dq.T P dq + g.T dq + # P values correspond to the diagonal pattern created in __init__. + Px = H_diag.copy() + + # Reset the previous refinement warm start. + self.refine_osqp_solver.warm_start( x=np.zeros(self.nv) ) + + previous_distance = np.linalg.norm(q - q_last) + + # --------------------------------------------------------- + # Sequential refinement loop + # --------------------------------------------------------- + for iteration in range(max_iter): + + # Update kinematics and frame Jacobians. + pin.computeJointJacobians( self.model, self.data, q, ) + + pin.framesForwardKinematics( self.model, self.data, q, ) + + current_placement = self.data.oMf[ee_frame_id] + + # Pose error relative to the pose saved from q_valid. + error_SE3 = current_placement.actInv( target_placement ) + + error_vec = pin.log(error_SE3).vector + pose_error_norm = np.linalg.norm(error_vec) + + J = pin.getFrameJacobian( + self.model, + self.data, + ee_frame_id, + pin.ReferenceFrame.LOCAL, + ) + + J_eff = pin.Jlog6(error_SE3) @ J + + # ----------------------------------------------------- + # Linear objective vector + # ----------------------------------------------------- + # 0.5*w_last*||q+dq-q_last||^2_Wlast + # the linear term is: + # w_last*W_last*(q-q_last) + # The same expansion applies to q_mid. + # ----------------------------------------------------- + g = (w_last * W_last @ (q - q_last) + w_mid * W_mid @ (q - self.q_mid) ) + + # ----------------------------------------------------- + # Pose equality constraint + # ----------------------------------------------------- + # e(q + dq) approximately equals: + # e(q) - J_eff dq + # Requiring the next error to be zero gives: + # J_eff dq = e(q) + # ----------------------------------------------------- + pose_lower = error_vec.copy() + pose_upper = error_vec.copy() + + # ----------------------------------------------------- + # Joint increment and position constraints + # ----------------------------------------------------- + joint_lower = np.maximum( + -dq_limit, + q_lower - q, + ) + + joint_upper = np.minimum( + dq_limit, + q_upper - q, + ) + + lower = np.concatenate([ + pose_lower, + joint_lower, + ]) + + upper = np.concatenate([ + pose_upper, + joint_upper, + ]) + + # ----------------------------------------------------- + # Update A numerical values + # ----------------------------------------------------- + # refine_A_pattern is CSC. For every joint/column j, + # its stored entries are: + # J_eff[0, j] + # J_eff[1, j] + # ... + # J_eff[5, j] + # I[j, j] = 1 + # This produces 7 values per column and 49 total. + # ----------------------------------------------------- + Ax = np.concatenate([ + np.concatenate([ + J_eff[:, joint_index], + np.array([1.0]), + ]) + for joint_index in range(self.nv) + ]) + + self.refine_osqp_solver.update( + Px=Px, + q=g, + Ax=Ax, + l=lower, + u=upper, + ) + + result = self.refine_osqp_solver.solve() + + if result.info.status not in ( + "solved", + "solved inaccurate", + ): + if debug: + print( + "Refinement QP failed at iteration " + f"{iteration}: {result.info.status}" + ) + return -2, q.tolist() + + dq = result.x + + if dq is None or not np.all(np.isfinite(dq)): + if debug: + print( + f"Invalid refinement result at iteration " + f"{iteration}" + ) + return -2, q.tolist() + + dq_norm = np.linalg.norm(dq) + + if debug: + distance_to_last = np.linalg.norm(q - q_last) + + print( + f"refine iteration={iteration:02d}, " + f"pose_error={pose_error_norm:.8f}, " + f"distance_to_last={distance_to_last:.6f}, " + f"dq_norm={dq_norm:.8f}" + ) + + # No useful redundant motion remains. + if dq_norm < joint_tolerance and pose_error_norm < pose_tolerance: + break + + q_candidate = pin.integrate( self.model, q, dq, ) + + q_candidate = np.clip( q_candidate, q_lower, q_upper, ) + + candidate_distance = np.linalg.norm( q_candidate - q_last ) + + # The pose-correction component can occasionally make the + # distance increase slightly. Permit a tiny numerical margin. + if candidate_distance> previous_distance + 1e-8 and pose_error_norm < pose_tolerance: + if debug: + print( "Refinement stopped because the candidate " "does not improve temporal continuity" ) + break + + q = q_candidate + previous_distance = candidate_distance + + # --------------------------------------------------------- + # Final pose verification + # --------------------------------------------------------- + pin.forwardKinematics( self.model, self.data, q, ) + + pin.updateFramePlacements( self.model, self.data, ) + + final_placement = self.data.oMf[ee_frame_id] + + final_error_SE3 = final_placement.actInv( target_placement ) + + final_error_vec = pin.log( final_error_SE3 ).vector + + final_pose_error = np.linalg.norm( final_error_vec ) + + if debug: + print( + f"Final refinement pose error: " + f"{final_pose_error:.8f}" + ) + print( + f"Original distance to last: " + f"{np.linalg.norm(np.asarray(q_valid) - q_last):.6f}" + ) + print( + f"Refined distance to last: " + f"{np.linalg.norm(q - q_last):.6f}" + ) + + if final_pose_error > pose_tolerance: + if debug: + print( + "Refined solution rejected because its " + "pose error is too large" + ) + return -3, np.asarray(q_valid).tolist() + + # --------------------------------------------------------- + # Final collision verification + # --------------------------------------------------------- + collision = self.collision_detect( q=q, stop_at_first_collision=True, ) + + if collision: + if debug: + print( + "Refined solution rejected because it is " + "in collision" + ) + return -4, np.asarray(q_valid).tolist() + + return 0, q.tolist() + + def collision_detect(self, q ,stop_at_first_collision=True, verbose=False ): q = np.asarray(q, dtype=np.float64).reshape(-1) @@ -510,10 +929,7 @@ class KinematicsSolver(): return J - def get_subchain_jacobian(self, - joint_angles, - frame_names - ): + def get_subchain_jacobian(self, joint_angles, frame_names ): q = pin.neutral(self.model) @@ -560,7 +976,6 @@ class KinematicsSolver(): active_joint_ids = [] - # Traverse upward to root while joint_id > 0: # Pinocchio joint indexing: @@ -577,201 +992,3 @@ class KinematicsSolver(): all_active_joint_ids.append(active_joint_ids) return all_active_joint_ids - - def plan_cartesian_trajectory(self, start_pos, end_pos, - start_rpy=None, end_rpy=None, - num_steps=20, tool='ee'): - """ - Plan a Cartesian trajectory with IK for each waypoint. - """ - # Get current end-effector pose if start_rpy not provided - if start_rpy is None: - # Try to find a valid starting configuration - test_angles = [0.1] * 7 - fk_test = self.forward_kinematics(test_angles,tool=tool) - start_rpy = fk_test['rpy'] - - if end_rpy is None: - end_rpy = start_rpy - - # First, check if target is reachable - print(f"\nChecking if target is reachable...") - target_pos = end_pos - target_rpy = end_rpy - - test_solution, success, error = self.inverse_kinematics( - target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool - ) - - if not success: - print(f"Warning: Target may be unreachable or difficult to reach") - print(f"Trying with relaxed tolerance...") - - # Initial guess for IK (start with zero configuration) - current_angles = [0.1] * 7 - trajectory = [] - - print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})") - print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})") - print("-" * 60) - - for i in range(num_steps + 1): - t = i / num_steps - - # Interpolate position - pos = [ - start_pos[0] + t * (end_pos[0] - start_pos[0]), - start_pos[1] + t * (end_pos[1] - start_pos[1]), - start_pos[2] + t * (end_pos[2] - start_pos[2]) - ] - - # Interpolate orientation - rpy = [ - start_rpy[0] + t * (end_rpy[0] - start_rpy[0]), - start_rpy[1] + t * (end_rpy[1] - start_rpy[1]), - start_rpy[2] + t * (end_rpy[2] - start_rpy[2]) - ] - - # Compute IK - joint_angles, success, error = self.inverse_kinematics( - pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool - ) - - if not success: - print(f" Waypoint {i}: IK failed!") - break - - # Verify - fk_verify = self.forward_kinematics(joint_angles, tool=tool) - - trajectory.append({ - 'step': i, - 't': t, - 'position': pos, - 'rpy': rpy, - 'joint_angles': joint_angles, - 'actual_position': fk_verify['position'], - 'error': error - }) - - # Update current angles for next iteration - current_angles = joint_angles - - if i % 5 == 0 or i == num_steps: - print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), " - f"error={error:.6f}m") - - return trajectory - - - -def main(): - """Main test function""" - - - rm75 = KinematicsSolver() - - # Test 1: Forward Kinematics - print("\n1. Forward Kinematics Test") - print("-" * 40) - - tool_name = "scissor" - joint_angles_zero = [0.1] * 7 - fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name) - - print(f"Init configuration:") - print(f" Position: ({fk_result['position'][0]:.3f}, " - f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m") - - # Test 2: Inverse Kinematics with more reachable target - print("\n2. Inverse Kinematics Test") - print("-" * 40) - - # Try a simpler target first - target_pos = [0.3, 0.2, 0.4] # More reachable position - target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation - - print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m") - - import time - init_joints = [0.2] * 7 - time0 = time.time() - for ii in range(100): - joint_solution, success, error = rm75.inverse_kinematics( - target_pos, target_rpy=target_rpy, initial_guess=init_joints, - max_iter=500, debug=False, tool=tool_name - ) - time1 = time.time() - print(f"Time: {time1 - time0}") - - if success: - print(f"✓ Solution found! Error: {error:.6f} m") - for i, angle in enumerate(joint_solution): - print(f" Joint {i + 1}: {degrees(angle):7.2f}°") - - # Verify - fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name) - print( - f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m") - else: - print("✗ IK failed to find a solution!") - - # Test 3: Jacobian - print("\n3. Jacobian Matrix") - print("-" * 40) - - J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name) - print(f"Jacobian shape: {J.shape}") - for i in range(min(3, J.shape[0])): - row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)]) - print(f" Row {i + 1}: {row_str}") - - # Test 4: Trajectory Planning with reachable positions - print("\n4. Cartesian Trajectory Planning") - print("-" * 40) - - start_pos = [0.3, 0.0, 0.4] # Start position - end_pos = [0.3, 0.0, 0.55] # End position (smaller movement) - - fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name) - - trajectory = rm75.plan_cartesian_trajectory( - start_pos, - end_pos, - start_rpy=fk0['rpy'], - end_rpy=[ - fk0['rpy'][0] + radians(10), - fk0['rpy'][1], - fk0['rpy'][2] - ], - num_steps=10, - tool=tool_name - ) - - if trajectory: - print(f"\n✓ Generated {len(trajectory)} waypoints") - - if success: - print("✓ Inverse kinematics working (with simplified target)") - else: - print("⚠ Inverse kinematics may need tuning - try different targets") - - - print("\n" + "=" * 60) - print(f'test subchain Jacobian, for future obstacle avoidance') - frame_names = [ - "link_2", - "link_4", - "link_7" - ] - Js_sub = rm75.get_subchain_jacobian( - joint_angles=joint_angles_zero, - frame_names=frame_names - ) - print(f'Js_sub: {Js_sub}') - - return rm75, trajectory - - -if __name__ == "__main__": - rm75, trajectory = main() diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py index eb175ee..c12af8f 100644 --- a/kine_ctrl/rm75_kine_rm.py +++ b/kine_ctrl/rm75_kine_rm.py @@ -102,7 +102,7 @@ class rm75_kine_api(): return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[q_s*180/math.pi for q_s in joint_angles] , flag=flag) - def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work"): + def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work", step_arm_angle = 15.0): ''' :param target_position: list of position values, m :param target_rpy: list of rpy values, rad @@ -126,10 +126,34 @@ class rm75_kine_api(): q_ref = [ 180/math.pi * ig for ig in initial_guess ] else: q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] - ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) - # print(f'the arm angle is ret = {ret}, and phi = {phi}') - params = rm_inverse_kinematics_params_t(q_ref, - target, 1) + ret, phi0 = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) + params = rm_inverse_kinematics_params_t(q_ref, target, 1) + + step_arm_angle = step_arm_angle + offsets = [0.0] + arm_angle = step_arm_angle + while arm_angle <= 180.0: + offsets += [arm_angle, -arm_angle] + arm_angle += step_arm_angle + + best_ret, best_q_out, best_dis = -1, None, None + for offset in offsets: + phi = ((phi0 + offset + 180.0) % 360.0) - 180.0 + ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) + if int(ret) != 0: + if best_q_out is None: + best_ret, best_q_out = ret, q_out + continue + + p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1) + pose_dis = cal_pose_deviation(p_fk, target) + if pose_dis < 0.01: + # success in ik calculation + return ret, [q / 180 * math.pi for q in q_out] + + if best_dis is None or pose_dis < best_dis: + best_ret, best_q_out, best_dis = -10, q_out, pose_dis + ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1) pose_dis = cal_pose_deviation(pose_fk, target)