ZhengLiu-cart/HarvestingRealman
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

necessary file for kinematics

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-02 15:45:29 +01:00
parent ed0f332883
commit a875ef6280
3 changed files with 963 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

824
harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py Normal file
View File

@ -0,0 +1,824 @@
#!/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"):
"""
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)
self.cfg_j_limit()
q_range = (
self.model.upperPositionLimit[:7] -
self.model.lowerPositionLimit[:7]
)
self.w_q_limit = np.diag(1.0 / (q_range ** 2))
self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
# ---------- for reused qp_solver ------------------
self.nv = 7
# Full dense symmetric matrix structure
# P_template = np.triu(np.ones((7, 7)))
self.P_pattern = sparse.triu(np.ones((7,7))).tocsc()
P_sparse = sparse.csc_matrix(self.P_pattern)
A_sparse = sparse.eye(7, format='csc')
self.osqp_solver = osqp.OSQP()
self.osqp_solver.setup(
P=P_sparse,
q=np.zeros(7),
A=A_sparse,
l=-np.ones(7),
u=np.ones(7),
verbose=False,
warm_start=True,
polish=False
)
self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
# Smaller value => joint moves more actively
# Larger value => joint moves less / more lazy
self.joint_motion_weight = np.diag([
0.3, 0.3, 0.3, 1.0,
1.0, 1.0, 1.0
])
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]
if rad_flag:
for i in range(7):
self.model.lowerPositionLimit[i] = min_j[i]
self.model.upperPositionLimit[i] = max_j[i]
else:
for i in range(7):
self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
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
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)}")
# 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
frame_id = self.tool_frames[tool]
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
# quat = pin.Quaternion(rotation)
pose = np.concatenate([position, rpy], axis=0)
return pose
# return {
# 'position': position,
# # 'rotation': rotation,
# 'rpy': rpy,
# 'quaternion': [quat.x, quat.y, quat.z, quat.w],
# # 'transform': frame_transform
# }
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:
tuple: (joint_angles, success, error)
"""
# 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]
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
# print("\n initial error =", np.linalg.norm(error_vec))
# print(error_vec)
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.00005
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.08, 0.08, 0.08, 0.08, 0.05, 0.05, 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, #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:
# return best_solution, True, best_error, iter_count
return 0, best_solution.tolist()
else:
# return q[:7].copy(), False, error_norm, iter_count
return -1, q[:7].copy().tolist()
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))
#
# # 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)"""
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
def plan_cartesian_trajectory(self, start_pos, end_pos,
start_rpy=None, end_rpy=None,
num_steps=20, tool='ee'):
"""
Plan a Cartesian trajectory with IK for each waypoint.
"""
# Get current end-effector pose if start_rpy not provided
if start_rpy is None:
# Try to find a valid starting configuration
test_angles = [0.1] * 7
fk_test = self.forward_kinematics(test_angles,tool=tool)
start_rpy = fk_test['rpy']
if end_rpy is None:
end_rpy = start_rpy
# First, check if target is reachable
print(f"\nChecking if target is reachable...")
target_pos = end_pos
target_rpy = end_rpy
test_solution, success, error = self.inverse_kinematics(
target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool
)
if not success:
print(f"Warning: Target may be unreachable or difficult to reach")
print(f"Trying with relaxed tolerance...")
# Initial guess for IK (start with zero configuration)
current_angles = [0.1] * 7
trajectory = []
print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})")
print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})")
print("-" * 60)
for i in range(num_steps + 1):
t = i / num_steps
# Interpolate position
pos = [
start_pos[0] + t * (end_pos[0] - start_pos[0]),
start_pos[1] + t * (end_pos[1] - start_pos[1]),
start_pos[2] + t * (end_pos[2] - start_pos[2])
]
# Interpolate orientation
rpy = [
start_rpy[0] + t * (end_rpy[0] - start_rpy[0]),
start_rpy[1] + t * (end_rpy[1] - start_rpy[1]),
start_rpy[2] + t * (end_rpy[2] - start_rpy[2])
]
# Compute IK
joint_angles, success, error = self.inverse_kinematics(
pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool
)
if not success:
print(f" Waypoint {i}: IK failed!")
break
# Verify
fk_verify = self.forward_kinematics(joint_angles, tool=tool)
trajectory.append({
'step': i,
't': t,
'position': pos,
'rpy': rpy,
'joint_angles': joint_angles,
'actual_position': fk_verify['position'],
'error': error
})
# Update current angles for next iteration
current_angles = joint_angles
if i % 5 == 0 or i == num_steps:
print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), "
f"error={error:.6f}m")
return trajectory
def main():
"""Main test function"""
rm75 = KinematicsSolver()
# Test 1: Forward Kinematics
print("\n1. Forward Kinematics Test")
print("-" * 40)
tool_name = "scissor"
joint_angles_zero = [0.1] * 7
fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name)
print(f"Init configuration:")
print(f" Position: ({fk_result['position'][0]:.3f}, "
f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m")
# 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
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
import time
init_joints = [0.2] * 7
time0 = time.time()
for ii in range(100):
joint_solution, success, error = rm75.inverse_kinematics(
target_pos, target_rpy=target_rpy, initial_guess=init_joints,
max_iter=500, debug=False, tool=tool_name
)
time1 = time.time()
print(f"Time: {time1 - time0}")
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}°")
# Verify
fk_verify = rm75.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 = rm75.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 = rm75.forward_kinematics([0.1] * 7, tool=tool_name)
trajectory = rm75.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 = rm75.get_subchain_jacobian(
joint_angles=joint_angles_zero,
frame_names=frame_names
)
print(f'Js_sub: {Js_sub}')
return rm75, trajectory
if __name__ == "__main__":
rm75, trajectory = main()
print("\n" + "=" * 60)
print("All tests completed!")
print("=" * 60)

133
harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py Normal file
View File

@ -0,0 +1,133 @@
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_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.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=[q_s*180/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"):
'''
: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 = target_position + 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, 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/180*math.pi for q in q_out]

6
harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py Normal file
View File

@ -0,0 +1,6 @@
def main():
print('Hi from harvest_arm_ctrl.')
if __name__ == '__main__':
main()