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

refine qp based controller

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-03 20:15:48 +01:00
parent 3febe65b6a
commit fb64f3c73a
3 changed files with 363 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files

181
kine_ctrl/main.py
View File

@ -1,8 +1,10 @@
from casadi import print_operator
# conda activate coppeliasim
# env fix, in terminal: ~/fix_robotics_env.sh
# env fix, in terminal: fix_robotics_env.sh
from rm75_kine_qp import KinematicsSolver as kine_ctrl
from rm75_kine_qp import KinematicsSolver as kine_qp
from rm75_kine_rm import rm75_kine_api as kine_rm
from rm75_mjc import MuJoCoPositionController
from Robotic_Arm.rm_robot_interface import *
@ -28,19 +30,19 @@ def demo_position_control():
robot_mjk.print_state()
time.sleep(0.5)
print("\n[Test 2] Move joint 2 to -30 degrees")
robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
robot_mjk.wait_until_reached()
robot_mjk.print_state()
time.sleep(0.5)
# print("\n[Test 2] Move joint 2 to -30 degrees")
# robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
# robot_mjk.wait_until_reached()
# robot_mjk.print_state()
# time.sleep(0.5)
#
# print("\n[Test 3] Move multiple joints simultaneously")
# robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
# robot_mjk.wait_until_reached()
# robot_mjk.print_state()
# time.sleep(0.5)
print("\n[Test 3] Move multiple joints simultaneously")
robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
robot_mjk.wait_until_reached()
robot_mjk.print_state()
time.sleep(0.5)
print("\n[Test 4] Return home")
print("\n[Test 4] Return home\n")
robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
robot_mjk.wait_until_reached()
robot_mjk.print_state()
@ -48,139 +50,62 @@ def demo_position_control():
#---------------------------------------------------------------------------
robot_kine = kine_ctrl()
# Test 1: Forward Kinematics
print("\n1. Forward Kinematics Test")
print("-" * 40)
joints = [10, 20, -30, -40, 50, 60, 70]
joints_rad = [radians(j) for j in joints] #radians(joints)
# target_position = [0.3, 0.2, 0.4]
# target_rpy = [0.0, 0.0, 3.14*0.25]
target_position = [0.17892041, 0.25274317, 0.83107248]
target_rpy = [0.78576018, 0.67554633, 1.86302226]
target_p = target_position + target_rpy
# target_p_rad = [radians(pos) for pos in target_position] + target_rpy
initial_guess = [11, 20, -30, -40, 50, 60, 71] # [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] #
initial_guess_rad = [ radians(j) for j in initial_guess ]
tool_name = "scissor"
joint_angles_zero = [0.1] * 7
fk_result = robot_kine.forward_kinematics(joint_angles_zero, tool=tool_name)
# Test 2: Inverse Kinematics with more reachable target
print("\n2. Inverse Kinematics Test")
print("-" * 40)
# Try a simpler target first
target_pos = [0.3, 0.2, 0.4] # More reachable position
target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation
robot_kine_qp = kine_qp()
print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}')
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
init_joints = [0.2] * 7
time0 = time.time()
for ii in range(100):
joint_solution, success, error = robot_kine.inverse_kinematics(
target_pos, target_rpy=target_rpy, initial_guess=init_joints,
joint_solution, success, error = robot_kine_qp.inverse_kinematics(
target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
max_iter=500, debug=False, tool=tool_name
)
time1 = time.time()
print(f"Time: {time1 - time0}")
print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
if success:
print(f"✓ Solution found! Error: {error:.6f} m")
for i, angle in enumerate(joint_solution):
print(f" Joint {i + 1}: {degrees(angle):7.2f}°")
print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
# Verify
fk_verify = robot_kine.forward_kinematics(joint_solution,tool=tool_name)
print(
f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
else:
print("✗ IK failed to find a solution!")
# Test 3: Jacobian
print("\n3. Jacobian Matrix")
print("-" * 40)
J = robot_kine.compute_jacobian(joint_angles_zero, tool=tool_name)
print(f"Jacobian shape: {J.shape}")
for i in range(min(3, J.shape[0])):
row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)])
print(f" Row {i + 1}: {row_str}")
# Test 4: Trajectory Planning with reachable positions
print("\n4. Cartesian Trajectory Planning")
print("-" * 40)
start_pos = [0.3, 0.0, 0.4] # Start position
end_pos = [0.3, 0.0, 0.55] # End position (smaller movement)
fk0 = robot_kine.forward_kinematics([0.1] * 7, tool=tool_name)
trajectory = robot_kine.plan_cartesian_trajectory(
start_pos,
end_pos,
start_rpy=fk0['rpy'],
end_rpy=[
fk0['rpy'][0] + radians(10),
fk0['rpy'][1],
fk0['rpy'][2]
],
num_steps=10,
tool=tool_name
)
if trajectory:
print(f"\n✓ Generated {len(trajectory)} waypoints")
if success:
print("✓ Inverse kinematics working (with simplified target)")
else:
print("⚠ Inverse kinematics may need tuning - try different targets")
print("\n" + "=" * 60)
print(f'test subchain Jacobian, for future obstacle avoidance')
frame_names = [
"link_2",
"link_4",
"link_7"
]
Js_sub = robot_kine.get_subchain_jacobian(
joint_angles=joint_angles_zero,
frame_names=frame_names
)
print(f'Js_sub: {Js_sub}')
# ---------- rm75 official algorithm -----------
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 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
robot_kine_rm = Algo(arm_model, force_type)
frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0])
robot_kine_rm.rm_algo_set_workframe(frame)
print(robot_kine_rm.rm_algo_get_curr_workframe())
frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0])
robot_kine_rm.rm_algo_set_toolframe(frame)
print(robot_kine_rm.rm_algo_get_curr_toolframe())
joint_max_limit = np.array([
3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
])*57
robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist())
joint_min_limit = np.array([
-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159
]) * 57
robot_kine_rm.rm_algo_set_joint_min_limit(joint_max_limit.tolist())
robot_kine_rm = kine_rm()
print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}')
ret, q = robot_kine_rm.inverse_kinematics(target_position=target_p[0:3], target_rpy=target_p[3:6],initial_guess=initial_guess, tool=tool_name)
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
ret, phi = robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
params = rm_inverse_kinematics_params_t([0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0],
[0.3, 0.0, 0.3, 3.14, 0.0, 3.14], 1)
ret, q_out = robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
print(f"rm_algo_inverse_kinematics_rm75_for_arm_angle ret: {ret} q_out: {q_out}")
print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}')
if ret == 0:
print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ')
try:
while robot_mjk.viewer and robot_mjk.viewer.is_running():
time.sleep(0.1)
except KeyboardInterrupt:
pass
print(f'\nDone\n')
# try:
# while robot_mjk.viewer and robot_mjk.viewer.is_running():
# time.sleep(0.1)
# except KeyboardInterrupt:
# pass
robot_mjk.stop()

