forked from ZhengLiu-cart/IK_qp
91 lines
2.9 KiB
Python
91 lines
2.9 KiB
Python
|
|
|
|
# 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()
|