forked from ZhengLiu-cart/IK_qp
196 lines
7.2 KiB
Python
196 lines
7.2 KiB
Python
|
|
|
|
# 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
|
|
|
|
def demo_position_control():
|
|
"""Demonstrate pure position control"""
|
|
|
|
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
|
|
|
|
|
|
|
|
# Create controller
|
|
robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
|
|
robot_mjk.start()
|
|
|
|
print("\n[Test 1] Move joint 1 to 45 degrees")
|
|
robot_mjk.send_command([0.785, 0, 0, 0, 0, 0, 0])
|
|
robot_mjk.wait_until_reached()
|
|
robot_mjk.print_state()
|
|
print("\n[Test 4] Return home\n")
|
|
robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
|
|
robot_mjk.wait_until_reached()
|
|
robot_mjk.print_state()
|
|
|
|
|
|
|
|
|
|
#---------------------------------------------------------------------------
|
|
|
|
|
|
joints = [10, 20, -30, -40, 50, 60, 70]
|
|
joints_rad = [radians(j) for j in joints] #radians(joints)
|
|
# target_position = [0.3, 0.2, 0.4]
|
|
# target_rpy = [0.0, 0.0, 3.14*0.25]
|
|
target_position = [0.17892041, 0.25274317, 0.83107248]
|
|
target_rpy = [0.78576018, 0.67554633, 1.86302226]
|
|
target_p = target_position + target_rpy
|
|
# target_p_rad = [radians(pos) for pos in target_position] + target_rpy
|
|
initial_guess = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] # [0.0, 20, -30, -40, 50, 60, 91] #
|
|
initial_guess_rad = [ radians(j) for j in initial_guess ]
|
|
tool_name = "scissor"
|
|
|
|
|
|
robot_kine_qp = kine_qp()
|
|
print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}')
|
|
|
|
time0 = time.time()
|
|
for i in range(100):
|
|
joint_solution, success, error, ite = robot_kine_qp.inverse_kinematics(
|
|
target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
|
|
max_iter=500, debug=False, tool=tool_name )
|
|
time1 = time.time()
|
|
print(f'used time by qp is {time1 - time0}')
|
|
|
|
if success:
|
|
print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}, iteration: {ite}\n')
|
|
print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
|
|
else:
|
|
print(f'solution: {joint_solution} success flag {success}, error {error}, iteration: {ite}\n')
|
|
|
|
|
|
|
|
# ---------- rm75 official algorithm -----------
|
|
robot_kine_rm = kine_rm()
|
|
print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}')
|
|
time2 = time.time()
|
|
for i in range(100):
|
|
ret, q = robot_kine_rm.inverse_kinematics(target_position=target_p[0:3], target_rpy=target_p[3:6],initial_guess=initial_guess, tool=tool_name)
|
|
time3 = time.time()
|
|
print(f'used time by rm is {time3 - time2}')
|
|
|
|
print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}')
|
|
if ret == 0:
|
|
print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ')
|
|
|
|
|
|
|
|
# -------------- for comparison ----------------
|
|
print(f'in the comparison part')
|
|
|
|
if True:
|
|
|
|
# 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])
|
|
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
|
|
lb = -ub
|
|
|
|
# lower_limits = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
|
|
# upper_limits = np.array([ 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159 ])
|
|
|
|
for i in range(7):
|
|
robot_kine_qp.model.lowerPositionLimit[i] = lb[i]/180*pi
|
|
robot_kine_qp.model.upperPositionLimit[i] = ub[i]/180*pi
|
|
|
|
robot_kine_rm.robot_kine_rm.rm_algo_set_joint_max_limit(ub)
|
|
robot_kine_rm.robot_kine_rm.rm_algo_set_joint_min_limit(lb)
|
|
|
|
result = np.array([[0,0],[0,0]], dtype=np.int32) # qp_fk, qp_ik, rm_fk, rm_ik
|
|
|
|
|
|
|
|
for i in range(1000):
|
|
print(f'\n-------------- in i = {i} ----------------')
|
|
joint_rand = np.random.uniform(ub/180*pi, lb/180*pi)
|
|
print(f'the predefined joints are {joint_rand}')
|
|
|
|
# -------------- fk ------------------
|
|
fk_qp1 = robot_kine_qp.forward_kinematics(joint_rand.tolist(), tool=tool_name)
|
|
fk_qp_p1 = np.concatenate([fk_qp1['position'], fk_qp1['rpy']], axis=0)
|
|
|
|
fk_rm_p1 = robot_kine_rm.forward_kinematics(q=(joint_rand*180/pi).tolist(), tool=tool_name)
|
|
|
|
d_fk_p1 = np.array(fk_rm_p1) - np.array(fk_qp_p1)
|
|
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)
|
|
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/180*pi, lb/180*pi)
|
|
print(f'the guess is {joint_rand_init}')
|
|
joint_solution, success, error, ite = robot_kine_qp.inverse_kinematics(
|
|
t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init,
|
|
max_iter=500, debug=False, tool=tool_name)
|
|
print(f'joint_solution = {joint_solution}, success = {success}, error = {error}, ite = {ite}')
|
|
|
|
if success:
|
|
fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)
|
|
fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0)
|
|
d_p_ik = np.linalg.norm( np.array(fk_qp_p2) - np.array(t_p) )
|
|
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
|
|
else:
|
|
fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)
|
|
fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0)
|
|
d_p_ik = np.linalg.norm(np.array(fk_qp_p2) - np.array(t_p))
|
|
print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
|
|
|
|
ret, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6],
|
|
initial_guess=(joint_rand_init*180/pi).tolist(), tool=tool_name)
|
|
if ret == 0:
|
|
fk_rm_p2 = robot_kine_rm.forward_kinematics(q=q, tool=tool_name)
|
|
d_p_ik = np.linalg.norm(np.array(fk_rm_p2) - np.array(t_p) )
|
|
print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik}')
|
|
if d_p_ik < 0.01:
|
|
result[1][1] += 1
|
|
else:
|
|
print(f'== fail in the rm ik, ret = {ret}y, q = {q}')
|
|
|
|
print(f'result is {result}')
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
print(f'\nDone\n')
|
|
|
|
|
|
# try:
|
|
# while robot_mjk.viewer and robot_mjk.viewer.is_running():
|
|
# time.sleep(0.1)
|
|
# except KeyboardInterrupt:
|
|
# pass
|
|
robot_mjk.stop()
|
|
|
|
|
|
|
|
def main():
|
|
|
|
demo_position_control()
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|