ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki 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 # 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 rm75_mjc import MuJoCoPositionController
from Robotic_Arm.rm_robot_interface import * from Robotic_Arm.rm_robot_interface import *
@ -28,19 +30,19 @@ def demo_position_control():
robot_mjk.print_state() robot_mjk.print_state()
time.sleep(0.5) time.sleep(0.5)
print("\n[Test 2] Move joint 2 to -30 degrees") # print("\n[Test 2] Move joint 2 to -30 degrees")
robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0]) # robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
robot_mjk.wait_until_reached() # robot_mjk.wait_until_reached()
robot_mjk.print_state() # robot_mjk.print_state()
time.sleep(0.5) # 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") print("\n[Test 4] Return home\n")
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")
robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0]) robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
robot_mjk.wait_until_reached() robot_mjk.wait_until_reached()
robot_mjk.print_state() 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" 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 robot_kine_qp = kine_qp()
target_pos = [0.3, 0.2, 0.4] # More reachable position print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}')
target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m") joint_solution, success, error = robot_kine_qp.inverse_kinematics(
target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
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,
max_iter=500, debug=False, tool=tool_name 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: if success:
print(f"✓ Solution found! Error: {error:.6f} m") print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
for i, angle in enumerate(joint_solution):
print(f" Joint {i + 1}: {degrees(angle):7.2f}°")
# 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 ----------- # ---------- rm75 official algorithm -----------
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 Robotic arm robot_kine_rm = kine_rm()
force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}')
# Initialize the robotic arm model and sensor type in the algorithm 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)
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())
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}')
ret, phi = robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) if ret == 0:
params = rm_inverse_kinematics_params_t([0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0], print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ')
[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}")
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() robot_mjk.stop()

413
kine_ctrl/rm75_kine_qp.py
View File

@ -16,8 +16,9 @@ class KinematicsSolver():
""" """
for realman 75b for realman 75b
Initialize robotic arm kinematics using Pinocchio (ROS2 version). 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) self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
# ------------------------------------------------- # -------------------------------------------------
@ -44,7 +45,7 @@ class KinematicsSolver():
) )
self.model.addFrame( self.model.addFrame(
pin.Frame( pin.Frame(
"scissor_tcp", "scissor",
self.model.getJointId("joint_7"), self.model.getJointId("joint_7"),
self.model.getFrameId("link_7"), self.model.getFrameId("link_7"),
scissor_offset, scissor_offset,
@ -66,7 +67,7 @@ class KinematicsSolver():
) )
self.model.addFrame( self.model.addFrame(
pin.Frame( pin.Frame(
"camera_frame", "camera",
self.model.getJointId("joint_7"), self.model.getJointId("joint_7"),
self.model.getFrameId("link_7"), self.model.getFrameId("link_7"),
camera_offset, camera_offset,
@ -79,8 +80,8 @@ class KinematicsSolver():
# ------------------------------------------------- # -------------------------------------------------
self.tool_frames = { self.tool_frames = {
"scissor": self.model.getFrameId("scissor_tcp"), "scissor": self.model.getFrameId("scissor"),
"camera": self.model.getFrameId("camera_frame"), "camera": self.model.getFrameId("camera"),
"ee": self.model.getFrameId("ee") "ee": self.model.getFrameId("ee")
} }
@ -106,9 +107,10 @@ class KinematicsSolver():
self.nv = 7 self.nv = 7
# Full dense symmetric matrix structure # 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') A_sparse = sparse.eye(7, format='csc')
@ -138,6 +140,8 @@ class KinematicsSolver():
Returns: Returns:
dict: Position, rotation, rpy, quaternion dict: Position, rotation, rpy, quaternion
unit: position: m
rpy: rad
""" """
if len(joint_angles) != 7: if len(joint_angles) != 7:
raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}") raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
@ -167,15 +171,15 @@ class KinematicsSolver():
return { return {
'position': position, 'position': position,
'rotation': rotation, # 'rotation': rotation,
'rpy': rpy, 'rpy': rpy,
'quaternion': [quat.x, quat.y, quat.z, quat.w], 'quaternion': [quat.x, quat.y, quat.z, quat.w],
'transform': frame_transform # 'transform': frame_transform
} }
def inverse_kinematics(self, target_position, target_rpy=None, def inverse_kinematics(self, target_position, target_rpy=None,
target_quat=None, initial_guess=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. Compute inverse kinematics using differential IK with multiple strategies.
@ -230,7 +234,7 @@ class KinematicsSolver():
self.model.upperPositionLimit[i]) self.model.upperPositionLimit[i])
# Differential IK with adaptive damping # Differential IK with adaptive damping
damping = 0.01 damping = 0.1
damping_reduction = 0.95 damping_reduction = 0.95
iter_count = 0 iter_count = 0
prev_error = float('inf') prev_error = float('inf')
@ -242,9 +246,20 @@ class KinematicsSolver():
self.data, self.data,
q, q,
ee_frame_id, 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: while iter_count < max_iter:
# Compute forward kinematics # Compute forward kinematics
@ -281,7 +296,7 @@ class KinematicsSolver():
self.model, self.model,
self.data, self.data,
ee_frame_id, ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED pin.ReferenceFrame.LOCAL
) )
# ========================= # =========================
@ -319,20 +334,32 @@ class KinematicsSolver():
# ------------------------ # ------------------------
# Update solver # Update solver
self.osqp_solver.update( self.osqp_solver.update(
Px=H_triu.data, Px= H_triu.data, #H[np.triu_indices(7)], #
q=g, q=g,
l=lb, l=lb,
u=ub u=ub
) )
print("iter", iter_count)
print("error", error_norm)
print("cond(H)", np.linalg.cond(H))
# Solve # Solve
result = self.osqp_solver.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': if result.info.status != 'solved':
break break
dq = result.x dq = result.x
print(f'pred = {J @ dq} and error_vec = {error_vec}')
if dq is None: if dq is None:
break break
@ -344,180 +371,198 @@ class KinematicsSolver():
prev_error = error_norm prev_error = error_norm
iter_count += 1 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: if best_solution is not None:
print(
"converged",
error_norm,
position_error
)
return best_solution, True, best_error return best_solution, True, best_error
else: else:
return None, False, None return None, False, None
def invese_kinematics_velocity(self, target_position, target_rpy=None, # def invese_kinematics_velocity(self, target_position, target_rpy=None,
target_quat=None, initial_guess=None, tool="ee"): # target_quat=None, initial_guess=None, tool="ee"):
""" # """
Compute the converging velocity (motion direction) of joints based on qp inverse kinematics. # Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
#
Args: # Args:
target_position: [x, y, z] target position (meters) # target_position: [x, y, z] target position (meters)
target_rpy: [roll, pitch, yaw] target orientation (radians) # target_rpy: [roll, pitch, yaw] target orientation (radians)
target_quat: [x, y, z, w] target orientation as quaternion # target_quat: [x, y, z, w] target orientation as quaternion
initial_guess: Initial joint angles (radians) # initial_guess: Initial joint angles (radians)
tool: the frame name ('scissor', 'camera', 'ee') # tool: the frame name ('scissor', 'camera', 'ee')
#
Returns: # Returns:
joint_velocity: np.array() # joint_velocity: np.array()
""" # """
# Build target SE3 placement # # Build target SE3 placement
if target_quat is not None: # if target_quat is not None:
quat = pin.Quaternion(target_quat[3], target_quat[0], # quat = pin.Quaternion(target_quat[3], target_quat[0],
target_quat[1], target_quat[2]) # target_quat[1], target_quat[2])
target_rotation = quat.matrix() # target_rotation = quat.matrix()
elif target_rpy is not None: # elif target_rpy is not None:
target_rotation = pin.rpy.rpyToMatrix(target_rpy[0], # target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
target_rpy[1], # target_rpy[1],
target_rpy[2]) # target_rpy[2])
else: # else:
target_rotation = np.eye(3) # target_rotation = np.eye(3)
#
target_placement = pin.SE3(target_rotation, np.array(target_position)) # target_placement = pin.SE3(target_rotation, np.array(target_position))
#
# Try multiple initial guesses # # Try multiple initial guesses
initial_guesses = [] # initial_guesses = []
#
if initial_guess is not None: # if initial_guess is not None:
initial_guesses.append(initial_guess) # initial_guesses.append(initial_guess)
else: # else:
# Try different initial configurations # # Try different initial configurations
initial_guesses.append([0.1] * 7) # Zero config # initial_guesses.append([0.1] * 7) # Zero config
initial_guesses.append([radians(30), radians(45), radians(30), # initial_guesses.append([radians(30), radians(45), radians(30),
radians(-45), radians(30), radians(-30), 0]) # radians(-45), radians(30), radians(-30), 0])
initial_guesses.append([radians(-30), radians(45), radians(-30), # initial_guesses.append([radians(-30), radians(45), radians(-30),
radians(45), radians(30), radians(30), 0]) # radians(45), radians(30), radians(30), 0])
#
best_solution = None # best_solution = None
best_error = float('inf') # best_error = float('inf')
#
for guess_idx, guess in enumerate(initial_guesses): # for guess_idx, guess in enumerate(initial_guesses):
q = pin.neutral(self.model) # q = pin.neutral(self.model)
for i, angle in enumerate(guess): # for i, angle in enumerate(guess):
if i < len(q): # if i < len(q):
q[i] = np.clip(angle, self.model.lowerPositionLimit[i], # q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
self.model.upperPositionLimit[i]) # self.model.upperPositionLimit[i])
#
# Differential IK with adaptive damping # # Differential IK with adaptive damping
damping = 0.01 # damping = 0.01
damping_reduction = 0.95 # damping_reduction = 0.95
iter_count = 0 # iter_count = 0
prev_error = float('inf') # prev_error = float('inf')
#
ee_frame_id = self.tool_frames[tool] # ee_frame_id = self.tool_frames[tool]
#
J = pin.computeFrameJacobian( # J = pin.computeFrameJacobian(
self.model, # self.model,
self.data, # self.data,
q, # q,
ee_frame_id, # ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED # pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
) # )
#
while iter_count < max_iter: # while iter_count < max_iter:
# Compute forward kinematics # # Compute forward kinematics
#
pin.computeJointJacobians(self.model, self.data, q) # pin.computeJointJacobians(self.model, self.data, q)
pin.framesForwardKinematics(self.model, self.data, q) # pin.framesForwardKinematics(self.model, self.data, q)
#
# Get current end-effector placement # # Get current end-effector placement
#
current_placement = self.data.oMf[ee_frame_id] # current_placement = self.data.oMf[ee_frame_id]
#
# Compute error # # Compute error
error_SE3 = current_placement.actInv(target_placement) # error_SE3 = current_placement.actInv(target_placement)
error_vec = pin.log(error_SE3).vector # error_vec = pin.log(error_SE3).vector
error_norm = np.linalg.norm(error_vec) # error_norm = np.linalg.norm(error_vec)
#
if error_norm < tolerance: # if error_norm < tolerance:
joint_angles = q[:7].copy() # joint_angles = q[:7].copy()
fk_result = self.forward_kinematics(joint_angles, tool=tool) # fk_result = self.forward_kinematics(joint_angles, tool=tool)
position_error = np.linalg.norm(fk_result['position'] - np.array(target_position)) # position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
#
if position_error < best_error: # if position_error < best_error:
best_error = position_error # best_error = position_error
best_solution = joint_angles # best_solution = joint_angles
break # break
#
# Check if error is increasing (diverging) # # Check if error is increasing (diverging)
if error_norm > prev_error * 1.1 and iter_count > 10: # if error_norm > prev_error * 1.1 and iter_count > 10:
damping = min(1.0, damping * 1.5) # damping = min(1.0, damping * 1.5)
else: # else:
damping = max(0.01, damping * damping_reduction) # damping = max(0.01, damping * damping_reduction)
#
J = pin.getFrameJacobian( # J = pin.getFrameJacobian(
self.model, # self.model,
self.data, # self.data,
ee_frame_id, # ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED # pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
) # )
#
# ========================= # # =========================
# QP-based IK # # QP-based IK
# ========================= # # =========================
#
H = J.T @ self.W @ J # H = J.T @ self.W @ J
H += damping * damping * np.eye(7) # H += damping * damping * np.eye(7)
#
H_triu = sparse.triu(H).tocsc() # H_triu = sparse.triu(H).tocsc()
#
g = -J.T @ self.W @ error_vec # g = -J.T @ self.W @ error_vec
#
# ------------------------- # # -------------------------
# Joint velocity constraints # # Joint velocity constraints
# ------------------------- # # -------------------------
#
dq_limit = 0.05 # rad per iteration # dq_limit = 0.05 # rad per iteration
#
lb = -dq_limit * np.ones(7) # lb = -dq_limit * np.ones(7)
ub = dq_limit * np.ones(7) # ub = dq_limit * np.ones(7)
#
# ------------------------- # # -------------------------
# Joint position constraints # # Joint position constraints
# ------------------------- # # -------------------------
#
q_min_step = self.model.lowerPositionLimit[:7] - q[:7] # q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
q_max_step = self.model.upperPositionLimit[:7] - q[:7] # q_max_step = self.model.upperPositionLimit[:7] - q[:7]
#
lb = np.maximum(lb, q_min_step) # lb = np.maximum(lb, q_min_step)
ub = np.minimum(ub, q_max_step) # ub = np.minimum(ub, q_max_step)
#
# ------------------------- # # -------------------------
# Solve QP # # Solve QP
# ------------------------ # # ------------------------
# Update solver # # Update solver
self.osqp_solver.update( # self.osqp_solver.update(
Px=H_triu.data, # Px=H_triu.data,
q=g, # q=g,
l=lb, # l=lb,
u=ub # u=ub
) # )
#
# Solve # # Solve
result = self.osqp_solver.solve() # result = self.osqp_solver.solve()
#
if result.info.status != 'solved': # if result.info.status != 'solved':
break # break
#
dq = result.x # dq = result.x
#
if dq is None: # if dq is None:
break # break
#
# Apply joint limits with scaling # # Apply joint limits with scaling
alpha = 0.5 # alpha = 0.5
q = pin.integrate(self.model, q, alpha * dq) # q = pin.integrate(self.model, q, alpha * dq)
#
prev_error = error_norm # prev_error = error_norm
iter_count += 1 # iter_count += 1
#
if best_solution is not None: # if best_solution is not None:
return best_solution, True, best_error # return best_solution, True, best_error
else: # else:
return None, False, None # return None, False, None
def compute_jacobian(self, joint_angles, tool="ee"): def compute_jacobian(self, joint_angles, tool="ee"):
"""Compute geometric Jacobian (6x7)""" """Compute geometric Jacobian (6x7)"""
@ -557,7 +602,7 @@ class KinematicsSolver():
self.model, self.model,
self.data, self.data,
frame_id, frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED pin.ReferenceFrame.LOCAL
) )
Js.append(J[:, active_joints]) 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