YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add qp_ik files

Browse Source
This commit is contained in:
YikaiFu-cart
2026-06-26 01:31:20 +08:00
parent 96b9014b7f
commit 7255daa69c
16 changed files with 1871 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
ik_qp/README.md Normal file
View File

@ -0,0 +1,36 @@
### This repo is for inverse kinematics and verification
In this branch, the qp-based inverse kinematics method is modified as a python class. The user can call it as in `main.py`
Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
Verification is done with Mujoco simulation.
Key specifications:
1. Time consumption.
2. Success rate
3. Minial joint variation.
Next:\
Comparison with Realman official IK method.
Embedded with current demo.
### Comparison (05June2026):
- With current dual arm joint limit,
```
ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
```
the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\
At least one solver works out the ik, rate = **74%**.
- With realman-75 physical joint limit,
```
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
lb = -ub
```
the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\
At least one solver works out the ik, rate = **84%**.

6
ik_qp/kine_ctrl/fix_robotics_env.sh Executable file
View File

@ -0,0 +1,6 @@
#!/bin/bash
echo "Fixing robotics environment..."
conda activate coppeliasim
export PYTHONPATH="/home/zl/miniforge3/envs/coppeliasim/lib/python3.10/site-packages"
pip install osqp==0.6.2.post8 --force-reinstall
python -c "import osqp; print(f'OSQP version: {osqp.__version__}')"

128
ik_qp/kine_ctrl/main.py Normal file
View File

@ -0,0 +1,128 @@
# conda activate coppeliasim
# env fix, in terminal: fix_robotics_env.sh
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 *
import time
from math import radians, degrees, pi, cos, sin
import numpy as np
# pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
# load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
tools_in_ee = {
'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
}
# joint limit
ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
# lb = -ub
tool_name = "scissor"
def main():
"""Demonstrate pure position control"""
# Create controller
robot_mjk = MuJoCoPositionController()
# ----------- rm75 qp based kine ------------
robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
robot_kine_qp.add_tool_frames(tools_in_ee)
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
# ---------- rm75 official algorithm -----------
robot_kine_rm = kine_rm()
robot_kine_rm.add_tool_frames(tools_in_ee)
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
# -------------- for comparison ----------------
print(f'in the comparison part')
if True:
result = np.array([[0,0],[0,0]], dtype=np.int32) # to collect ik result qp_fk, qp_ik, rm_fk, rm_ik
solve_sum = 0
for i in range(10):
print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(ub, lb)
print(f'the predefined joints are {joint_rand}')
# -------------- fk ------------------
fk_qp_p1 = robot_kine_qp.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
fk_rm_p1 = robot_kine_rm.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
d_fk = cal_pose_deviation(pose1=fk_rm_p1, pose2=fk_qp_p1)
print(f'fk_qp_p1 = {fk_qp_p1}, fk_rm_p1 = {fk_rm_p1}, d_fk = {d_fk}\n')
# ----------- ik ----------------
t_p = fk_rm_p1
joint_rand_init = np.random.uniform(ub, lb)
print(f'the guess is {joint_rand_init}')
ret_qp, q = robot_kine_qp.inverse_kinematics( target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
if ret_qp == 0:
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
if d_p_ik < 0.01:
result[0][1] += 1
robot_mjk.send_command(q)
robot_mjk.wait_until_reached()
robot_mjk.print_state()
else:
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
if ret_rm == 0:
fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
if d_p_ik < 0.01:
result[1][1] += 1
else:
print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}')
if ret_qp == 0 or ret_rm == 0:
solve_sum += 1
print(f'results with qp and rm for ik are {result}')
print(f'solve_sum is {solve_sum}')
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] > pi:
d_fk_p1[j] -= 2 * pi
while d_fk_p1[j] < -pi:
d_fk_p1[j] += 2 * pi
d_fk = np.linalg.norm(d_fk_p1)
return d_fk
if __name__ == "__main__":
main()

809
ik_qp/kine_ctrl/rm75_kine_qp.py Normal file
View File

