add ik q_solved refinement function, to make it as close as possible to last_q.

update the rm75_kine_rm.py to included a series of arm-angle for inverse kinematics calculation.

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:
+                # robot_mjk.send_command(q)
-                #     time.sleep(1)
+                # 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):

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()
-
+        self.nv = 7
-        q_range = (  self.model.upperPositionLimit[:7] - self.model.lowerPositionLimit[: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()
+        self.ik_P_pattern = sparse.triu( np.ones((self.nv, self.nv))  ).tocsc()
-
-        P_sparse = sparse.csc_matrix(self.P_pattern)
-
-        A_sparse = sparse.eye(7, format='csc')
-
         self.osqp_solver = osqp.OSQP()
 
         self.osqp_solver.setup(
-            P=P_sparse,
+            P=self.ik_P_pattern,
-            q=np.zeros(7),
+            q=np.zeros(self.nv),
-            A=A_sparse,
+            A=sparse.eye(self.nv, format='csc'),
-            l=-np.ones(7),
+            l=-np.ones(self.nv),
-            u=np.ones(7),
+            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,
+    def get_subchain_jacobian(self,  joint_angles,  frame_names  ):
-                              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()

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)
+        ret, phi0 = 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)
-        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)