forked from ZhengLiu-cart/IK_qp
49 lines
1.7 KiB
Python
49 lines
1.7 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}')
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|