ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d1e096cb3da8566a668d7493e370382bf4071b24
IK_qp/rm75_kinematics.py
T
LiuzhengSJ d1e096cb3d delete unnecessary files.
2026-08-05 16:14:39 +01:00

1237 lines
43 KiB
Python
Raw Blame History

'''
Robotic arm kinematics solver for realman-75
Files:
rm75_kinematics.py
How to use:
Example in test1.py:
python test1.py
'''
import pinocchio as pin
from Robotic_Arm.rm_robot_interface import *
import numpy as np
import osqp
from scipy import sparse
import math
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 = 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 = 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)
self.ik_sts = False
self.joint_solved = [0.0] * 7
def get_ik_result(self, target_position, target_rpy, initial_guess, tool):
'''
Try both the RM official solver and the QP solver; on success, store the result.
return: True if either solver succeeded, joint_solved in rad.
'''
ret_rm, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool)
if ret_rm != 0:
ret_rm, q_out = self.robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300)
if ret_rm == 0:
self.joint_solved = q_out
self.ik_sts = True
else:
self.ik_sts = False
return self.ik_sts, self.joint_solved
def get_fk_result(self, joint_angles, tool):
'''
Get the forward kinematics result for given joint angles and tool.
:param joint_angles: list of joint values, in rad
:param return: [x,y,z,rx,ry,rz], m & rad
return: [x, y, z, rx, ry, rz] in meters and radians.
'''
fk_result = self.robot_kine_rm.forward_kinematics(joint_angles, flag=1, tool=tool)
return fk_result
def get_self_collision(self, joint_angles):
'''
Check for self-collision given joint angles.
:param joint_angles: list of joint values, in rad
:return: True if self-collision is detected, False otherwise.
'''
collision_detected = self.robot_kine_qp.collision_detect(joint_angles)
return collision_detected
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 * math.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
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
Reference in New Issue View Git Blame Copy Permalink