# 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 solve_sum = 0 for i in range(10): 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 robot_mjk.send_command(joint_solution) robot_mjk.wait_until_reached() robot_mjk.print_state() 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}') if success or ret == 0: solve_sum += 1 print(f'result is {result}') print(f'solve_sum is {solve_sum}') 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()