@ -0,0 +1,809 @@
#!/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()
# ---------- 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])
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_posture = 0.0001
J_eff = pin.Jlog6(error_SE3) @ J #J #
H = J_eff.T @ self.W @ J_eff
# H = J.T @ self.W @ J
H += damping * damping * np.eye(7)
H += w_posture * np.eye(7)
H_triu = sparse.triu(H).tocsc()
g = -J_eff.T @ self.W @ error_vec
g += w_posture * (q[:7] - q_ref[:7])
# 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, #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
ik_qp/kine_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]

297
ik_qp/kine_ctrl/rm75_mjc.py Normal file
View File

@ -0,0 +1,297 @@
#!/usr/bin/env python3
"""
Pure Position Control for MuJoCo - No velocity commands, no forces
Direct joint position control with smoothing
"""
import mujoco
import mujoco.viewer
import numpy as np
import threading
import time
from pathlib import Path
class MuJoCoPositionController:
"""
Pure position control - directly sets joint positions
No velocity commands, no forces - completely stable
"""
def __init__(self, urdf_path="./urdf_rm75/RM75-B.urdf", smoothness=0.05, enable_viewer=True):
"""
Args:
urdf_path: Path to URDF file
smoothness: Motion smoothness (0.02=very smooth, 0.1=fast)
enable_viewer: Show MuJoCo viewer
"""
# Load model
self.model = mujoco.MjModel.from_xml_path(urdf_path)
self.data = mujoco.MjData(self.model)
self.time_interval = 0.02
print(f'time interval: {self.model.opt.timestep}')
# Robot info
self.n_joints = self.model.njnt
# Get joint limits
self.joint_lower_limits = []
self.joint_upper_limits = []
for i in range(self.n_joints):
self.joint_lower_limits.append(self.model.jnt_range[i, 0])
self.joint_upper_limits.append(self.model.jnt_range[i, 1])
print(f"Loaded robot: {self.n_joints} joints")
for i in range(self.n_joints):
print(
f" {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]")
# Target joint angles (in radians)
self.target_joints = self.data.qpos[:self.n_joints].copy()
# Smoothing factor (0-1, lower = smoother)
self.smoothness = smoothness
# Thread safety
self.command_lock = threading.Lock()
self.feedback_lock = threading.Lock()
self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
self.max_ang_inc = 0.02
# Control flags
self.running = False
self.simulation_thread = None
# Viewer
self.viewer = None
if enable_viewer:
try:
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
print("Viewer launched")
except Exception as e:
print(f"Viewer warning: {e}")
self.start()
def start(self):
"""Start the simulation thread"""
if self.running:
return
self.running = True
self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True)
self.simulation_thread.start()
print("Simulation thread started")
def stop(self):
"""Stop the simulation thread"""
self.running = False
if self.simulation_thread:
self.simulation_thread.join(timeout=2.0)
if self.viewer:
self.viewer.close()
print("Simulation stopped")
def send_command(self, joint_positions):
"""
Send target joint positions
Args:
joint_positions: Array of target joint angles (radians)
"""
cmd = np.array(joint_positions[:self.n_joints], dtype=np.float64)
# Apply joint limits
for i in range(self.n_joints):
cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
with self.command_lock:
self.target_joints = cmd
def get_feedback(self):
"""Get current joint positions"""
with self.feedback_lock:
return self.current_feedback_joint.copy()
def get_target(self):
"""Get current target positions"""
with self.command_lock:
return self.target_joints.copy()
def _simulation_loop(self):
"""
Main simulation loop - PURE POSITION CONTROL
No velocity commands, no forces - just direct position setting
"""
last_time = time.time()
# For smooth interpolation
current_joints = self.data.qpos[:self.n_joints].copy()
while self.running:
# Get target command
with self.command_lock:
target = self.target_joints.copy()
# Get current positions
current_joints = self.data.qpos[:self.n_joints].copy()
# Smooth interpolation toward target
# This creates natural motion without velocity commands
alpha = self.smoothness
next_joints = current_joints + np.clip(alpha * (target - current_joints) , -self.max_ang_inc, self.max_ang_inc)
# DIRECT POSITION CONTROL - Set joint positions
self.data.qpos[:self.n_joints] = next_joints
# IMPORTANT: Set velocities to zero to prevent physics from moving joints
# This ensures pure kinematic control
self.data.qvel[:self.n_joints] = 0
# Step physics (this will apply gravity, collisions, etc. to other bodies)
mujoco.mj_step(self.model, self.data)
# After step, ensure our joint positions are maintained
# (Physics might have altered them slightly)
self.data.qpos[:self.n_joints] = next_joints
self.data.qvel[:self.n_joints] = 0
# Update feedback
with self.feedback_lock:
self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
# Sync viewer
if self.viewer:
self.viewer.sync()
# Maintain real-time speed
elapsed = time.time() - last_time
sleep_time = self.time_interval - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
last_time = time.time()
def move_to_joints(self, target, duration=1.0):
"""
Move to target joints over specified duration
Args:
target: Target joint joints
duration: Time to complete movement (seconds)
"""
start_js = self.get_feedback()
end_js = np.array(target[:self.n_joints])
# Apply limits
for i in range(self.n_joints):
end_js[i] = np.clip(end_js[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
n_steps = int(duration / self.time_interval)
print(f" Moving over {duration}s ({n_steps} steps)")
for step in range(n_steps):
alpha = (step + 1) / n_steps
# Use easing for smoother motion
ease_alpha = 1 - (1 - alpha) ** 2 # Quadratic ease-out
current_target = start_js + ease_alpha * (end_js - start_js)
self.send_command(current_target)
time.sleep(self.time_interval)
# Ensure exact target
self.send_command(end_js)
time.sleep(0.1)
def wait_until_reached(self, tolerance=0.01, timeout=10.0):
"""
Wait until robot reaches target position
Args:
tolerance: Position error tolerance (radians)
timeout: Maximum wait time (seconds)
"""
start_time = time.time()
while time.time() - start_time < timeout:
current = self.get_feedback()
target = self.get_target()
error = np.max(np.abs(target - current))
if error < tolerance:
return True
time.sleep(0.01)
return False
def print_state(self):
"""Print current robot state"""
joints = self.get_feedback()
target = self.get_target()
print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...")
print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...")
# Demo
def demo_position_control():
"""Demonstrate pure position control"""
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
if not Path(urdf_path).exists():
print(f"Error: URDF not found at {urdf_path}")
return
print("=" * 60)
print("Pure Position Control Demo")
print("=" * 60)
# Create controller
robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
robot.start()
time.sleep(1)
print("\n[Test 1] Move joint 1 to 45 degrees")
robot.send_command([0.785, 0, 0, 0, 0, 0, 0])
robot.wait_until_reached()
robot.print_state()
time.sleep(0.5)
print("\n[Test 2] Move joint 2 to -30 degrees")
robot.send_command([0, -0.524, 0, 0, 0, 0, 0])
robot.wait_until_reached()
robot.print_state()
time.sleep(0.5)
print("\n[Test 3] Move multiple joints simultaneously")
robot.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
robot.wait_until_reached()
robot.print_state()
time.sleep(0.5)
print("\n[Test 4] Return home")
robot.send_command([0, 0, 0, 0, 0, 0, 0])
robot.wait_until_reached()
robot.print_state()
print("\n" + "=" * 60)
print("✓ All tests passed! Robot is stable and controllable.")
print("=" * 60)
print("\nInteractive mode - close viewer to exit")
try:
while robot.viewer and robot.viewer.is_running():
time.sleep(0.1)
except KeyboardInterrupt:
pass
robot.stop()
if __name__ == "__main__":
demo_position_control()

9
ik_qp/kine_ctrl/urdf_rm75/RM75-B.csv Normal file
View File

@ -0,0 +1,9 @@
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
base_link,0.00049987,5.2709E-05,0.060019,0,0,0,0.83887,0.0017232,-3.1058E-06,-3.7924E-05,0.0017051,1.3691E-06,0.00090158,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,,连杆1-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
link_1,1.4803E-07,-0.021108,-0.025186,0,0,0,0.59354,0.0012661,6.0354E-09,-6.3788E-09,0.0011817,-0.00021121,0.00056132,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,,连杆2-1,link_1,joint_1,joint_1,revolute,0,0,0.2405,0,0,0,base_link,0,0,1,60,3.14,-3.106,3.106,,,,,,,,
link_2,4.2145E-07,-0.076129,0.011078,0,0,0,0.43285,0.0012584,1.4694E-09,-5.7413E-09,0.00031747,0.000279,0.0012225,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,,连杆3-1,link_2,joint_2,joint_2,revolute,0,0,0,-1.5708,0,0,link_1,0,0,1,60,3.14,-2.2689,2.2689,,,,,,,,
link_3,-3.2093E-07,-0.023545,-0.027347,0,0,0,0.43132,0.00079433,1.02E-09,1.3908E-08,0.00073037,-0.00014262,0.00031507,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,,连杆4-1,link_3,joint_3,joint_3,revolute,0,-0.256,0,1.5708,0,0,link_2,0,0,1,30,3.14,-3.106,3.106,,,,,,,,
link_4,5.0722E-06,-0.059593,0.010569,0,0,0,0.28963,0.00063737,7.0681E-08,3.8708E-08,0.00015648,0.00014461,0.00061418,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,,连杆5-1,link_4,joint_4,joint_4,revolute,0,0,0,-1.5708,0,0,link_3,0,0,1,30,3.14,-2.356,2.356,,,,,,,,
link_5,2.7551E-07,-0.018042,-0.02154,0,0,0,0.23942,0.00028595,1.9823E-09,-1.192E-09,0.00026273,-4.424E-05,0.0001199,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,,连杆6-1,link_5,joint_5,joint_5,revolute,0,-0.21,0,1.5708,0,0,link_4,0,0,1,10,3.14,-3.106,3.106,,,,,,,,
link_6,3.4947E-06,-0.059381,0.0073681,0,0,0,0.2188,0.00035054,3.4456E-08,1.7975E-08,0.00010493,7.8243E-05,0.00033448,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,,连杆7-1,link_6,joint_6,joint_6,revolute,0,0,0,-1.5708,0,0,link_5,0,0,1,10,3.14,-2.234,2.234,,,,,,,,
link_7,0.00081557,1.3323E-05,-0.012705,0,0,0,0.065037,2.1144E-05,2.2774E-08,2.5471E-08,1.8109E-05,1.019E-08,3.19E-05,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,,末端法兰件 方案一-1,link_7,joint_7,joint_7,revolute,0,-0.144,0,1.5708,0,0,link_6,0,0,1,10,3.14,-6.28,6.28,,,,,,,,
1 Link Name Center of Mass X Center of Mass Y Center of Mass Z Center of Mass Roll Center of Mass Pitch Center of Mass Yaw Mass Moment Ixx Moment Ixy Moment Ixz Moment Iyy Moment Iyz Moment Izz Visual X Visual Y Visual Z Visual Roll Visual Pitch Visual Yaw Mesh Filename Color Red Color Green Color Blue Color Alpha Collision X Collision Y Collision Z Collision Roll Collision Pitch Collision Yaw Collision Mesh Filename Material Name SW Components Coordinate System Axis Name Joint Name Joint Type Joint Origin X Joint Origin Y Joint Origin Z Joint Origin Roll Joint Origin Pitch Joint Origin Yaw Parent Joint Axis X Joint Axis Y Joint Axis Z Limit Effort Limit Velocity Limit Lower Limit Upper Calibration rising Calibration falling Dynamics Damping Dynamics Friction Safety Soft Upper Safety Soft Lower Safety K Position Safety K Velocity
2 base_link 0.00049987 5.2709E-05 0.060019 0 0 0 0.83887 0.0017232 -3.1058E-06 -3.7924E-05 0.0017051 1.3691E-06 0.00090158 0 0 0 0 0 0 package://RM75-B/meshes/base_link.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/base_link.STL 连杆1-1 base_link 0 0 0 0 0 0 0 0 0
3 link_1 1.4803E-07 -0.021108 -0.025186 0 0 0 0.59354 0.0012661 6.0354E-09 -6.3788E-09 0.0011817 -0.00021121 0.00056132 0 0 0 0 0 0 package://RM75-B/meshes/link_1.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/link_1.STL 连杆2-1 link_1 joint_1 joint_1 revolute 0 0 0.2405 0 0 0 base_link 0 0 1 60 3.14 -3.106 3.106
4 link_2 4.2145E-07 -0.076129 0.011078 0 0 0 0.43285 0.0012584 1.4694E-09 -5.7413E-09 0.00031747 0.000279 0.0012225 0 0 0 0 0 0 package://RM75-B/meshes/link_2.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/link_2.STL 连杆3-1 link_2 joint_2 joint_2 revolute 0 0 0 -1.5708 0 0 link_1 0 0 1 60 3.14 -2.2689 2.2689
5 link_3 -3.2093E-07 -0.023545 -0.027347 0 0 0 0.43132 0.00079433 1.02E-09 1.3908E-08 0.00073037 -0.00014262 0.00031507 0 0 0 0 0 0 package://RM75-B/meshes/link_3.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/link_3.STL 连杆4-1 link_3 joint_3 joint_3 revolute 0 -0.256 0 1.5708 0 0 link_2 0 0 1 30 3.14 -3.106 3.106
6 link_4 5.0722E-06 -0.059593 0.010569 0 0 0 0.28963 0.00063737 7.0681E-08 3.8708E-08 0.00015648 0.00014461 0.00061418 0 0 0 0 0 0 package://RM75-B/meshes/link_4.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/link_4.STL 连杆5-1 link_4 joint_4 joint_4 revolute 0 0 0 -1.5708 0 0 link_3 0 0 1 30 3.14 -2.356 2.356
7 link_5 2.7551E-07 -0.018042 -0.02154 0 0 0 0.23942 0.00028595 1.9823E-09 -1.192E-09 0.00026273 -4.424E-05 0.0001199 0 0 0 0 0 0 package://RM75-B/meshes/link_5.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/link_5.STL 连杆6-1 link_5 joint_5 joint_5 revolute 0 -0.21 0 1.5708 0 0 link_4 0 0 1 10 3.14 -3.106 3.106
8 link_6 3.4947E-06 -0.059381 0.0073681 0 0 0 0.2188 0.00035054 3.4456E-08 1.7975E-08 0.00010493 7.8243E-05 0.00033448 0 0 0 0 0 0 package://RM75-B/meshes/link_6.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/link_6.STL 连杆7-1 link_6 joint_6 joint_6 revolute 0 0 0 -1.5708 0 0 link_5 0 0 1 10 3.14 -2.234 2.234
9 link_7 0.00081557 1.3323E-05 -0.012705 0 0 0 0.065037 2.1144E-05 2.2774E-08 2.5471E-08 1.8109E-05 1.019E-08 3.19E-05 0 0 0 0 0 0 package://RM75-B/meshes/link_7.STL 1 1 1 1 0 0 0 0 0 0 package://RM75-B/meshes/link_7.STL 末端法兰件 方案一-1 link_7 joint_7 joint_7 revolute 0 -0.144 0 1.5708 0 0 link_6 0 0 1 10 3.14 -6.28 6.28

453
ik_qp/kine_ctrl/urdf_rm75/RM75-B.urdf Normal file
View File

@ -0,0 +1,453 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="RM75-B">
<link
name="base_link">
<inertial>
<origin
xyz="0.00049987 5.2709E-05 0.060019"
rpy="0 0 0" />
<mass
value="1.862" />
<inertia
ixx="0.0017232"
ixy="-3.1058E-06"
ixz="-3.7924E-05"
iyy="0.0017051"
iyz="1.3691E-06"
izz="0.00090158" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="link_1">
<inertial>
<origin
xyz="0.000241 -0.013273 -0.00995"
rpy="0 0 0" />
<mass
value="1.574" />
<inertia
ixx="0.002487573"
ixy="0.000009663"
ixz="-0.000007909"
iyy="0.002321038"
iyz="0.000179393"
izz="0.001450554" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_1.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_1.STL" />
</geometry>
</collision>
</link>
<joint
name="joint_1"
type="revolute">
<origin
xyz="0 0 0.2405"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="link_1" />
<axis
xyz="0 0 1" />
<limit
lower="-3.106"
upper="3.106"
effort="60"
velocity="3.14" />
</joint>
<link
name="link_2">
<inertial>
<origin
xyz="-0.000357 -0.106789 0.005329"
rpy="0 0 0" />
<mass
value="1.217" />
<inertia
ixx="0.003494121"
ixy="0.000002921"
ixz="-0.000005613"
iyy="0.000892721"
iyz="-0.000583884"
izz="0.003444080" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_2.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_2.STL" />
</geometry>
</collision>
</link>
<joint
name="joint_2"
type="revolute">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="link_1" />
<child
link="link_2" />
<axis
xyz="0 0 1" />
<limit
lower="-2.2689"
upper="2.2689"
effort="60"
velocity="3.14" />
</joint>
<link
name="link_3">
<inertial>
<origin
xyz="0.000003 -0.01398 -0.011324"
rpy="0 0 0" />
<mass
value="1.11" />
<inertia
ixx="0.001836663"
ixy="0.000002259"
ixz="-0.000004216"
iyy="0.001498875"
iyz="0.000037167"
izz="0.001062545" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_3.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_3.STL" />
</geometry>
</collision>
</link>
<joint
name="joint_3"
type="revolute">
<origin
xyz="0 -0.256 0"
rpy="1.5708 0 0" />
<parent
link="link_2" />
<child
link="link_3" />
<axis
xyz="0 0 1" />
<limit
lower="-3.106"
upper="3.106"
effort="30"
velocity="3.14" />
</joint>
<link
name="link_4">
<inertial>
<origin
xyz="-0.000005 -0.084658 0.004747"
rpy="0 0 0" />
<mass
value="0.685" />
<inertia
ixx="0.001282444"
ixy="-0.000000551"
ixz="-0.000000630"
iyy="0.000373013"
iyz="-0.000232084"
izz="0.001256177" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_4.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_4.STL" />
</geometry>
</collision>
</link>
<joint
name="joint_4"
type="revolute">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="link_3" />
<child
link="link_4" />
<axis
xyz="0 0 1" />
<limit
lower="-2.356"
upper="2.356"
effort="30"
velocity="3.14" />
</joint>
<link
name="link_5">
<inertial>
<origin
xyz="0.000078 -0.012937 -0.008781"
rpy="0 0 0" />
<mass
value="0.619" />
<inertia
ixx="0.000627336"
ixy="0.000001636"
ixz="-0.000001345"
iyy="0.000542455"
iyz="0.000034970"
izz="0.000370291" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_5.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_5.STL" />
</geometry>
</collision>
</link>
<joint
name="joint_5"
type="revolute">
<origin
xyz="0 -0.21 0"
rpy="1.5708 0 0" />
<parent
link="link_4" />
<child
link="link_5" />
<axis
xyz="0 0 1" />
<limit
lower="-3.106"
upper="3.106"
effort="10"
velocity="3.14" />
</joint>
<link
name="link_6">
<inertial>
<origin
xyz="-0.000014 -0.078524 0.002819"
rpy="0 0 0" />
<mass
value="0.602" />
<inertia
ixx="0.000780774"
ixy="-0.000000121"
ixz="-0.000000469"
iyy="0.000289973"
iyz="-0.000120513"
izz="0.000763955" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_6.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_6.STL" />
</geometry>
</collision>
</link>
<joint
name="joint_6"
type="revolute">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="link_5" />
<child
link="link_6" />
<axis
xyz="0 0 1" />
<limit
lower="-2.234"
upper="2.234"
effort="10"
velocity="3.14" />
</joint>
<link
name="link_7">
<inertial>
<origin
xyz="0.001094 -0.000077 -0.010119"
rpy="0 0 0" />
<mass
value="0.107" />
<inertia
ixx="0.000044123"
ixy="-0.000000064"
ixz="0.0000003"
iyy="0.000035078"
iyz="-0.000000029"
izz="0.000065445" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_7.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="meshes/link_7.STL" />
</geometry>
</collision>
</link>
<joint
name="joint_7"
type="revolute">
<origin
xyz="0 -0.144 0"
rpy="1.5708 0 0" />
<parent
link="link_6" />
<child
link="link_7" />
<axis
xyz="0 0 1" />
<limit
lower="-6.28"
upper="6.28"
effort="10"
velocity="3.14" />
</joint>
</robot>

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/base_link.STL Normal file
View File

Binary file not shown.

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_1.STL Normal file
View File

Binary file not shown.

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_2.STL Normal file
View File

Binary file not shown.

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_3.STL Normal file
View File

Binary file not shown.

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_4.STL Normal file
View File

Binary file not shown.

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_5.STL Normal file
View File

Binary file not shown.

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_6.STL Normal file
View File

Binary file not shown.

BIN
ik_qp/kine_ctrl/urdf_rm75/meshes/link_7.STL Normal file
View File

Binary file not shown.