Compare commits
5
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 LiuzhengSJ
|
d1e096cb3d | ||
|
LiuzhengSJ
|
60eb37b0ed | ||
|
LiuzhengSJ
|
c0e181cc50 | ||
|
LiuzhengSJ
|
2e25951bc0 | ||
|
LiuzhengSJ
|
d86438115f |
@@ -1,53 +1,42 @@
|
||||
### 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`
|
||||
In this branch, the **integrated** inverse kinematics method is packed into one python class **rm75_kinematics** in ``rm75_kinematics.py``.
|
||||
|
||||
Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
|
||||
The user can call it as in `test1.py`, `test2.py`.
|
||||
|
||||
Verification is done with Mujoco simulation.
|
||||
How to use
|
||||
|
||||
Key specifications:
|
||||
1. Time consumption.
|
||||
2. Success rate
|
||||
3. Minial joint variation.
|
||||
|
||||
Cost:
|
||||
|
||||
<img src="img/cost.jpg" alt="Cost" width="400">
|
||||
|
||||
|
||||
### Comparison (05June2026):
|
||||
|
||||
- With current dual arm joint limit,
|
||||
```aiignore
|
||||
from rm75_kinematics import rm75_kinematics
|
||||
robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',
|
||||
mesh_dir='./urdf_rm75',
|
||||
tcps=["scissor_tcp", "camera_tcp"],
|
||||
tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
|
||||
```
|
||||
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])
|
||||
|
||||
Parameter definition:
|
||||
|
||||
- urdf_path: the robot description file, ending with `.urdf`.
|
||||
- mesh_dir: the robot parts, ending with `.stl`.
|
||||
- tcps: the tool central points defined in urdf file, if no, ignore it.
|
||||
- tools_in_ee: the installation of different tools attached to the end-effector of the arm (jont7+link7).
|
||||
|
||||
|
||||
### Current functions ###
|
||||
|
||||
1. `get_ik_result`
|
||||
```aiignore
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ],
|
||||
target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool=tool_name)
|
||||
```
|
||||
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,
|
||||
2. `get_fk_result`
|
||||
```aiignore
|
||||
p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
|
||||
```
|
||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
|
||||
lb = -ub
|
||||
|
||||
3. `get_self_collision`
|
||||
```aiignore
|
||||
self_collision_sts = robot_kine.get_self_collision(q)
|
||||
```
|
||||
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%**.
|
||||
|
||||
### update(1st July 2026)
|
||||
|
||||
In each iteration, update optimization formula:
|
||||
|
||||
- new cost item for distance from middle of the joint range.
|
||||
- set up different weight for different joints motion.
|
||||
|
||||
<img src="img/optimization.png" alt="Cost" width="400">
|
||||
|
||||
<img src="img/cons.png" alt="Cost" width="400">
|
||||
|
||||
<img src="img/osqp.png" alt="Cost" width="400">
|
||||
|
||||
Visualization:
|
||||
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 25 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 69 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 44 KiB |
@@ -1,6 +0,0 @@
|
||||
#!/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__}')"
|
||||
@@ -1,150 +0,0 @@
|
||||
|
||||
|
||||
# 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 os
|
||||
cwd = os.getcwd()
|
||||
|
||||
|
||||
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(urdf_path="./urdf_rm75/RM75-SCI.urdf")
|
||||
|
||||
|
||||
# ----------- rm75 qp based kine ------------
|
||||
robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./urdf_rm75', tcps=["scissor_tcp", "camera_tcp"])
|
||||
robot_kine_qp.add_tool_frames(tools_in_ee)
|
||||
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||
|
||||
fp = robot_kine_qp.forward_kinematics(np.ones(7)*0.3,tool='scissor_tcp')
|
||||
print(f'forward kine res = {fp}')
|
||||
ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=fp[0:3], target_rpy=fp[3:6], initial_guess=np.zeros(7),
|
||||
tool='scissor_tcp')
|
||||
|
||||
|
||||
# ---------- 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)
|
||||
|
||||
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool="no_tool")
|
||||
|
||||
print(f'ret_rm = {ret_rm}, q = {q}')
|
||||
pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
|
||||
print(f'pose = {pose}')
|
||||
print('-'*100)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# -------------- 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}')
|
||||
robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
|
||||
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_rm}')
|
||||
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}')
|
||||
robot_mjk.stop()
|
||||
|
||||
|
||||
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()
|
||||
@@ -1,9 +0,0 @@
|
||||
numpy
|
||||
pandas
|
||||
matplotlib
|
||||
tqdm
|
||||
scipy
|
||||
urdfpy
|
||||
pin
|
||||
osqp
|
||||
Robotic_Arm
|
||||
@@ -1,179 +0,0 @@
|
||||
|
||||
from Robotic_Arm.rm_robot_interface import *
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
class rm75_kine_api():
|
||||
def __init__(self):
|
||||
# ---------- rm75 official algorithm -----------
|
||||
print(f'------- the realman official kinematic initialising -------')
|
||||
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm
|
||||
force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
|
||||
# Initialize the robotic arm model and sensor type in the algorithm
|
||||
self.robot_kine_rm = Algo(arm_model, force_type)
|
||||
|
||||
self.cfg_j_limit()
|
||||
|
||||
self.work_frames = {
|
||||
'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
|
||||
}
|
||||
|
||||
self.tool_name = "no_tool"
|
||||
self.work_name = "work"
|
||||
|
||||
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
|
||||
if max_j is None:
|
||||
max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
|
||||
if min_j is None:
|
||||
min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
|
||||
|
||||
max_j = np.array(max_j)
|
||||
min_j = np.array(min_j)
|
||||
if rad_flag:
|
||||
self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
|
||||
self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
|
||||
else:
|
||||
self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
|
||||
self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
|
||||
|
||||
def cfg_work_frame(self , frame_name):
|
||||
self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
|
||||
|
||||
def get_work_frame(self):
|
||||
return self.robot_kine_rm.rm_algo_get_curr_workframe()
|
||||
|
||||
def cfg_tool_frame(self, frame_name ):
|
||||
self.robot_kine_rm.rm_algo_set_toolframe(self.tool_frames[frame_name])
|
||||
|
||||
def get_tool_frame(self):
|
||||
return self.robot_kine_rm.rm_algo_get_curr_toolframe()
|
||||
|
||||
def quaternion_to_euler(self, q):
|
||||
"""
|
||||
Convert quaternion to Euler angles (roll, pitch, yaw)
|
||||
|
||||
Args:
|
||||
qx, qy, qz, qw: quaternion components
|
||||
|
||||
Returns:
|
||||
tuple: (roll, pitch, yaw) in radians
|
||||
"""
|
||||
# Roll (x-axis rotation)
|
||||
sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
|
||||
cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
|
||||
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# Pitch (y-axis rotation)
|
||||
sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
|
||||
if abs(sinp) >= 1:
|
||||
pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
|
||||
else:
|
||||
pitch = np.arcsin(sinp)
|
||||
|
||||
# Yaw (z-axis rotation)
|
||||
siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
|
||||
cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
|
||||
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return [roll, pitch, yaw]
|
||||
|
||||
def add_tool_frames(self, dict_frames):
|
||||
self.tool_frames = {}
|
||||
for tool_name in dict_frames:
|
||||
tool_attr = dict_frames[tool_name]
|
||||
position = tool_attr[0][0:3]
|
||||
rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
|
||||
f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
|
||||
|
||||
self.tool_frames.update({tool_name:f})
|
||||
|
||||
def forward_kinematics(self, joint_angles, flag = 1 , tool="omnipic", work="work"):
|
||||
'''
|
||||
:param joint_angles: list of joint values, in rad
|
||||
:param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
|
||||
:param return: [x,y,z,rx,ry,rz], m & rad
|
||||
'''
|
||||
if tool != self.tool_name:
|
||||
self.tool_name = tool
|
||||
self.cfg_tool_frame(tool)
|
||||
if work != self.work_name:
|
||||
self.work_name = work
|
||||
self.cfg_work_frame(work)
|
||||
|
||||
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", step_arm_angle = 15.0):
|
||||
'''
|
||||
:param target_position: list of position values, m
|
||||
:param target_rpy: list of rpy values, rad
|
||||
:param initial_guess: initial guess of angles, rad
|
||||
:param tool: tool name, refer to self.tool_frames
|
||||
:param work: work name, refer to self.work_frames
|
||||
|
||||
return ret: state of ik calculation, 0:success, -2: out of workspace
|
||||
[q_]: the ik calculated angles for joints, rad
|
||||
'''
|
||||
if tool != self.tool_name:
|
||||
self.tool_name = tool
|
||||
self.cfg_tool_frame(tool)
|
||||
if work != self.work_name:
|
||||
self.work_name = work
|
||||
self.cfg_work_frame(work)
|
||||
|
||||
target = list(target_position) + list(target_rpy)
|
||||
|
||||
if initial_guess is not None:
|
||||
q_ref = [ 180/math.pi * ig for ig in initial_guess ]
|
||||
else:
|
||||
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
|
||||
ret, phi0 = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
|
||||
params = rm_inverse_kinematics_params_t(q_ref, target, 1)
|
||||
|
||||
offsets = [0.0]
|
||||
arm_angle = step_arm_angle
|
||||
while arm_angle <= 180.0:
|
||||
offsets += [arm_angle, -arm_angle]
|
||||
arm_angle += step_arm_angle
|
||||
|
||||
best_ret, best_q_out, best_dis = -1, None, None
|
||||
for offset in offsets:
|
||||
phi = ((phi0 + offset + 180.0) % 360.0) - 180.0
|
||||
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||
if int(ret) != 0:
|
||||
if best_q_out is None:
|
||||
best_ret, best_q_out = ret, q_out
|
||||
continue
|
||||
|
||||
p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
|
||||
pose_dis = cal_pose_deviation(p_fk, target)
|
||||
if pose_dis < 0.01:
|
||||
# success in ik calculation
|
||||
return ret, [q / 180 * math.pi for q in q_out]
|
||||
|
||||
if best_dis is None or pose_dis < best_dis:
|
||||
best_ret, best_q_out, best_dis = -10, q_out, pose_dis
|
||||
|
||||
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||
pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
|
||||
pose_dis = cal_pose_deviation(pose_fk, target)
|
||||
|
||||
# print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
|
||||
#
|
||||
# print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
|
||||
# print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
|
||||
if int(ret) < 0:
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
elif pose_dis < 0.01:
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
else:
|
||||
return -10, [ q/180*math.pi for q in q_out]
|
||||
|
||||
def cal_pose_deviation(pose1, pose2):
|
||||
d_fk_p1 = np.array(pose1) - np.array(pose2)
|
||||
for j in [3, 4, 5]:
|
||||
while d_fk_p1[j] > math.pi:
|
||||
d_fk_p1[j] -= 2 * math.pi
|
||||
while d_fk_p1[j] < -math.pi:
|
||||
d_fk_p1[j] += 2 * math.pi
|
||||
d_fk = np.linalg.norm(d_fk_p1)
|
||||
return d_fk
|
||||
@@ -1,297 +0,0 @@
|
||||
#!/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()
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 796 KiB |
@@ -1,106 +0,0 @@
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 1. Load the data
|
||||
# --------------------------------------------------
|
||||
file_name = "rm75b_comfort_workspace_v_minis_collision.csv"
|
||||
csv_path = Path(file_name)
|
||||
|
||||
# The file has no column names, so header=None is important.
|
||||
df_csv = pd.read_csv(
|
||||
csv_path,
|
||||
)
|
||||
rate_res = df_csv.iloc[:, :4]
|
||||
try:
|
||||
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
|
||||
except:
|
||||
rate_res_sort = rate_res
|
||||
|
||||
DECIMALS = 4
|
||||
|
||||
try:
|
||||
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
|
||||
except:
|
||||
x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
|
||||
|
||||
nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
|
||||
|
||||
|
||||
ik_rates = rate_res_sort.to_numpy()
|
||||
|
||||
# --------------------------------------------------
|
||||
# 2. Create an output directory
|
||||
# --------------------------------------------------
|
||||
output_dir = Path(file_name.split(".")[0])
|
||||
output_dir.mkdir(exist_ok=True)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 3. Use the same colour scale for every z-plane
|
||||
# --------------------------------------------------
|
||||
df = rate_res_sort
|
||||
value_min = df["ik_success_rate"].min()
|
||||
value_max = df["ik_success_rate"].max()
|
||||
|
||||
# More levels give a smoother-looking contour plot.
|
||||
levels = np.linspace(value_min, value_max, 51)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 4. Draw one contour plot for each z-plane
|
||||
# --------------------------------------------------
|
||||
for z_value, plane in df.groupby("z", sort=True):
|
||||
|
||||
# Rows become y-coordinates, columns become x-coordinates.
|
||||
grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
|
||||
|
||||
x = grid.columns.to_numpy()
|
||||
y = grid.index.to_numpy()
|
||||
ik_grid = grid.to_numpy()
|
||||
|
||||
X, Y = np.meshgrid(x, y)
|
||||
|
||||
fig, ax = plt.subplots(figsize=(7, 6))
|
||||
|
||||
contour = ax.contourf(
|
||||
X,
|
||||
Y,
|
||||
ik_grid,
|
||||
levels=levels,
|
||||
cmap="viridis",
|
||||
extend="both",
|
||||
)
|
||||
|
||||
highlight_levels = [0.6, 0.7]
|
||||
# Only plot if the levels are within the data range (optional)
|
||||
if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
|
||||
lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
|
||||
colors='red', linewidths=2, linestyles='solid')
|
||||
# Optionally label the lines
|
||||
ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
|
||||
|
||||
|
||||
colorbar = fig.colorbar(contour, ax=ax)
|
||||
colorbar.set_label("IK rate")
|
||||
|
||||
ax.set_title(f"IK rate at z = {z_value:.2f}")
|
||||
ax.set_xlabel("x")
|
||||
ax.set_ylabel("y")
|
||||
ax.set_aspect("equal")
|
||||
|
||||
fig.tight_layout()
|
||||
|
||||
output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
|
||||
fig.savefig(output_path, dpi=200, bbox_inches="tight")
|
||||
plt.close(fig)
|
||||
|
||||
print(f"Plots saved to: {output_dir.resolve()}")
|
||||
@@ -1,109 +0,0 @@
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 1. Load the data
|
||||
# --------------------------------------------------
|
||||
file_name = "workspace minisci collision.csv"
|
||||
csv_path = Path(file_name)
|
||||
|
||||
# The file has no column names, so header=None is important.
|
||||
df_csv = pd.read_csv(
|
||||
csv_path,
|
||||
header=None, # the file has no header row
|
||||
names=['x', 'y', 'z', 'ik_success_rate'] # assign names
|
||||
)
|
||||
|
||||
rate_res = df_csv.iloc[:, :4]
|
||||
try:
|
||||
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
|
||||
except:
|
||||
rate_res_sort = rate_res
|
||||
|
||||
DECIMALS = 4
|
||||
|
||||
try:
|
||||
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
|
||||
except:
|
||||
x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
|
||||
|
||||
nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
|
||||
|
||||
|
||||
ik_rates = rate_res_sort.to_numpy()
|
||||
|
||||
# --------------------------------------------------
|
||||
# 2. Create an output directory
|
||||
# --------------------------------------------------
|
||||
output_dir = Path(file_name.split(".")[0])
|
||||
output_dir.mkdir(exist_ok=True)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 3. Use the same colour scale for every z-plane
|
||||
# --------------------------------------------------
|
||||
df = rate_res_sort
|
||||
value_min = df["ik_success_rate"].min()
|
||||
value_max = df["ik_success_rate"].max()
|
||||
|
||||
# More levels give a smoother-looking contour plot.
|
||||
levels = np.linspace(value_min, value_max, 51)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 4. Draw one contour plot for each z-plane
|
||||
# --------------------------------------------------
|
||||
for z_value, plane in df.groupby("z", sort=True):
|
||||
|
||||
# Rows become y-coordinates, columns become x-coordinates.
|
||||
grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
|
||||
|
||||
x = grid.columns.to_numpy()
|
||||
y = grid.index.to_numpy()
|
||||
ik_grid = grid.to_numpy()
|
||||
|
||||
X, Y = np.meshgrid(x, y)
|
||||
|
||||
fig, ax = plt.subplots(figsize=(7, 6))
|
||||
|
||||
contour = ax.contourf(
|
||||
X,
|
||||
Y,
|
||||
ik_grid,
|
||||
levels=levels,
|
||||
cmap="viridis",
|
||||
extend="both",
|
||||
)
|
||||
|
||||
highlight_levels = [0.6, 0.7]
|
||||
# Only plot if the levels are within the data range (optional)
|
||||
if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
|
||||
lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
|
||||
colors='red', linewidths=2, linestyles='solid')
|
||||
# Optionally label the lines
|
||||
ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
|
||||
|
||||
|
||||
colorbar = fig.colorbar(contour, ax=ax)
|
||||
colorbar.set_label("IK rate")
|
||||
|
||||
ax.set_title(f"IK rate at z = {z_value:.2f}")
|
||||
ax.set_xlabel("x")
|
||||
ax.set_ylabel("y")
|
||||
ax.set_aspect("equal")
|
||||
|
||||
fig.tight_layout()
|
||||
|
||||
output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
|
||||
fig.savefig(output_path, dpi=200, bbox_inches="tight")
|
||||
plt.close(fig)
|
||||
|
||||
print(f"Plots saved to: {output_dir.resolve()}")
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 246 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 442 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 120 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 123 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 128 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 133 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 134 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 136 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 129 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 123 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 119 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 115 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 109 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 101 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 90 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 77 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 67 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 57 KiB |
-10626
File diff suppressed because it is too large
Load Diff
-10625
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,726 +0,0 @@
|
||||
"""
|
||||
RM75-B comfortable workspace evaluator.
|
||||
|
||||
You provide:
|
||||
- URDF file path '/home/zl/Downloads/urdf_rm75/RM75-B.urdf'
|
||||
- your own IK solver inside solve_ik()
|
||||
|
||||
This script computes:
|
||||
- IK success rate
|
||||
- joint-limit comfort
|
||||
- manipulability
|
||||
- singularity / condition number score
|
||||
- final comfort score
|
||||
|
||||
Recommended install:
|
||||
pip install numpy scipy urdfpy pandas matplotlib tqdm
|
||||
|
||||
Optional:
|
||||
pip install plotly
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
from tqdm import tqdm
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
from urdfpy import URDF
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
# 1. Get the absolute path of the directory containing this current script
|
||||
current_dir = Path(__file__).resolve().parent
|
||||
|
||||
# 2. Get the parent (upper) directory
|
||||
parent_dir = current_dir.parent
|
||||
|
||||
# 3. Add the parent directory to the system path
|
||||
sys.path.insert(0, str(parent_dir))
|
||||
|
||||
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
|
||||
|
||||
# Cartesian workspace grid, in meters.
|
||||
# Adjust according to your robot placement and task.
|
||||
X_RANGE = (-0.7, 0.7)
|
||||
Y_RANGE = (-0.7, 0.7)
|
||||
Z_RANGE = (-0.10, 0.8)
|
||||
|
||||
GRID_RESOLUTION = 0.05 # 5 cm. Use 0.02 for finer but slower.
|
||||
|
||||
num_orientations = 120
|
||||
|
||||
tool_name = "scissor"
|
||||
|
||||
|
||||
|
||||
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-SCI.urdf'
|
||||
|
||||
output_csv = "workspace" + tool_name + URDF_PATH.split('/')[-1].split('.')[0] + ".csv"
|
||||
|
||||
|
||||
# Comfort thresholds
|
||||
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
|
||||
MAX_CONDITION_NUMBER = 150.0
|
||||
MIN_MANIPULABILITY_RATIO = 0.10
|
||||
|
||||
# Scoring weights
|
||||
WEIGHT_IK_SUCCESS = 0.70
|
||||
WEIGHT_JOINT_LIMIT = 0.10
|
||||
WEIGHT_MANIPULABILITY = 0.1
|
||||
WEIGHT_SINGULARITY = 0.1
|
||||
|
||||
|
||||
|
||||
|
||||
# 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),
|
||||
'v_minis': np.array([[0.0, 0.1, 0.1, -np.sqrt(2) * 0.5, 0.0, 0.0, np.sqrt(2) * 0.5],[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
|
||||
|
||||
|
||||
MESH_DIR = str(Path(URDF_PATH).parent)
|
||||
|
||||
# ----------- rm75 qp based kine ------------
|
||||
robot_kine_qp = kine_qp(urdf_path=URDF_PATH, mesh_dir=MESH_DIR, tcps=["scissor_tcp", "camera_tcp"])
|
||||
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)
|
||||
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 1. USER SETTINGS
|
||||
# ============================================================
|
||||
|
||||
|
||||
|
||||
BASE_LINK = "base_link"
|
||||
TCP_LINK = "link_7"
|
||||
|
||||
JOINT_NAMES = [
|
||||
"joint_1",
|
||||
"joint_2",
|
||||
"joint_3",
|
||||
"joint_4",
|
||||
"joint_5",
|
||||
"joint_6",
|
||||
"joint_7",
|
||||
]
|
||||
|
||||
|
||||
|
||||
# Numerical Jacobian settings
|
||||
JACOBIAN_EPS = 1e-5
|
||||
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 2. TASK ORIENTATION SAMPLING
|
||||
# ============================================================
|
||||
|
||||
def make_task_orientations(num_orientations=num_orientations, seed=1):
|
||||
"""
|
||||
Random orientation sampling using RM's Euler convention:
|
||||
|
||||
R = Rz @ Ry @ Rx
|
||||
|
||||
Note:
|
||||
This samples Euler angles randomly.
|
||||
It is useful, but not perfectly uniform over SO(3).
|
||||
"""
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
orientations = []
|
||||
|
||||
for _ in range(num_orientations):
|
||||
rx = rng.uniform(-np.pi, np.pi)
|
||||
ry = rng.uniform(-np.pi / 2.0, np.pi / 2.0)
|
||||
rz = rng.uniform(-np.pi, np.pi)
|
||||
|
||||
orientations.append([rx, ry, rz])
|
||||
|
||||
return orientations
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 3. IK FUNCTION GOES HERE
|
||||
# ============================================================
|
||||
|
||||
def solve_ik(target_position, target_rotation):
|
||||
"""
|
||||
Replace this function with your own IK solver.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
target_position : np.ndarray, shape (3,)
|
||||
Desired TCP position in base_link frame.
|
||||
|
||||
target_rotation : np.ndarray, shape (3, 3)
|
||||
Desired TCP rotation matrix in base_link frame.
|
||||
|
||||
Returns
|
||||
-------
|
||||
None
|
||||
If IK fails.
|
||||
|
||||
or
|
||||
|
||||
np.ndarray, shape (7,)
|
||||
One IK solution.
|
||||
|
||||
or
|
||||
|
||||
list[np.ndarray]
|
||||
Multiple IK solutions.
|
||||
|
||||
Important:
|
||||
Joint order must be:
|
||||
[joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7]
|
||||
"""
|
||||
|
||||
initial_guess = [0.1] * 7
|
||||
|
||||
ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name, max_iter=250)
|
||||
# print(f'---- with qp ik, ret_qp: {ret_qp}, q = {q}')
|
||||
if ret_qp == 0:
|
||||
if not robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True):
|
||||
return q
|
||||
|
||||
|
||||
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name)
|
||||
# print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
|
||||
if ret_rm == 0:
|
||||
if not robot_kine_qp.collision_detect(q, stop_at_first_collision=True, verbose=True):
|
||||
return q
|
||||
|
||||
|
||||
return None
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 4. URDF / FK UTILITIES
|
||||
# ============================================================
|
||||
|
||||
def load_robot_and_limits(urdf_path):
|
||||
robot = URDF.load(urdf_path)
|
||||
|
||||
joints = []
|
||||
lower = []
|
||||
upper = []
|
||||
|
||||
joint_map = {j.name: j for j in robot.joints}
|
||||
|
||||
for name in JOINT_NAMES:
|
||||
joint = joint_map[name]
|
||||
joints.append(joint)
|
||||
|
||||
if joint.limit is None:
|
||||
raise ValueError(f"Joint {name} has no limit in URDF.")
|
||||
|
||||
lower.append(joint.limit.lower)
|
||||
upper.append(joint.limit.upper)
|
||||
|
||||
lower = np.asarray(lower, dtype=float)
|
||||
upper = np.asarray(upper, dtype=float)
|
||||
|
||||
return robot, lower, upper
|
||||
|
||||
|
||||
# def q_to_cfg(q):
|
||||
# """
|
||||
# Convert joint vector to urdfpy FK config dictionary.
|
||||
# """
|
||||
# return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
|
||||
|
||||
|
||||
# def fk_transform(robot, q):
|
||||
# """
|
||||
# Forward kinematics from base_link to TCP_LINK.
|
||||
#
|
||||
# Returns
|
||||
# -------
|
||||
# T : np.ndarray, shape (4, 4)
|
||||
# """
|
||||
# cfg = q_to_cfg(q)
|
||||
# fk = robot.link_fk(cfg=cfg)
|
||||
# tcp_link = robot.link_map[TCP_LINK]
|
||||
# return fk[tcp_link]
|
||||
#
|
||||
#
|
||||
# def fk_position(robot, q):
|
||||
# T = fk_transform(robot, q)
|
||||
# return T[:3, 3]
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 5. COMFORT METRICS
|
||||
# ============================================================
|
||||
|
||||
def is_within_joint_limits(q, lower, upper, tol=1e-8):
|
||||
q = np.asarray(q)
|
||||
return np.all(q >= lower - tol) and np.all(q <= upper + tol)
|
||||
|
||||
|
||||
def joint_limit_score(q, lower, upper):
|
||||
"""
|
||||
Score in [0, 1].
|
||||
1 means every joint is at center of its range.
|
||||
0 means at least one joint is at its limit.
|
||||
"""
|
||||
|
||||
q = np.asarray(q)
|
||||
mid = 0.5 * (lower + upper)
|
||||
half_range = 0.5 * (upper - lower)
|
||||
|
||||
per_joint_score = 1.0 - np.abs(q - mid) / half_range
|
||||
per_joint_score = np.clip(per_joint_score, 0.0, 1.0)
|
||||
|
||||
# Conservative: one bad joint makes the whole pose less comfortable.
|
||||
return float(np.min(per_joint_score))
|
||||
|
||||
|
||||
def joint_margin(q, lower, upper):
|
||||
"""
|
||||
Minimum normalized distance to joint limits.
|
||||
|
||||
0.15 means the closest joint is 15% away from its limit.
|
||||
"""
|
||||
q = np.asarray(q)
|
||||
margin_lower = (q - lower) / (upper - lower)
|
||||
margin_upper = (upper - q) / (upper - lower)
|
||||
margin = np.minimum(margin_lower, margin_upper)
|
||||
return float(np.min(margin))
|
||||
|
||||
|
||||
def q_to_cfg(q):
|
||||
"""
|
||||
Convert joint vector to urdfpy FK config dictionary.
|
||||
"""
|
||||
return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
|
||||
|
||||
|
||||
def fk_transform(robot, q):
|
||||
"""
|
||||
Forward kinematics from base_link to TCP_LINK.
|
||||
|
||||
Returns
|
||||
-------
|
||||
T : np.ndarray, shape (4, 4)
|
||||
"""
|
||||
cfg = q_to_cfg(q)
|
||||
fk = robot.link_fk(cfg=cfg)
|
||||
tcp_link = robot.link_map[TCP_LINK]
|
||||
return fk[tcp_link]
|
||||
|
||||
|
||||
def numerical_geometric_jacobian(robot, q, eps=1e-5):
|
||||
"""
|
||||
Numerical 6D geometric-like Jacobian, shape (6, 7).
|
||||
|
||||
Top 3 rows:
|
||||
linear velocity approximation
|
||||
|
||||
Bottom 3 rows:
|
||||
angular velocity approximation as rotation-vector difference
|
||||
|
||||
This is useful for manipulability and singularity checks.
|
||||
"""
|
||||
q = np.asarray(q, dtype=float)
|
||||
n = len(q)
|
||||
|
||||
J = np.zeros((6, n))
|
||||
|
||||
T0 = fk_transform(robot, q)
|
||||
p0 = T0[:3, 3]
|
||||
R0 = T0[:3, :3]
|
||||
|
||||
for i in range(n):
|
||||
q_plus = q.copy()
|
||||
q_minus = q.copy()
|
||||
|
||||
q_plus[i] += eps
|
||||
q_minus[i] -= eps
|
||||
|
||||
T_plus = fk_transform(robot, q_plus)
|
||||
T_minus = fk_transform(robot, q_minus)
|
||||
|
||||
p_plus = T_plus[:3, 3]
|
||||
p_minus = T_minus[:3, 3]
|
||||
|
||||
R_plus = T_plus[:3, :3]
|
||||
R_minus = T_minus[:3, :3]
|
||||
|
||||
# Linear part
|
||||
J[:3, i] = (p_plus - p_minus) / (2.0 * eps)
|
||||
|
||||
# Angular part
|
||||
# Relative rotation from minus to plus.
|
||||
dR = R_plus @ R_minus.T
|
||||
rotvec = R.from_matrix(dR).as_rotvec()
|
||||
J[3:, i] = rotvec / (2.0 * eps)
|
||||
|
||||
return J
|
||||
|
||||
|
||||
def manipulability_score_from_jacobian(J):
|
||||
"""
|
||||
Yoshikawa-style manipulability.
|
||||
|
||||
For a 6x7 Jacobian:
|
||||
w = sqrt(det(J J.T))
|
||||
|
||||
To improve numerical robustness, compute from singular values.
|
||||
"""
|
||||
singular_values = np.linalg.svd(J, compute_uv=False)
|
||||
|
||||
# Product of singular values.
|
||||
# For a 6x7 Jacobian, there are 6 singular values.
|
||||
w = float(np.prod(singular_values))
|
||||
|
||||
return w
|
||||
|
||||
|
||||
def condition_number_from_jacobian(J, min_sigma=1e-9):
|
||||
singular_values = np.linalg.svd(J, compute_uv=False)
|
||||
sigma_max = np.max(singular_values)
|
||||
sigma_min = np.min(singular_values)
|
||||
|
||||
if sigma_min < min_sigma:
|
||||
return np.inf
|
||||
|
||||
return float(sigma_max / sigma_min)
|
||||
|
||||
|
||||
def singularity_score(condition_number):
|
||||
"""
|
||||
Score in [0, 1].
|
||||
Higher is better.
|
||||
|
||||
condition_number = 1 is ideal.
|
||||
Very large means near singularity.
|
||||
"""
|
||||
if not np.isfinite(condition_number):
|
||||
return 0.0
|
||||
|
||||
return float(1.0 / condition_number)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 6. IK RESULT HANDLING
|
||||
# ============================================================
|
||||
|
||||
def normalize_ik_solutions(ik_result):
|
||||
"""
|
||||
Your IK returns:
|
||||
- None if failed
|
||||
- one list/array of 7 joint values if successful
|
||||
"""
|
||||
if ik_result is None:
|
||||
return []
|
||||
|
||||
q = np.asarray(ik_result, dtype=float).reshape(-1)
|
||||
|
||||
if q.shape[0] != 7:
|
||||
return []
|
||||
|
||||
return [q]
|
||||
|
||||
|
||||
def evaluate_single_solution(robot, q, lower, upper):
|
||||
"""
|
||||
Evaluate one IK solution.
|
||||
|
||||
Returns a dictionary with metrics.
|
||||
"""
|
||||
|
||||
if q.shape[0] != 7:
|
||||
return None
|
||||
|
||||
if not is_within_joint_limits(q, lower, upper):
|
||||
return None
|
||||
|
||||
jl_score = joint_limit_score(q, lower, upper)
|
||||
jl_margin = joint_margin(q, lower, upper)
|
||||
|
||||
J = numerical_geometric_jacobian(robot, q, eps=JACOBIAN_EPS)
|
||||
|
||||
manip = manipulability_score_from_jacobian(J)
|
||||
cond = condition_number_from_jacobian(J)
|
||||
sing_score = singularity_score(cond)
|
||||
|
||||
valid_by_thresholds = (
|
||||
jl_margin >= MIN_JOINT_MARGIN
|
||||
and cond <= MAX_CONDITION_NUMBER
|
||||
)
|
||||
|
||||
return {
|
||||
"q": q,
|
||||
"joint_limit_score": jl_score,
|
||||
"joint_margin": jl_margin,
|
||||
"manipulability": manip,
|
||||
"condition_number": cond,
|
||||
"singularity_score": sing_score,
|
||||
"valid_by_thresholds": valid_by_thresholds,
|
||||
}
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 7. MAIN WORKSPACE EVALUATION
|
||||
# ============================================================
|
||||
|
||||
def make_grid():
|
||||
xs = np.arange(X_RANGE[0], X_RANGE[1] + 1e-9, GRID_RESOLUTION)
|
||||
ys = np.arange(Y_RANGE[0], Y_RANGE[1] + 1e-9, GRID_RESOLUTION)
|
||||
zs = np.arange(Z_RANGE[0], Z_RANGE[1] + 1e-9, GRID_RESOLUTION)
|
||||
|
||||
points = []
|
||||
|
||||
for x in xs:
|
||||
for y in ys:
|
||||
for z in zs:
|
||||
points.append(np.array([x, y, z], dtype=float))
|
||||
|
||||
return points
|
||||
|
||||
|
||||
def evaluate_workspace():
|
||||
robot, lower, upper = load_robot_and_limits(URDF_PATH)
|
||||
orientations = make_task_orientations()
|
||||
grid_points = make_grid()
|
||||
|
||||
rows = []
|
||||
|
||||
# First pass stores raw manipulability.
|
||||
# Later we normalize manipulability by max observed value.
|
||||
all_valid_solution_metrics = []
|
||||
|
||||
print(f"Loaded robot from: {URDF_PATH}")
|
||||
print(f"Grid points: {len(grid_points)}")
|
||||
print(f"Orientations per point: {len(orientations)}")
|
||||
print("Evaluating IK reachability and raw metrics...")
|
||||
|
||||
for point in tqdm(grid_points):
|
||||
point_solution_metrics = []
|
||||
|
||||
attempted = 0
|
||||
ik_success_count = 0
|
||||
|
||||
|
||||
for rpy in orientations:
|
||||
attempted += 1
|
||||
|
||||
|
||||
ik_result = solve_ik(point, rpy)
|
||||
|
||||
# print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
|
||||
candidate_solutions = normalize_ik_solutions(ik_result)
|
||||
|
||||
if len(candidate_solutions) == 0:
|
||||
continue
|
||||
|
||||
evaluated_solutions = []
|
||||
|
||||
for q in candidate_solutions:
|
||||
# pose = robot_kine_qp.forward_kinematics(joint_angles=q, tool=tool_name)
|
||||
# print(f'the fk of q is {pose}\n')
|
||||
metrics = evaluate_single_solution(robot, q, lower, upper)
|
||||
# print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}')
|
||||
if metrics is not None:
|
||||
evaluated_solutions.append(metrics)
|
||||
|
||||
if len(evaluated_solutions) == 0:
|
||||
continue
|
||||
|
||||
ik_success_count += 1
|
||||
|
||||
# Use the best solution for this pose.
|
||||
# At this stage, manipulability is not normalized,
|
||||
# so use joint score + singularity score as temporary ranking.
|
||||
best = max(
|
||||
evaluated_solutions,
|
||||
key=lambda m: 0.6 * m["joint_limit_score"] + 0.4 * m["singularity_score"]
|
||||
)
|
||||
|
||||
point_solution_metrics.append(best)
|
||||
all_valid_solution_metrics.append(best)
|
||||
print(f'this position+all orientations, the point_solution_metrics = {point_solution_metrics}')
|
||||
ik_success_rate = ik_success_count / attempted if attempted > 0 else 0.0
|
||||
|
||||
if len(point_solution_metrics) == 0:
|
||||
rows.append({
|
||||
"x": point[0],
|
||||
"y": point[1],
|
||||
"z": point[2],
|
||||
"ik_success_rate": 0.0,
|
||||
"joint_limit_score": 0.0,
|
||||
"joint_margin": 0.0,
|
||||
"manipulability": 0.0,
|
||||
"manipulability_score": 0.0,
|
||||
"condition_number": np.inf,
|
||||
"singularity_score": 0.0,
|
||||
"comfort_score": 0.0,
|
||||
"comfortable": False,
|
||||
"reachable": False,
|
||||
})
|
||||
else:
|
||||
# Average over task orientations.
|
||||
rows.append({
|
||||
"x": point[0],
|
||||
"y": point[1],
|
||||
"z": point[2],
|
||||
"ik_success_rate": ik_success_rate,
|
||||
"joint_limit_score": np.mean([m["joint_limit_score"] for m in point_solution_metrics]),
|
||||
"joint_margin": np.mean([m["joint_margin"] for m in point_solution_metrics]),
|
||||
"manipulability": np.mean([m["manipulability"] for m in point_solution_metrics]),
|
||||
"manipulability_score": 0.0, # filled later
|
||||
"condition_number": np.mean([m["condition_number"] for m in point_solution_metrics]),
|
||||
"singularity_score": np.mean([m["singularity_score"] for m in point_solution_metrics]),
|
||||
"comfort_score": 0.0, # filled later
|
||||
"comfortable": False,
|
||||
"reachable": True,
|
||||
})
|
||||
|
||||
df = pd.DataFrame(rows)
|
||||
|
||||
# Normalize manipulability by maximum observed value.
|
||||
max_manip = df["manipulability"].replace([np.inf, -np.inf], np.nan).max()
|
||||
|
||||
if max_manip is None or not np.isfinite(max_manip) or max_manip <= 0:
|
||||
max_manip = 1.0
|
||||
|
||||
df["manipulability_score"] = df["manipulability"] / max_manip
|
||||
df["manipulability_score"] = df["manipulability_score"].clip(0.0, 1.0)
|
||||
|
||||
# Final comfort score.
|
||||
df["comfort_score"] = (
|
||||
WEIGHT_IK_SUCCESS * df["ik_success_rate"]
|
||||
+ WEIGHT_JOINT_LIMIT * df["joint_limit_score"]
|
||||
+ WEIGHT_MANIPULABILITY * df["manipulability_score"]
|
||||
+ WEIGHT_SINGULARITY * df["singularity_score"]
|
||||
)
|
||||
|
||||
# Comfortable binary classification.
|
||||
df["comfortable"] = (
|
||||
(df["reachable"] == True)
|
||||
& (df["ik_success_rate"] >= 0.80)
|
||||
& (df["joint_margin"] >= MIN_JOINT_MARGIN)
|
||||
& (df["condition_number"] <= MAX_CONDITION_NUMBER)
|
||||
& (df["manipulability_score"] >= MIN_MANIPULABILITY_RATIO)
|
||||
)
|
||||
|
||||
return df
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 8. PLOTTING
|
||||
# ============================================================
|
||||
|
||||
def plot_workspace(df):
|
||||
"""
|
||||
3D scatter plot:
|
||||
gray/low = low comfort
|
||||
brighter = higher comfort
|
||||
"""
|
||||
|
||||
reachable = df[df["reachable"] == True]
|
||||
|
||||
if len(reachable) == 0:
|
||||
print("No reachable points found. Check your IK function.")
|
||||
return
|
||||
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, projection="3d")
|
||||
|
||||
sc = ax.scatter(
|
||||
reachable["x"],
|
||||
reachable["y"],
|
||||
reachable["z"],
|
||||
c=reachable["comfort_score"],
|
||||
s=12,
|
||||
alpha=0.8,
|
||||
)
|
||||
|
||||
ax.set_title("RM75-B Comfortable Workspace")
|
||||
ax.set_xlabel("X [m]")
|
||||
ax.set_ylabel("Y [m]")
|
||||
ax.set_zlabel("Z [m]")
|
||||
|
||||
fig.colorbar(sc, ax=ax, label="Comfort score")
|
||||
plt.show()
|
||||
|
||||
|
||||
def plot_comfortable_only(df):
|
||||
comfortable = df[df["comfortable"] == True]
|
||||
|
||||
if len(comfortable) == 0:
|
||||
print("No comfortable points found under current thresholds.")
|
||||
return
|
||||
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, projection="3d")
|
||||
|
||||
ax.scatter(
|
||||
comfortable["x"],
|
||||
comfortable["y"],
|
||||
comfortable["z"],
|
||||
c=comfortable["comfort_score"],
|
||||
s=16,
|
||||
alpha=0.9,
|
||||
)
|
||||
|
||||
ax.set_title("RM75-B Comfortable Region Only")
|
||||
ax.set_xlabel("X [m]")
|
||||
ax.set_ylabel("Y [m]")
|
||||
ax.set_zlabel("Z [m]")
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 9. ENTRY POINT
|
||||
# ============================================================
|
||||
|
||||
if __name__ == "__main__":
|
||||
df = evaluate_workspace()
|
||||
|
||||
df.to_csv(output_csv, index=False)
|
||||
|
||||
print(f"\nSaved result to: {output_csv}")
|
||||
|
||||
print("\nSummary:")
|
||||
print(f"Total grid points: {len(df)}")
|
||||
print(f"Reachable points: {df['reachable'].sum()}")
|
||||
print(f"Comfortable points: {df['comfortable'].sum()}")
|
||||
|
||||
if df["reachable"].sum() > 0:
|
||||
print(f"Max comfort score: {df['comfort_score'].max():.3f}")
|
||||
print(f"Mean comfort score: {df[df['reachable']]['comfort_score'].mean():.3f}")
|
||||
|
||||
plot_workspace(df)
|
||||
plot_comfortable_only(df)
|
||||
@@ -1,18 +1,81 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import os
|
||||
|
||||
|
||||
'''
|
||||
Robotic arm kinematics solver for realman-75
|
||||
|
||||
Files:
|
||||
rm75_kinematics.py
|
||||
|
||||
How to use:
|
||||
Example in test1.py:
|
||||
python test1.py
|
||||
'''
|
||||
import pinocchio as pin
|
||||
from Robotic_Arm.rm_robot_interface import *
|
||||
import numpy as np
|
||||
import osqp
|
||||
from scipy import sparse
|
||||
from math import radians, degrees, pi, cos, sin
|
||||
import time
|
||||
import threading
|
||||
import math
|
||||
|
||||
|
||||
class rm75_kinematics():
|
||||
def __init__(self,urdf_path="", mesh_dir="", tcps=None,tools_in_ee=None,min_j=0.0, max_j=0.0):
|
||||
self.robot_kine_qp = rm75_kine_qp(urdf_path=urdf_path, mesh_dir=mesh_dir)
|
||||
self.robot_kine_qp.add_tool_frames(tools_in_ee)
|
||||
self.robot_kine_qp.cfg_j_limit(min_j=min_j, max_j=max_j, rad_flag=True)
|
||||
|
||||
# ---------- rm75 official algorithm -----------
|
||||
self.robot_kine_rm = rm75_kine_api()
|
||||
self.robot_kine_rm.add_tool_frames(tools_in_ee)
|
||||
self.robot_kine_rm.cfg_j_limit(min_j=min_j, max_j=max_j, rad_flag=True)
|
||||
|
||||
self.ik_sts = False
|
||||
self.joint_solved = [0.0] * 7
|
||||
|
||||
def get_ik_result(self, target_position, target_rpy, initial_guess, tool):
|
||||
'''
|
||||
Try both the RM official solver and the QP solver; on success, store the result.
|
||||
return: True if either solver succeeded, joint_solved in rad.
|
||||
'''
|
||||
ret_rm, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool)
|
||||
if ret_rm != 0:
|
||||
ret_rm, q_out = self.robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300)
|
||||
|
||||
if ret_rm == 0:
|
||||
self.joint_solved = q_out
|
||||
self.ik_sts = True
|
||||
else:
|
||||
self.ik_sts = False
|
||||
|
||||
return self.ik_sts, self.joint_solved
|
||||
|
||||
def get_fk_result(self, joint_angles, tool):
|
||||
'''
|
||||
Get the forward kinematics result for given joint angles and tool.
|
||||
:param joint_angles: list of joint values, in rad
|
||||
:param return: [x,y,z,rx,ry,rz], m & rad
|
||||
return: [x, y, z, rx, ry, rz] in meters and radians.
|
||||
'''
|
||||
fk_result = self.robot_kine_rm.forward_kinematics(joint_angles, flag=1, tool=tool)
|
||||
return fk_result
|
||||
|
||||
def get_self_collision(self, joint_angles):
|
||||
'''
|
||||
Check for self-collision given joint angles.
|
||||
:param joint_angles: list of joint values, in rad
|
||||
:return: True if self-collision is detected, False otherwise.
|
||||
'''
|
||||
collision_detected = self.robot_kine_qp.collision_detect(joint_angles)
|
||||
return collision_detected
|
||||
|
||||
|
||||
|
||||
class KinematicsSolver():
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
class rm75_kine_qp():
|
||||
def __init__(self, urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75", tcps=None):
|
||||
"""
|
||||
for realman 75b
|
||||
@@ -20,7 +83,7 @@ class KinematicsSolver():
|
||||
unit: m, rad
|
||||
"""
|
||||
print(f' ------------ the qp based kinematic initialising -----------')
|
||||
self.model, self.collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
|
||||
self.model = pin.buildModelFromUrdf(urdf_path)
|
||||
|
||||
self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir)
|
||||
self.geom_model.addAllCollisionPairs()
|
||||
@@ -179,7 +242,7 @@ class KinematicsSolver():
|
||||
if max_j is None:
|
||||
max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
|
||||
|
||||
k = 1.0 if rad_flag is True else 1.0 / 180 * pi
|
||||
k = 1.0 if rad_flag is True else 1.0 / 180 * math.pi
|
||||
|
||||
for i in range(7):
|
||||
self.model.lowerPositionLimit[i] = min_j[i] * k
|
||||
@@ -992,3 +1055,182 @@ class KinematicsSolver():
|
||||
all_active_joint_ids.append(active_joint_ids)
|
||||
|
||||
return all_active_joint_ids
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
class rm75_kine_api():
|
||||
def __init__(self):
|
||||
# ---------- rm75 official algorithm -----------
|
||||
print(f'------- the realman official kinematic initialising -------')
|
||||
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm
|
||||
force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
|
||||
# Initialize the robotic arm model and sensor type in the algorithm
|
||||
self.robot_kine_rm = Algo(arm_model, force_type)
|
||||
|
||||
self.cfg_j_limit()
|
||||
|
||||
self.work_frames = {
|
||||
'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
|
||||
}
|
||||
|
||||
self.tool_name = "no_tool"
|
||||
self.work_name = "work"
|
||||
|
||||
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
|
||||
if max_j is None:
|
||||
max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
|
||||
if min_j is None:
|
||||
min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
|
||||
|
||||
max_j = np.array(max_j)
|
||||
min_j = np.array(min_j)
|
||||
if rad_flag:
|
||||
self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
|
||||
self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
|
||||
else:
|
||||
self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
|
||||
self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
|
||||
|
||||
def cfg_work_frame(self , frame_name):
|
||||
self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
|
||||
|
||||
def get_work_frame(self):
|
||||
return self.robot_kine_rm.rm_algo_get_curr_workframe()
|
||||
|
||||
def cfg_tool_frame(self, frame_name ):
|
||||
self.robot_kine_rm.rm_algo_set_toolframe(self.tool_frames[frame_name])
|
||||
|
||||
def get_tool_frame(self):
|
||||
return self.robot_kine_rm.rm_algo_get_curr_toolframe()
|
||||
|
||||
def quaternion_to_euler(self, q):
|
||||
"""
|
||||
Convert quaternion to Euler angles (roll, pitch, yaw)
|
||||
|
||||
Args:
|
||||
qx, qy, qz, qw: quaternion components
|
||||
|
||||
Returns:
|
||||
tuple: (roll, pitch, yaw) in radians
|
||||
"""
|
||||
# Roll (x-axis rotation)
|
||||
sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
|
||||
cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
|
||||
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# Pitch (y-axis rotation)
|
||||
sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
|
||||
if abs(sinp) >= 1:
|
||||
pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
|
||||
else:
|
||||
pitch = np.arcsin(sinp)
|
||||
|
||||
# Yaw (z-axis rotation)
|
||||
siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
|
||||
cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
|
||||
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return [roll, pitch, yaw]
|
||||
|
||||
def add_tool_frames(self, dict_frames):
|
||||
self.tool_frames = {}
|
||||
for tool_name in dict_frames:
|
||||
tool_attr = dict_frames[tool_name]
|
||||
position = tool_attr[0][0:3]
|
||||
rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
|
||||
f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
|
||||
|
||||
self.tool_frames.update({tool_name:f})
|
||||
|
||||
def forward_kinematics(self, joint_angles, flag = 1 , tool="omnipic", work="work"):
|
||||
'''
|
||||
:param joint_angles: list of joint values, in rad
|
||||
:param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
|
||||
:param return: [x,y,z,rx,ry,rz], m & rad
|
||||
'''
|
||||
if tool != self.tool_name:
|
||||
self.tool_name = tool
|
||||
self.cfg_tool_frame(tool)
|
||||
if work != self.work_name:
|
||||
self.work_name = work
|
||||
self.cfg_work_frame(work)
|
||||
return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[float(q_s)*180.0/math.pi for q_s in joint_angles] , flag=flag)
|
||||
|
||||
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work", step_arm_angle = 15.0):
|
||||
'''
|
||||
:param target_position: list of position values, m
|
||||
:param target_rpy: list of rpy values, rad
|
||||
:param initial_guess: initial guess of angles, rad
|
||||
:param tool: tool name, refer to self.tool_frames
|
||||
:param work: work name, refer to self.work_frames
|
||||
|
||||
return ret: state of ik calculation, 0:success, -2: out of workspace
|
||||
[q_]: the ik calculated angles for joints, rad
|
||||
'''
|
||||
if tool != self.tool_name:
|
||||
self.tool_name = tool
|
||||
self.cfg_tool_frame(tool)
|
||||
if work != self.work_name:
|
||||
self.work_name = work
|
||||
self.cfg_work_frame(work)
|
||||
|
||||
target = list(target_position) + list(target_rpy)
|
||||
|
||||
if initial_guess is not None:
|
||||
q_ref = [ 180/math.pi * ig for ig in initial_guess ]
|
||||
else:
|
||||
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
|
||||
ret, phi0 = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
|
||||
params = rm_inverse_kinematics_params_t(q_ref, target, 1)
|
||||
|
||||
offsets = [0.0]
|
||||
arm_angle = step_arm_angle
|
||||
while arm_angle <= 180.0:
|
||||
offsets += [arm_angle, -arm_angle]
|
||||
arm_angle += step_arm_angle
|
||||
|
||||
best_ret, best_q_out, best_dis = -1, None, None
|
||||
for offset in offsets:
|
||||
phi = ((phi0 + offset + 180.0) % 360.0) - 180.0
|
||||
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||
if int(ret) != 0:
|
||||
if best_q_out is None:
|
||||
best_ret, best_q_out = ret, q_out
|
||||
continue
|
||||
|
||||
p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
|
||||
pose_dis = cal_pose_deviation(p_fk, target)
|
||||
if pose_dis < 0.01:
|
||||
# success in ik calculation
|
||||
return ret, [q / 180 * math.pi for q in q_out]
|
||||
|
||||
if best_dis is None or pose_dis < best_dis:
|
||||
best_ret, best_q_out, best_dis = -10, q_out, pose_dis
|
||||
|
||||
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||
pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
|
||||
pose_dis = cal_pose_deviation(pose_fk, target)
|
||||
|
||||
# print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
|
||||
#
|
||||
# print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
|
||||
# print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
|
||||
if int(ret) < 0:
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
elif pose_dis < 0.01:
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
else:
|
||||
return -10, [ q/180*math.pi for q in q_out]
|
||||
|
||||
def cal_pose_deviation(pose1, pose2):
|
||||
d_fk_p1 = np.array(pose1) - np.array(pose2)
|
||||
for j in [3, 4, 5]:
|
||||
while d_fk_p1[j] > math.pi:
|
||||
d_fk_p1[j] -= 2 * math.pi
|
||||
while d_fk_p1[j] < -math.pi:
|
||||
d_fk_p1[j] += 2 * math.pi
|
||||
d_fk = np.linalg.norm(d_fk_p1)
|
||||
return d_fk
|
||||
@@ -0,0 +1,48 @@
|
||||
|
||||
|
||||
# conda activate coppeliasim
|
||||
# env fix, in terminal: fix_robotics_env.sh
|
||||
|
||||
from rm75_kinematics import rm75_kinematics
|
||||
|
||||
from math import pi
|
||||
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([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_kine = rm75_kinematics(urdf_path='urdf_rm75/RM75-SCI.urdf', mesh_dir='urdf_rm75',
|
||||
tcps=["scissor_tcp", "camera_tcp"], tools_in_ee=tools_in_ee, min_j=lb, max_j=ub)
|
||||
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool=tool_name)
|
||||
|
||||
self_collision_sts = robot_kine.get_self_collision(q)
|
||||
|
||||
p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
|
||||
|
||||
print(f'self_collision_sts: {self_collision_sts}')
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
# conda activate coppeliasim
|
||||
# env fix, in terminal: fix_robotics_env.sh
|
||||
|
||||
from rm75_kinematics import rm75_kinematics
|
||||
|
||||
from math import pi
|
||||
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([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_kine = rm75_kinematics(urdf_path='urdf_rm75/RM75-SCI.urdf', mesh_dir='urdf_rm75',
|
||||
tcps=["scissor_tcp", "camera_tcp"], tools_in_ee=tools_in_ee, min_j=lb, max_j=ub)
|
||||
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool=tool_name)
|
||||
|
||||
self_collision_sts = robot_kine.get_self_collision(q)
|
||||
|
||||
p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
|
||||
|
||||
print(f'self_collision_sts: {self_collision_sts}')
|
||||
|
||||
import numpy as np
|
||||
|
||||
ik_suc = 0
|
||||
|
||||
for i in range(100):
|
||||
joint_rand = np.random.uniform(ub, lb)
|
||||
|
||||
p_t = robot_kine.get_fk_result(joint_angles=joint_rand.tolist(), tool=tool_name)
|
||||
|
||||
joint_rand_init = np.random.uniform(ub, lb)
|
||||
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=p_t[0:3], target_rpy=p_t[3:6],
|
||||
initial_guess=joint_rand_init, tool=tool_name)
|
||||
|
||||
|
||||
|
||||
|
||||
p_fk = robot_kine.get_fk_result(joint_angles=q, tool=tool_name)
|
||||
|
||||
d_p_ik = cal_pose_deviation(pose1=p_t, pose2=p_fk)
|
||||
|
||||
coll_sts = robot_kine.get_self_collision(joint_angles=q)
|
||||
if ret_ik == True:
|
||||
|
||||
print(f'\n---- success, in the ik, j_t = {joint_rand}, q = {q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
|
||||
|
||||
ik_suc += 1
|
||||
else:
|
||||
print(f'\n**** ik failed, in the ik, j_t = {joint_rand}, q={q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
|
||||
|
||||
|
||||
print(f'ik_suc: {ik_suc}')
|
||||
|
||||
|
||||
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()
|
||||
Reference in New Issue
Block a user