413
kine_ctrl/rm75_kine_qp.py
View File

@ -16,8 +16,9 @@ class KinematicsSolver():
"""
for realman 75b
Initialize robotic arm kinematics using Pinocchio (ROS2 version).
unit: m, rad
"""
print(f' ------------ the qp based kinematic initialising -----------')
self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
# -------------------------------------------------
@ -44,7 +45,7 @@ class KinematicsSolver():
)
self.model.addFrame(
pin.Frame(
"scissor_tcp",
"scissor",
self.model.getJointId("joint_7"),
self.model.getFrameId("link_7"),
scissor_offset,
@ -66,7 +67,7 @@ class KinematicsSolver():
)
self.model.addFrame(
pin.Frame(
"camera_frame",
"camera",
self.model.getJointId("joint_7"),
self.model.getFrameId("link_7"),
camera_offset,
@ -79,8 +80,8 @@ class KinematicsSolver():
# -------------------------------------------------
self.tool_frames = {
"scissor": self.model.getFrameId("scissor_tcp"),
"camera": self.model.getFrameId("camera_frame"),
"scissor": self.model.getFrameId("scissor"),
"camera": self.model.getFrameId("camera"),
"ee": self.model.getFrameId("ee")
}
@ -106,9 +107,10 @@ class KinematicsSolver():
self.nv = 7
# Full dense symmetric matrix structure
P_template = np.triu(np.ones((7, 7)))
# P_template = np.triu(np.ones((7, 7)))
self.P_pattern = sparse.triu(np.ones((7,7))).tocsc()
P_sparse = sparse.csc_matrix(P_template)
P_sparse = sparse.csc_matrix(self.P_pattern)
A_sparse = sparse.eye(7, format='csc')
@ -138,6 +140,8 @@ class KinematicsSolver():
Returns:
dict: Position, rotation, rpy, quaternion
unit: position: m
rpy: rad
"""
if len(joint_angles) != 7:
raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
@ -167,15 +171,15 @@ class KinematicsSolver():
return {
'position': position,
'rotation': rotation,
# 'rotation': rotation,
'rpy': rpy,
'quaternion': [quat.x, quat.y, quat.z, quat.w],
'transform': frame_transform
# 'transform': frame_transform
}
def inverse_kinematics(self, target_position, target_rpy=None,
target_quat=None, initial_guess=None,
max_iter=200, tolerance=1e-3, debug=False, tool="ee"):
max_iter=500, tolerance=3e-3, debug=False, tool="ee"):
"""
Compute inverse kinematics using differential IK with multiple strategies.
@ -230,7 +234,7 @@ class KinematicsSolver():
self.model.upperPositionLimit[i])
# Differential IK with adaptive damping
damping = 0.01
damping = 0.1
damping_reduction = 0.95
iter_count = 0
prev_error = float('inf')
@ -242,9 +246,20 @@ class KinematicsSolver():
self.data,
q,
ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
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
print("initial error =", np.linalg.norm(error_vec))
print(error_vec)
while iter_count < max_iter:
# Compute forward kinematics
@ -281,7 +296,7 @@ class KinematicsSolver():
self.model,
self.data,
ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
pin.ReferenceFrame.LOCAL
)
# =========================
@ -319,20 +334,32 @@ class KinematicsSolver():
# ------------------------
# Update solver
self.osqp_solver.update(
Px=H_triu.data,
Px= H_triu.data, #H[np.triu_indices(7)], #
q=g,
l=lb,
u=ub
)
print("iter", iter_count)
print("error", error_norm)
print("cond(H)", np.linalg.cond(H))
# Solve
result = self.osqp_solver.solve()
print("OSQP status =", result.info.status)
print("dq =", result.x)
if result.x is not None:
print("dq norm:", np.linalg.norm(result.x))
if result.info.status != 'solved':
break
dq = result.x
print(f'pred = {J @ dq} and error_vec = {error_vec}')
if dq is None:
break
@ -344,180 +371,198 @@ class KinematicsSolver():
prev_error = error_norm
iter_count += 1
print("target:", target_position, target_rpy)
print("initial guess:", np.degrees(initial_guess))
fk0 = self.forward_kinematics(initial_guess)
print("fk guess:", fk0)
print("initial error norm:", error_norm)
print(iter_count,
error_norm,
result.info.status)
if best_solution is not None:
print(
"converged",
error_norm,
position_error
)
return best_solution, True, best_error
else:
return None, False, None
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))
# 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
initial_guesses.append([radians(30), radians(45), radians(30),
radians(-45), radians(30), radians(-30), 0])
initial_guesses.append([radians(-30), radians(45), radians(-30),
radians(45), radians(30), radians(30), 0])
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])
# Differential IK with adaptive damping
damping = 0.01
damping_reduction = 0.95
iter_count = 0
prev_error = float('inf')
ee_frame_id = self.tool_frames[tool]
J = pin.computeFrameJacobian(
self.model,
self.data,
q,
ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
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:
joint_angles = q[:7].copy()
fk_result = self.forward_kinematics(joint_angles, tool=tool)
position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
if position_error < best_error:
best_error = position_error
best_solution = joint_angles
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_WORLD_ALIGNED
)
# =========================
# QP-based IK
# =========================
H = J.T @ self.W @ J
H += damping * damping * np.eye(7)
H_triu = sparse.triu(H).tocsc()
g = -J.T @ self.W @ error_vec
# -------------------------
# Joint velocity constraints
# -------------------------
dq_limit = 0.05 # 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,
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 = 0.5
q = pin.integrate(self.model, q, alpha * dq)
prev_error = error_norm
iter_count += 1
if best_solution is not None:
return best_solution, True, best_error
else:
return None, False, None
# 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))
#
# # 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
# initial_guesses.append([radians(30), radians(45), radians(30),
# radians(-45), radians(30), radians(-30), 0])
# initial_guesses.append([radians(-30), radians(45), radians(-30),
# radians(45), radians(30), radians(30), 0])
#
# 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])
#
# # Differential IK with adaptive damping
# damping = 0.01
# damping_reduction = 0.95
# iter_count = 0
# prev_error = float('inf')
#
# ee_frame_id = self.tool_frames[tool]
#
# J = pin.computeFrameJacobian(
# self.model,
# self.data,
# q,
# ee_frame_id,
# pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
# )
#
# 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:
# joint_angles = q[:7].copy()
# fk_result = self.forward_kinematics(joint_angles, tool=tool)
# position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
#
# if position_error < best_error:
# best_error = position_error
# best_solution = joint_angles
# 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_WORLD_ALIGNED
# )
#
# # =========================
# # QP-based IK
# # =========================
#
# H = J.T @ self.W @ J
# H += damping * damping * np.eye(7)
#
# H_triu = sparse.triu(H).tocsc()
#
# g = -J.T @ self.W @ error_vec
#
# # -------------------------
# # Joint velocity constraints
# # -------------------------
#
# dq_limit = 0.05 # 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,
# 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 = 0.5
# q = pin.integrate(self.model, q, alpha * dq)
#
# prev_error = error_norm
# iter_count += 1
#
# if best_solution is not None:
# return best_solution, True, best_error
# else:
# return None, False, None
def compute_jacobian(self, joint_angles, tool="ee"):
"""Compute geometric Jacobian (6x7)"""
@ -557,7 +602,7 @@ class KinematicsSolver():
self.model,
self.data,
frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
pin.ReferenceFrame.LOCAL
)
Js.append(J[:, active_joints])

80
kine_ctrl/rm75_kine_rm.py Normal file
View File

@ -0,0 +1,80 @@
from Robotic_Arm.rm_robot_interface import *
import numpy as np
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_65 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.tool_frames = {
'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.144, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72),
'camera': rm_frame_t(frame_name="camera", pose=(0.05, 0.02, 0.10, -1.57, 0, -1.57), payload=1, x=0, y=0, z=72)
}
self.work_frames = {
'work': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
}
self.tool_name = "ee"
self.work_name = "work"
def cfg_limit(self):
joint_max_limit = np.array([
3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
]) * 57
self.robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist())
joint_min_limit = np.array([
-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159
]) * 57
self.robot_kine_rm.rm_algo_set_joint_min_limit(joint_min_limit.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 forward_kinematics(self, q, flag = 1 , tool="ee", work="work"):
'''
:param q: list of joint values, in degree
flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
'''
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=q, flag=flag)
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="ee", work="work"):
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 = target_position + target_rpy
if initial_guess is not None:
q_ref = initial_guess
else:
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
params = rm_inverse_kinematics_params_t(q_ref,
target, 1)
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
return ret, q_out