ZhongyuWang-cart/IK_qp
1
0
Fork 0
forked from ZhengLiu-cart/IK_qp
Code Pull Requests Activity

the branch with concise robot kinematics class for rm75

Browse Source
This commit is contained in:
LiuzhengSJ
2026-08-05 13:08:38 +01:00
parent 8ff29b1cc9
commit d86438115f
38 changed files with 88 additions and 65203 deletions
Download Patch File Download Diff File Expand all files Collapse all files
kine_ctrl/rm75_kine/__init__.py
View File
kine_ctrl/rm75_kine/rm75_kine_qp.py
+994
View File
@@ -0,0 +1,994 @@
#!/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
kine_ctrl/rm75_kine/rm75_kine_rm.py
+179
View File
@@ -0,0 +1,179 @@
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)
print(joint_angles)
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