diff --git a/README.md b/README.md
index 2e1145d..a1dcdc8 100644
--- a/README.md
+++ b/README.md
@@ -1,48 +1,42 @@
### This repo is for inverse kinematics and verification
-In this branch, the qp-based inverse kinematics method is modified as a python class. The user can call it as in `main.py`
+In this branch, the **integrated** inverse kinematics method is packed as a python class.
-Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
+The user can call it as in `test1.py`, `test2.py`.
-Verification is done with Mujoco simulation.
+How to use
-Key specifications:
-1. Time consumption.
-2. Success rate
-3. Minial joint variation.
-
-Next:\
-Comparison with Realman official IK method.
-Embedded with current demo.
-
-
-### Comparison (05June2026):
-
-- With current dual arm joint limit,
+```aiignore
+from rm75_kinematics import rm75_kinematics
+robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',
+ mesh_dir='./urdf_rm75',
+ tcps=["scissor_tcp", "camera_tcp"],
+ tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
```
-ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
-lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
+
+Parameter definition:
+
+- urdf_path: the robot description file, ending with `.urdf`.
+- mesh_dir: the robot parts, ending with `.stl`.
+- tcps: the tool central points defined in urdf file, if no, ignore it.
+- tools_in_ee: the installation of different tools attached to the end-effector of the arm (jont7+link7).
+
+
+### Current functions ###
+
+1. `get_ik_result`
+```aiignore
+ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ],
+ target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
+ initial_guess=[0.1] * 7, tool=tool_name)
```
-the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\
-At least one solver works out the ik, rate = **74%**.
-- With realman-75 physical joint limit,
+2. `get_fk_result`
+```aiignore
+p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
```
-ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
-lb = -ub
-```
-the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\
-At least one solver works out the ik, rate = **84%**.
-### update(1st July 2026)
-
-In each iteration, update optimization formula:
-
-- new cost item for distance from middle of the joint range.
-- set up different weight for different joints motion.
-
-
-
-
-
-
\ No newline at end of file
+3. `get_self_collision`
+```aiignore
+self_collision_sts = robot_kine.get_self_collision(q)
+```
\ No newline at end of file
diff --git a/kine_ctrl/rm75_kine/__init__.py b/kine_ctrl/rm75_kine/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/kine_ctrl/rm75_kine/rm75_kine_qp.py b/kine_ctrl/rm75_kine/rm75_kine_qp.py
deleted file mode 100644
index 43681ea..0000000
--- a/kine_ctrl/rm75_kine/rm75_kine_qp.py
+++ /dev/null
@@ -1,994 +0,0 @@
-#!/usr/bin/env python3
-import sys
-import os
-
-import pinocchio as pin
-import numpy as np
-import osqp
-from scipy import sparse
-from math import radians, degrees, pi, cos, sin
-import time
-import threading
-
-
-
-class KinematicsSolver():
- def __init__(self, urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75", tcps=None):
- """
- for realman 75b
- Initialize robotic arm kinematics using Pinocchio (ROS2 version).
- unit: m, rad
- """
- print(f' ------------ the qp based kinematic initialising -----------')
- self.model = pin.buildModelFromUrdf(urdf_path)
-
- self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir)
- 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[: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[:self.nv] + self.model.upperPositionLimit[:self.nv])
-
- # ---------------------------------------------------------
- # 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.ik_P_pattern = sparse.triu( np.ones((self.nv, self.nv)) ).tocsc()
- self.osqp_solver = osqp.OSQP()
-
- self.osqp_solver.setup(
- 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 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:
- self.tcps = []
- else:
- self.tcps = tcps
- self.tcp_ids = []
- try:
- for tcp in self.tcps:
- tcp_id = self.model.getFrameId(tcp)
- self.tcp_ids.append(tcp_id)
- except:
- print(f'tcp_id of {tcp} not found')
-
- def add_frame(self,frame_name, position, rotationXYZ):
- '''
- :param frame_name: str
- :param position: [x, y, z] target position (meters)
- :param rotationXYZ: [x, y, z] target rotation (rad)
- '''
- camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0], rotationXYZ[1], rotationXYZ[2] )
- camera_offset = pin.SE3(
- camera_rotation,
- np.array(position)
- )
- self.model.addFrame( pin.Frame( frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) )
-
- def add_tool_frames(self,dict_frames):
- self.tool_frames ={}
- for tool_name in dict_frames:
- tool_attr = dict_frames[tool_name]
- position = tool_attr[0][0:3]
- rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
- self.add_frame(tool_name, position, rotationXYZ)
- self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)})
- self.data = self.model.createData()
-
-
- def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
- if min_j is None:
- min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]
- if max_j is None:
- max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
-
- k = 1.0 if rad_flag is True else 1.0 / 180 * pi
-
- for i in range(7):
- self.model.lowerPositionLimit[i] = min_j[i] * k
- self.model.upperPositionLimit[i] = max_j[i] * k
-
-
- def forward_kinematics(self, joint_angles, tool="omnipic"):
- """
- Compute forward kinematics.
- Args:
- joint_angles: List or array of 7 joint angles (radians)
- tool: Name of frame to compute
- """
- if len(joint_angles) != 7:
- raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
-
- # Create configuration vector
- q = pin.neutral(self.model)
- for i, angle in enumerate(joint_angles):
- q[i] = angle
-
- # Compute forward kinematics
- pin.forwardKinematics(self.model, self.data, q)
- pin.updateFramePlacements(self.model, self.data)
-
- # Get frame transform
- if tool in self.tcps:
- frame_id = self.tcp_ids[self.tcps.index(tool)]
- else:
- try:
- frame_id = self.tool_frames[tool]
- except:
- print(f'{tool} definition not found')
-
- frame_transform = self.data.oMf[frame_id]
-
- # Extract results
- position = frame_transform.translation.copy()
- rotation = frame_transform.rotation.copy()
-
- # Compute RPY
- rpy = pin.rpy.matrixToRpy(rotation)
-
- # Compute quaternion
- pose = np.concatenate([position, rpy], axis=0)
- return pose
-
- def inverse_kinematics(self, target_position, target_rpy=None,
- target_quat=None, initial_guess=None,
- max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
- """
- Compute inverse kinematics using differential IK with multiple strategies.
- Args:
- target_position: [x, y, z] target position (meters)
- target_rpy: [roll, pitch, yaw] target orientation (radians)
- target_quat: [x, y, z, w] target orientation as quaternion
- initial_guess: Initial joint angles (radians)
- max_iter: Maximum iterations
- tolerance: Error tolerance
- debug: Print debug information
- tool: the frame name ('scissor', 'camera', 'ee')
- Returns:
- sts, q_solved
- """
- # Build target SE3 placement
- if target_quat is not None:
- quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2])
- target_rotation = quat.matrix()
- elif target_rpy is not None:
- target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
- target_rpy[1],
- target_rpy[2])
- else:
- target_rotation = np.eye(3)
-
- target_placement = pin.SE3(target_rotation, np.array(target_position))
-
- # Try multiple initial guesses
- initial_guesses = []
-
- if initial_guess is not None:
- initial_guesses.append(initial_guess)
- else:
- # Try different initial configurations
- initial_guesses.append([0.1] * 7) # Zero config
-
-
- best_solution = None
- best_error = float('inf')
-
- for guess_idx, guess in enumerate(initial_guesses):
- q = pin.neutral(self.model)
- for i, angle in enumerate(guess):
- if i < len(q):
- q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
- self.model.upperPositionLimit[i])
- q_ref = q.copy()
-
- # Differential IK with adaptive damping
- damping = 0.1
- damping_reduction = 0.95
- iter_count = 0
- prev_error = float('inf')
-
- # ee_frame_id = self.tool_frames[tool]
- if tool in self.tcps:
- ee_frame_id = self.tcp_ids[self.tcps.index(tool)]
- else:
- try:
- ee_frame_id = self.tool_frames[tool]
- except:
- print(f'{tool} definition not found')
-
- J = pin.computeFrameJacobian(
- self.model,
- self.data,
- q,
- ee_frame_id,
- pin.ReferenceFrame.LOCAL
- )
-
- pin.forwardKinematics(self.model, self.data, q)
- pin.updateFramePlacements(self.model, self.data)
-
- current_placement = self.data.oMf[ee_frame_id]
-
- error_SE3 = current_placement.actInv(target_placement)
- error_vec = pin.log(error_SE3).vector
-
- while iter_count < max_iter:
- # Compute forward kinematics
-
- pin.computeJointJacobians(self.model, self.data, q)
- pin.framesForwardKinematics(self.model, self.data, q)
-
- # Get current end-effector placement
- current_placement = self.data.oMf[ee_frame_id]
-
- # Compute error
- error_SE3 = current_placement.actInv(target_placement)
- error_vec = pin.log(error_SE3).vector
- error_norm = np.linalg.norm(error_vec)
-
- if error_norm < tolerance:
- if error_norm < best_error:
- best_error = error_norm
- best_solution = q[:7].copy()
- break
-
- # Check if error is increasing (diverging)
- if error_norm > prev_error * 1.1 and iter_count > 10:
- damping = min(1.0, damping * 1.5)
- else:
- damping = max(0.01, damping * damping_reduction)
-
-
- J = pin.getFrameJacobian(
- self.model,
- self.data,
- ee_frame_id,
- pin.ReferenceFrame.LOCAL
- )
-
- # =========================
- # QP-based IK
- # =========================
- w_ref = 0.0001
- w_limit_mid = 0.00002
-
- J_eff = pin.Jlog6(error_SE3) @ J #J #
-
- H = J_eff.T @ self.W @ J_eff
-
-
- H += damping * damping * self.joint_motion_weight
- H += w_ref * np.eye(7)
- H += w_limit_mid * self.w_q_limit
-
- H_triu = sparse.triu(H).tocsc()
-
- g = -J_eff.T @ self.W @ error_vec
- g += w_ref * (q[:7] - q_ref[:7])
- g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
-
- # -------------------------
- # Joint velocity constraints
- # -------------------------
- dq_limit = np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ]) # rad per iteration
-
- lb = -dq_limit * np.ones(7)
- ub = dq_limit * np.ones(7)
-
- # -------------------------
- # Joint position constraints
- # -------------------------
-
- q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
- q_max_step = self.model.upperPositionLimit[:7] - q[:7]
-
- lb = np.maximum(lb, q_min_step)
- ub = np.minimum(ub, q_max_step)
-
- # -------------------------
- # Solve QP
- # ------------------------
- # Update solver
- self.osqp_solver.update(
- Px= H_triu.data, #H[np.triu_indices(7)], #
- q=g,
- l=lb,
- u=ub
- )
-
- # Solve
- result = self.osqp_solver.solve()
- if result.info.status != 'solved':
- break
-
- dq = result.x
-
- if dq is None:
- break
-
- # Apply joint limits with scaling
- alpha = 1.0
- q = pin.integrate(self.model, q, alpha * dq)
-
- prev_error = error_norm
- iter_count += 1
-
- if best_solution is not None:
- collision = self.collision_detect(q=best_solution, stop_at_first_collision=True)
-
- if collision is False:
- # return best_solution, True, best_error, iter_count
- return 0, best_solution.tolist()
- else:
- return -2, q[:7].copy().tolist()
- else:
- # 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)
-
- if q.shape[0] != self.model.nq:
- raise ValueError(f"q size mismatch: expected {self.model.nq}, got {q.shape[0]}")
-
- # Update robot kinematics
- pin.forwardKinematics(self.model, self.data, q)
- pin.updateGeometryPlacements(
- self.model,
- self.data,
- self.geom_model,
- self.geom_data,
- q
- )
-
- # Now compute collisions on the updated geometry model
- collision = pin.computeCollisions(
- self.geom_model,
- self.geom_data,
- stop_at_first_collision
- )
-
- if verbose:
- print(f"the collision is {collision}\n")
-
- for k, cr in enumerate(self.geom_data.collisionResults):
- if cr.isCollision():
- cp = self.geom_model.collisionPairs[k]
- geom1 = self.geom_model.geometryObjects[cp.first]
- geom2 = self.geom_model.geometryObjects[cp.second]
-
- print(
- f"collision pair {k}: "
- f"{geom1.name} <--> {geom2.name}"
- )
-
- return bool(collision)
-
- def remove_adjacent_collision_pairs(self, verbose=True):
- """
- Remove collision pairs between same/adjacent parent joints.
- This avoids false positives such as:
- base_link_0 <--> link_1_0
- """
- pairs_to_remove = []
-
- for pair_id, pair in enumerate(self.geom_model.collisionPairs):
- geom1 = self.geom_model.geometryObjects[pair.first]
- geom2 = self.geom_model.geometryObjects[pair.second]
-
- j1 = geom1.parentJoint
- j2 = geom2.parentJoint
-
- # Same body or directly connected bodies
- if j1 == j2 or abs(j1 - j2) <= 1:
- pairs_to_remove.append(pair_id)
-
- if verbose:
- print(
- "Removing adjacent pair:",
- pair_id,
- geom1.name,
- "<-->",
- geom2.name,
- "parentJoint:",
- j1,
- j2,
- )
-
- for pair_id in reversed(pairs_to_remove):
- self.geom_model.removeCollisionPair(
- self.geom_model.collisionPairs[pair_id]
- )
-
- # Important: recreate geometry data after modifying pairs
- self.geom_data = pin.GeometryData(self.geom_model)
-
- if verbose:
- print("Remaining collision pairs:", len(self.geom_model.collisionPairs))
-
- def quaternion_to_euler(self, q):
- """
- Convert quaternion to Euler angles (roll, pitch, yaw)
-
- Args:
- qx, qy, qz, qw: quaternion components
-
- Returns:
- tuple: (roll, pitch, yaw) in radians
- """
- # Roll (x-axis rotation)
- sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
- cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
- roll = np.arctan2(sinr_cosp, cosr_cosp)
-
- # Pitch (y-axis rotation)
- sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
- if abs(sinp) >= 1:
- pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
- else:
- pitch = np.arcsin(sinp)
-
- # Yaw (z-axis rotation)
- siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
- cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
- yaw = np.arctan2(siny_cosp, cosy_cosp)
-
- return [roll, pitch, yaw]
-
- # def invese_kinematics_velocity(self, target_position, target_rpy=None,
- # target_quat=None, initial_guess=None, tool="ee"):
- # """
- # Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
- #
- # Args:
- # target_position: [x, y, z] target position (meters)
- # target_rpy: [roll, pitch, yaw] target orientation (radians)
- # target_quat: [x, y, z, w] target orientation as quaternion
- # initial_guess: Initial joint angles (radians)
- # tool: the frame name ('scissor', 'camera', 'ee')
- #
- # Returns:
- # joint_velocity: np.array()
- # """
- # # Build target SE3 placement
- # if target_quat is not None:
- # quat = pin.Quaternion(target_quat[3], target_quat[0],
- # target_quat[1], target_quat[2])
- # target_rotation = quat.matrix()
- # elif target_rpy is not None:
- # target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
- # target_rpy[1],
- # target_rpy[2])
- # else:
- # target_rotation = np.eye(3)
- #
- # target_placement = pin.SE3(target_rotation, np.array(target_position))
- #
-
- def compute_jacobian(self, joint_angles, tool="ee"):
- """Compute geometric Jacobian (6x7)"""
- q = pin.neutral(self.model)
- for i, angle in enumerate(joint_angles):
- q[i] = angle
-
- pin.forwardKinematics(self.model, self.data, q)
- pin.updateFramePlacements(self.model, self.data)
- ee_frame_id = self.tool_frames[tool]
- J = pin.computeFrameJacobian(self.model, self.data, q, ee_frame_id)
-
- return J
-
- def get_subchain_jacobian(self, joint_angles, frame_names ):
-
- q = pin.neutral(self.model)
-
- all_active_joints = self.get_active_joints_from_frame(frame_names)
-
- for i in range(7):
- q[i] = joint_angles[i]
-
- pin.forwardKinematics(self.model, self.data, q)
- pin.updateFramePlacements(self.model, self.data)
- pin.computeJointJacobians(self.model, self.data, q)
-
- Js = []
-
- for frame_name, active_joints in zip(frame_names, all_active_joints):
- frame_id = self.model.getFrameId(frame_name)
-
- J = pin.getFrameJacobian(
- self.model,
- self.data,
- frame_id,
- pin.ReferenceFrame.LOCAL
- )
- Js.append(J[:, active_joints])
-
- return Js
-
- def get_active_joints_from_frame(self, frame_names):
- """
- Return active joint indices affecting a frame.
-
- Example:
- frame_name='link_4'
- -> [0,1,2,3]
- """
- all_active_joint_ids = []
- for frame_name in frame_names:
- frame_id = self.model.getFrameId(frame_name)
-
- # Parent joint of this frame
- joint_id = self.model.frames[frame_id].parentJoint
-
- print(f'frame_id = {frame_id}, and joint_id = {joint_id}')
-
- active_joint_ids = []
-
- # Traverse upward to root
- while joint_id > 0:
- # Pinocchio joint indexing:
- # universe joint = 0
- # robot joints start from 1
-
- active_joint_ids.append(joint_id - 1)
-
- # Move to parent joint
- joint_id = self.model.parents[joint_id]
-
- # Reverse so order becomes base -> tip
- active_joint_ids.reverse()
- all_active_joint_ids.append(active_joint_ids)
-
- return all_active_joint_ids
diff --git a/kine_ctrl/rm75_kine/rm75_kine_rm.py b/kine_ctrl/rm75_kine/rm75_kine_rm.py
deleted file mode 100644
index ccd0166..0000000
--- a/kine_ctrl/rm75_kine/rm75_kine_rm.py
+++ /dev/null
@@ -1,178 +0,0 @@
-
-from Robotic_Arm.rm_robot_interface import *
-import numpy as np
-import math
-
-class rm75_kine_api():
- def __init__(self):
- # ---------- rm75 official algorithm -----------
- print(f'------- the realman official kinematic initialising -------')
- arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm
- force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
- # Initialize the robotic arm model and sensor type in the algorithm
- self.robot_kine_rm = Algo(arm_model, force_type)
-
- self.cfg_j_limit()
-
- self.work_frames = {
- 'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
- }
-
- self.tool_name = "no_tool"
- self.work_name = "work"
-
- def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
- if max_j is None:
- max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
- if min_j is None:
- min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
-
- max_j = np.array(max_j)
- min_j = np.array(min_j)
- if rad_flag:
- self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
- self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
- else:
- self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
- self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
-
- def cfg_work_frame(self , frame_name):
- self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
-
- def get_work_frame(self):
- return self.robot_kine_rm.rm_algo_get_curr_workframe()
-
- def cfg_tool_frame(self, frame_name ):
- self.robot_kine_rm.rm_algo_set_toolframe(self.tool_frames[frame_name])
-
- def get_tool_frame(self):
- return self.robot_kine_rm.rm_algo_get_curr_toolframe()
-
- def quaternion_to_euler(self, q):
- """
- Convert quaternion to Euler angles (roll, pitch, yaw)
-
- Args:
- qx, qy, qz, qw: quaternion components
-
- Returns:
- tuple: (roll, pitch, yaw) in radians
- """
- # Roll (x-axis rotation)
- sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
- cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
- roll = np.arctan2(sinr_cosp, cosr_cosp)
-
- # Pitch (y-axis rotation)
- sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
- if abs(sinp) >= 1:
- pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
- else:
- pitch = np.arcsin(sinp)
-
- # Yaw (z-axis rotation)
- siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
- cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
- yaw = np.arctan2(siny_cosp, cosy_cosp)
-
- return [roll, pitch, yaw]
-
- def add_tool_frames(self, dict_frames):
- self.tool_frames = {}
- for tool_name in dict_frames:
- tool_attr = dict_frames[tool_name]
- position = tool_attr[0][0:3]
- rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
- f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
-
- self.tool_frames.update({tool_name:f})
-
- def forward_kinematics(self, joint_angles, flag = 1 , tool="omnipic", work="work"):
- '''
- :param joint_angles: list of joint values, in rad
- :param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
- :param return: [x,y,z,rx,ry,rz], m & rad
- '''
- if tool != self.tool_name:
- self.tool_name = tool
- self.cfg_tool_frame(tool)
- if work != self.work_name:
- self.work_name = work
- self.cfg_work_frame(work)
- return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[float(q_s)*180.0/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", step_arm_angle = 15.0):
- '''
- :param target_position: list of position values, m
- :param target_rpy: list of rpy values, rad
- :param initial_guess: initial guess of angles, rad
- :param tool: tool name, refer to self.tool_frames
- :param work: work name, refer to self.work_frames
-
- return ret: state of ik calculation, 0:success, -2: out of workspace
- [q_]: the ik calculated angles for joints, rad
- '''
- if tool != self.tool_name:
- self.tool_name = tool
- self.cfg_tool_frame(tool)
- if work != self.work_name:
- self.work_name = work
- self.cfg_work_frame(work)
-
- target = list(target_position) + list(target_rpy)
-
- if initial_guess is not None:
- 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, 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)
-
- 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)
-
- # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
- #
- # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
- # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
- if int(ret) < 0:
- return ret, [ q/180*math.pi for q in q_out]
- elif pose_dis < 0.01:
- return ret, [ q/180*math.pi for q in q_out]
- else:
- return -10, [ q/180*math.pi for q in q_out]
-
-def cal_pose_deviation(pose1, pose2):
- d_fk_p1 = np.array(pose1) - np.array(pose2)
- for j in [3, 4, 5]:
- while d_fk_p1[j] > math.pi:
- d_fk_p1[j] -= 2 * math.pi
- while d_fk_p1[j] < -math.pi:
- d_fk_p1[j] += 2 * math.pi
- d_fk = np.linalg.norm(d_fk_p1)
- return d_fk
diff --git a/kine_ctrl/rm75_kinematics.py b/kine_ctrl/rm75_kinematics.py
index 624fcc8..63977bd 100644
--- a/kine_ctrl/rm75_kinematics.py
+++ b/kine_ctrl/rm75_kinematics.py
@@ -5,26 +5,22 @@ Robotic arm kinematics solver for realman-75
Files:
rm75_kinematics.py
- rm75_kine/rm75_kine_qp.py
- rm75_kine/rm75_kine_rm.py
How to use:
- Example in main.py:
- python main.py
+ Example in test1.py:
+ python test1.py
'''
-from kine_ctrl.rm75_kine.rm75_kine_qp import KinematicsSolver as kine_qp
-from kine_ctrl.rm75_kine.rm75_kine_rm import rm75_kine_api as kine_rm
class rm75_kinematics():
def __init__(self,urdf_path="", mesh_dir="", tcps=None,tools_in_ee=None,min_j=0.0, max_j=0.0):
- self.robot_kine_qp = kine_qp(urdf_path=urdf_path, mesh_dir=mesh_dir)
+ self.robot_kine_qp = rm75_kine_qp(urdf_path=urdf_path, mesh_dir=mesh_dir)
self.robot_kine_qp.add_tool_frames(tools_in_ee)
self.robot_kine_qp.cfg_j_limit(min_j=min_j, max_j=max_j, rad_flag=True)
# ---------- rm75 official algorithm -----------
- self.robot_kine_rm = kine_rm()
+ self.robot_kine_rm = rm75_kine_api()
self.robot_kine_rm.add_tool_frames(tools_in_ee)
self.robot_kine_rm.cfg_j_limit(min_j=min_j, max_j=max_j, rad_flag=True)
@@ -69,3 +65,1178 @@ class rm75_kinematics():
+#!/usr/bin/env python3
+import sys
+import os
+
+import pinocchio as pin
+import numpy as np
+import osqp
+from scipy import sparse
+from math import radians, degrees, pi, cos, sin
+import time
+import threading
+
+
+
+class rm75_kine_qp():
+ def __init__(self, urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75", tcps=None):
+ """
+ for realman 75b
+ Initialize robotic arm kinematics using Pinocchio (ROS2 version).
+ unit: m, rad
+ """
+ print(f' ------------ the qp based kinematic initialising -----------')
+ self.model = pin.buildModelFromUrdf(urdf_path)
+
+ self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir)
+ 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[: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[:self.nv] + self.model.upperPositionLimit[:self.nv])
+
+ # ---------------------------------------------------------
+ # 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.ik_P_pattern = sparse.triu( np.ones((self.nv, self.nv)) ).tocsc()
+ self.osqp_solver = osqp.OSQP()
+
+ self.osqp_solver.setup(
+ 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 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:
+ self.tcps = []
+ else:
+ self.tcps = tcps
+ self.tcp_ids = []
+ try:
+ for tcp in self.tcps:
+ tcp_id = self.model.getFrameId(tcp)
+ self.tcp_ids.append(tcp_id)
+ except:
+ print(f'tcp_id of {tcp} not found')
+
+ def add_frame(self,frame_name, position, rotationXYZ):
+ '''
+ :param frame_name: str
+ :param position: [x, y, z] target position (meters)
+ :param rotationXYZ: [x, y, z] target rotation (rad)
+ '''
+ camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0], rotationXYZ[1], rotationXYZ[2] )
+ camera_offset = pin.SE3(
+ camera_rotation,
+ np.array(position)
+ )
+ self.model.addFrame( pin.Frame( frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) )
+
+ def add_tool_frames(self,dict_frames):
+ self.tool_frames ={}
+ for tool_name in dict_frames:
+ tool_attr = dict_frames[tool_name]
+ position = tool_attr[0][0:3]
+ rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
+ self.add_frame(tool_name, position, rotationXYZ)
+ self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)})
+ self.data = self.model.createData()
+
+
+ def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
+ if min_j is None:
+ min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]
+ if max_j is None:
+ max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
+
+ k = 1.0 if rad_flag is True else 1.0 / 180 * pi
+
+ for i in range(7):
+ self.model.lowerPositionLimit[i] = min_j[i] * k
+ self.model.upperPositionLimit[i] = max_j[i] * k
+
+
+ def forward_kinematics(self, joint_angles, tool="omnipic"):
+ """
+ Compute forward kinematics.
+ Args:
+ joint_angles: List or array of 7 joint angles (radians)
+ tool: Name of frame to compute
+ """
+ if len(joint_angles) != 7:
+ raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
+
+ # Create configuration vector
+ q = pin.neutral(self.model)
+ for i, angle in enumerate(joint_angles):
+ q[i] = angle
+
+ # Compute forward kinematics
+ pin.forwardKinematics(self.model, self.data, q)
+ pin.updateFramePlacements(self.model, self.data)
+
+ # Get frame transform
+ if tool in self.tcps:
+ frame_id = self.tcp_ids[self.tcps.index(tool)]
+ else:
+ try:
+ frame_id = self.tool_frames[tool]
+ except:
+ print(f'{tool} definition not found')
+
+ frame_transform = self.data.oMf[frame_id]
+
+ # Extract results
+ position = frame_transform.translation.copy()
+ rotation = frame_transform.rotation.copy()
+
+ # Compute RPY
+ rpy = pin.rpy.matrixToRpy(rotation)
+
+ # Compute quaternion
+ pose = np.concatenate([position, rpy], axis=0)
+ return pose
+
+ def inverse_kinematics(self, target_position, target_rpy=None,
+ target_quat=None, initial_guess=None,
+ max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
+ """
+ Compute inverse kinematics using differential IK with multiple strategies.
+ Args:
+ target_position: [x, y, z] target position (meters)
+ target_rpy: [roll, pitch, yaw] target orientation (radians)
+ target_quat: [x, y, z, w] target orientation as quaternion
+ initial_guess: Initial joint angles (radians)
+ max_iter: Maximum iterations
+ tolerance: Error tolerance
+ debug: Print debug information
+ tool: the frame name ('scissor', 'camera', 'ee')
+ Returns:
+ sts, q_solved
+ """
+ # Build target SE3 placement
+ if target_quat is not None:
+ quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2])
+ target_rotation = quat.matrix()
+ elif target_rpy is not None:
+ target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
+ target_rpy[1],
+ target_rpy[2])
+ else:
+ target_rotation = np.eye(3)
+
+ target_placement = pin.SE3(target_rotation, np.array(target_position))
+
+ # Try multiple initial guesses
+ initial_guesses = []
+
+ if initial_guess is not None:
+ initial_guesses.append(initial_guess)
+ else:
+ # Try different initial configurations
+ initial_guesses.append([0.1] * 7) # Zero config
+
+
+ best_solution = None
+ best_error = float('inf')
+
+ for guess_idx, guess in enumerate(initial_guesses):
+ q = pin.neutral(self.model)
+ for i, angle in enumerate(guess):
+ if i < len(q):
+ q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
+ self.model.upperPositionLimit[i])
+ q_ref = q.copy()
+
+ # Differential IK with adaptive damping
+ damping = 0.1
+ damping_reduction = 0.95
+ iter_count = 0
+ prev_error = float('inf')
+
+ # ee_frame_id = self.tool_frames[tool]
+ if tool in self.tcps:
+ ee_frame_id = self.tcp_ids[self.tcps.index(tool)]
+ else:
+ try:
+ ee_frame_id = self.tool_frames[tool]
+ except:
+ print(f'{tool} definition not found')
+
+ J = pin.computeFrameJacobian(
+ self.model,
+ self.data,
+ q,
+ ee_frame_id,
+ pin.ReferenceFrame.LOCAL
+ )
+
+ pin.forwardKinematics(self.model, self.data, q)
+ pin.updateFramePlacements(self.model, self.data)
+
+ current_placement = self.data.oMf[ee_frame_id]
+
+ error_SE3 = current_placement.actInv(target_placement)
+ error_vec = pin.log(error_SE3).vector
+
+ while iter_count < max_iter:
+ # Compute forward kinematics
+
+ pin.computeJointJacobians(self.model, self.data, q)
+ pin.framesForwardKinematics(self.model, self.data, q)
+
+ # Get current end-effector placement
+ current_placement = self.data.oMf[ee_frame_id]
+
+ # Compute error
+ error_SE3 = current_placement.actInv(target_placement)
+ error_vec = pin.log(error_SE3).vector
+ error_norm = np.linalg.norm(error_vec)
+
+ if error_norm < tolerance:
+ if error_norm < best_error:
+ best_error = error_norm
+ best_solution = q[:7].copy()
+ break
+
+ # Check if error is increasing (diverging)
+ if error_norm > prev_error * 1.1 and iter_count > 10:
+ damping = min(1.0, damping * 1.5)
+ else:
+ damping = max(0.01, damping * damping_reduction)
+
+
+ J = pin.getFrameJacobian(
+ self.model,
+ self.data,
+ ee_frame_id,
+ pin.ReferenceFrame.LOCAL
+ )
+
+ # =========================
+ # QP-based IK
+ # =========================
+ w_ref = 0.0001
+ w_limit_mid = 0.00002
+
+ J_eff = pin.Jlog6(error_SE3) @ J #J #
+
+ H = J_eff.T @ self.W @ J_eff
+
+
+ H += damping * damping * self.joint_motion_weight
+ H += w_ref * np.eye(7)
+ H += w_limit_mid * self.w_q_limit
+
+ H_triu = sparse.triu(H).tocsc()
+
+ g = -J_eff.T @ self.W @ error_vec
+ g += w_ref * (q[:7] - q_ref[:7])
+ g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
+
+ # -------------------------
+ # Joint velocity constraints
+ # -------------------------
+ dq_limit = np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ]) # rad per iteration
+
+ lb = -dq_limit * np.ones(7)
+ ub = dq_limit * np.ones(7)
+
+ # -------------------------
+ # Joint position constraints
+ # -------------------------
+
+ q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
+ q_max_step = self.model.upperPositionLimit[:7] - q[:7]
+
+ lb = np.maximum(lb, q_min_step)
+ ub = np.minimum(ub, q_max_step)
+
+ # -------------------------
+ # Solve QP
+ # ------------------------
+ # Update solver
+ self.osqp_solver.update(
+ Px= H_triu.data, #H[np.triu_indices(7)], #
+ q=g,
+ l=lb,
+ u=ub
+ )
+
+ # Solve
+ result = self.osqp_solver.solve()
+ if result.info.status != 'solved':
+ break
+
+ dq = result.x
+
+ if dq is None:
+ break
+
+ # Apply joint limits with scaling
+ alpha = 1.0
+ q = pin.integrate(self.model, q, alpha * dq)
+
+ prev_error = error_norm
+ iter_count += 1
+
+ if best_solution is not None:
+ collision = self.collision_detect(q=best_solution, stop_at_first_collision=True)
+
+ if collision is False:
+ # return best_solution, True, best_error, iter_count
+ return 0, best_solution.tolist()
+ else:
+ return -2, q[:7].copy().tolist()
+ else:
+ # 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)
+
+ if q.shape[0] != self.model.nq:
+ raise ValueError(f"q size mismatch: expected {self.model.nq}, got {q.shape[0]}")
+
+ # Update robot kinematics
+ pin.forwardKinematics(self.model, self.data, q)
+ pin.updateGeometryPlacements(
+ self.model,
+ self.data,
+ self.geom_model,
+ self.geom_data,
+ q
+ )
+
+ # Now compute collisions on the updated geometry model
+ collision = pin.computeCollisions(
+ self.geom_model,
+ self.geom_data,
+ stop_at_first_collision
+ )
+
+ if verbose:
+ print(f"the collision is {collision}\n")
+
+ for k, cr in enumerate(self.geom_data.collisionResults):
+ if cr.isCollision():
+ cp = self.geom_model.collisionPairs[k]
+ geom1 = self.geom_model.geometryObjects[cp.first]
+ geom2 = self.geom_model.geometryObjects[cp.second]
+
+ print(
+ f"collision pair {k}: "
+ f"{geom1.name} <--> {geom2.name}"
+ )
+
+ return bool(collision)
+
+ def remove_adjacent_collision_pairs(self, verbose=True):
+ """
+ Remove collision pairs between same/adjacent parent joints.
+ This avoids false positives such as:
+ base_link_0 <--> link_1_0
+ """
+ pairs_to_remove = []
+
+ for pair_id, pair in enumerate(self.geom_model.collisionPairs):
+ geom1 = self.geom_model.geometryObjects[pair.first]
+ geom2 = self.geom_model.geometryObjects[pair.second]
+
+ j1 = geom1.parentJoint
+ j2 = geom2.parentJoint
+
+ # Same body or directly connected bodies
+ if j1 == j2 or abs(j1 - j2) <= 1:
+ pairs_to_remove.append(pair_id)
+
+ if verbose:
+ print(
+ "Removing adjacent pair:",
+ pair_id,
+ geom1.name,
+ "<-->",
+ geom2.name,
+ "parentJoint:",
+ j1,
+ j2,
+ )
+
+ for pair_id in reversed(pairs_to_remove):
+ self.geom_model.removeCollisionPair(
+ self.geom_model.collisionPairs[pair_id]
+ )
+
+ # Important: recreate geometry data after modifying pairs
+ self.geom_data = pin.GeometryData(self.geom_model)
+
+ if verbose:
+ print("Remaining collision pairs:", len(self.geom_model.collisionPairs))
+
+ def quaternion_to_euler(self, q):
+ """
+ Convert quaternion to Euler angles (roll, pitch, yaw)
+
+ Args:
+ qx, qy, qz, qw: quaternion components
+
+ Returns:
+ tuple: (roll, pitch, yaw) in radians
+ """
+ # Roll (x-axis rotation)
+ sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
+ cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
+ roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+ # Pitch (y-axis rotation)
+ sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
+ if abs(sinp) >= 1:
+ pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
+ else:
+ pitch = np.arcsin(sinp)
+
+ # Yaw (z-axis rotation)
+ siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
+ cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
+ yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+ return [roll, pitch, yaw]
+
+ # def invese_kinematics_velocity(self, target_position, target_rpy=None,
+ # target_quat=None, initial_guess=None, tool="ee"):
+ # """
+ # Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
+ #
+ # Args:
+ # target_position: [x, y, z] target position (meters)
+ # target_rpy: [roll, pitch, yaw] target orientation (radians)
+ # target_quat: [x, y, z, w] target orientation as quaternion
+ # initial_guess: Initial joint angles (radians)
+ # tool: the frame name ('scissor', 'camera', 'ee')
+ #
+ # Returns:
+ # joint_velocity: np.array()
+ # """
+ # # Build target SE3 placement
+ # if target_quat is not None:
+ # quat = pin.Quaternion(target_quat[3], target_quat[0],
+ # target_quat[1], target_quat[2])
+ # target_rotation = quat.matrix()
+ # elif target_rpy is not None:
+ # target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
+ # target_rpy[1],
+ # target_rpy[2])
+ # else:
+ # target_rotation = np.eye(3)
+ #
+ # target_placement = pin.SE3(target_rotation, np.array(target_position))
+ #
+
+ def compute_jacobian(self, joint_angles, tool="ee"):
+ """Compute geometric Jacobian (6x7)"""
+ q = pin.neutral(self.model)
+ for i, angle in enumerate(joint_angles):
+ q[i] = angle
+
+ pin.forwardKinematics(self.model, self.data, q)
+ pin.updateFramePlacements(self.model, self.data)
+ ee_frame_id = self.tool_frames[tool]
+ J = pin.computeFrameJacobian(self.model, self.data, q, ee_frame_id)
+
+ return J
+
+ def get_subchain_jacobian(self, joint_angles, frame_names ):
+
+ q = pin.neutral(self.model)
+
+ all_active_joints = self.get_active_joints_from_frame(frame_names)
+
+ for i in range(7):
+ q[i] = joint_angles[i]
+
+ pin.forwardKinematics(self.model, self.data, q)
+ pin.updateFramePlacements(self.model, self.data)
+ pin.computeJointJacobians(self.model, self.data, q)
+
+ Js = []
+
+ for frame_name, active_joints in zip(frame_names, all_active_joints):
+ frame_id = self.model.getFrameId(frame_name)
+
+ J = pin.getFrameJacobian(
+ self.model,
+ self.data,
+ frame_id,
+ pin.ReferenceFrame.LOCAL
+ )
+ Js.append(J[:, active_joints])
+
+ return Js
+
+ def get_active_joints_from_frame(self, frame_names):
+ """
+ Return active joint indices affecting a frame.
+
+ Example:
+ frame_name='link_4'
+ -> [0,1,2,3]
+ """
+ all_active_joint_ids = []
+ for frame_name in frame_names:
+ frame_id = self.model.getFrameId(frame_name)
+
+ # Parent joint of this frame
+ joint_id = self.model.frames[frame_id].parentJoint
+
+ print(f'frame_id = {frame_id}, and joint_id = {joint_id}')
+
+ active_joint_ids = []
+
+ # Traverse upward to root
+ while joint_id > 0:
+ # Pinocchio joint indexing:
+ # universe joint = 0
+ # robot joints start from 1
+
+ active_joint_ids.append(joint_id - 1)
+
+ # Move to parent joint
+ joint_id = self.model.parents[joint_id]
+
+ # Reverse so order becomes base -> tip
+ active_joint_ids.reverse()
+ all_active_joint_ids.append(active_joint_ids)
+
+ return all_active_joint_ids
+
+
+
+
+from Robotic_Arm.rm_robot_interface import *
+import numpy as np
+import math
+
+class rm75_kine_api():
+ def __init__(self):
+ # ---------- rm75 official algorithm -----------
+ print(f'------- the realman official kinematic initialising -------')
+ arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm
+ force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
+ # Initialize the robotic arm model and sensor type in the algorithm
+ self.robot_kine_rm = Algo(arm_model, force_type)
+
+ self.cfg_j_limit()
+
+ self.work_frames = {
+ 'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
+ }
+
+ self.tool_name = "no_tool"
+ self.work_name = "work"
+
+ def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
+ if max_j is None:
+ max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
+ if min_j is None:
+ min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
+
+ max_j = np.array(max_j)
+ min_j = np.array(min_j)
+ if rad_flag:
+ self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
+ self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
+ else:
+ self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
+ self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
+
+ def cfg_work_frame(self , frame_name):
+ self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
+
+ def get_work_frame(self):
+ return self.robot_kine_rm.rm_algo_get_curr_workframe()
+
+ def cfg_tool_frame(self, frame_name ):
+ self.robot_kine_rm.rm_algo_set_toolframe(self.tool_frames[frame_name])
+
+ def get_tool_frame(self):
+ return self.robot_kine_rm.rm_algo_get_curr_toolframe()
+
+ def quaternion_to_euler(self, q):
+ """
+ Convert quaternion to Euler angles (roll, pitch, yaw)
+
+ Args:
+ qx, qy, qz, qw: quaternion components
+
+ Returns:
+ tuple: (roll, pitch, yaw) in radians
+ """
+ # Roll (x-axis rotation)
+ sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
+ cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
+ roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+ # Pitch (y-axis rotation)
+ sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
+ if abs(sinp) >= 1:
+ pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
+ else:
+ pitch = np.arcsin(sinp)
+
+ # Yaw (z-axis rotation)
+ siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
+ cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
+ yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+ return [roll, pitch, yaw]
+
+ def add_tool_frames(self, dict_frames):
+ self.tool_frames = {}
+ for tool_name in dict_frames:
+ tool_attr = dict_frames[tool_name]
+ position = tool_attr[0][0:3]
+ rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
+ f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
+
+ self.tool_frames.update({tool_name:f})
+
+ def forward_kinematics(self, joint_angles, flag = 1 , tool="omnipic", work="work"):
+ '''
+ :param joint_angles: list of joint values, in rad
+ :param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
+ :param return: [x,y,z,rx,ry,rz], m & rad
+ '''
+ if tool != self.tool_name:
+ self.tool_name = tool
+ self.cfg_tool_frame(tool)
+ if work != self.work_name:
+ self.work_name = work
+ self.cfg_work_frame(work)
+ return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[float(q_s)*180.0/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", step_arm_angle = 15.0):
+ '''
+ :param target_position: list of position values, m
+ :param target_rpy: list of rpy values, rad
+ :param initial_guess: initial guess of angles, rad
+ :param tool: tool name, refer to self.tool_frames
+ :param work: work name, refer to self.work_frames
+
+ return ret: state of ik calculation, 0:success, -2: out of workspace
+ [q_]: the ik calculated angles for joints, rad
+ '''
+ if tool != self.tool_name:
+ self.tool_name = tool
+ self.cfg_tool_frame(tool)
+ if work != self.work_name:
+ self.work_name = work
+ self.cfg_work_frame(work)
+
+ target = list(target_position) + list(target_rpy)
+
+ if initial_guess is not None:
+ 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, 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)
+
+ 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)
+
+ # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
+ #
+ # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
+ # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
+ if int(ret) < 0:
+ return ret, [ q/180*math.pi for q in q_out]
+ elif pose_dis < 0.01:
+ return ret, [ q/180*math.pi for q in q_out]
+ else:
+ return -10, [ q/180*math.pi for q in q_out]
+
+def cal_pose_deviation(pose1, pose2):
+ d_fk_p1 = np.array(pose1) - np.array(pose2)
+ for j in [3, 4, 5]:
+ while d_fk_p1[j] > math.pi:
+ d_fk_p1[j] -= 2 * math.pi
+ while d_fk_p1[j] < -math.pi:
+ d_fk_p1[j] += 2 * math.pi
+ d_fk = np.linalg.norm(d_fk_p1)
+ return d_fk
diff --git a/kine_ctrl/test1.py b/kine_ctrl/test1.py
index 03d3aca..ad9325d 100644
--- a/kine_ctrl/test1.py
+++ b/kine_ctrl/test1.py
@@ -3,7 +3,7 @@
# conda activate coppeliasim
# env fix, in terminal: fix_robotics_env.sh
-from rm75_kinematics import rm75_kinematics
+from rm75_kinematics import rm75_kinematics
from math import pi
import numpy as np