124 lines
3.7 KiB
Python
124 lines
3.7 KiB
Python
from casadi import print_operator
|
|
|
|
# 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()
|
|
time.sleep(1)
|
|
|
|
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()
|
|
time.sleep(0.5)
|
|
|
|
# print("\n[Test 2] Move joint 2 to -30 degrees")
|
|
# robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
|
|
# robot_mjk.wait_until_reached()
|
|
# robot_mjk.print_state()
|
|
# time.sleep(0.5)
|
|
#
|
|
# print("\n[Test 3] Move multiple joints simultaneously")
|
|
# robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
|
|
# robot_mjk.wait_until_reached()
|
|
# robot_mjk.print_state()
|
|
# time.sleep(0.5)
|
|
|
|
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)}')
|
|
|
|
joint_solution, success, error = 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 )
|
|
|
|
if success:
|
|
print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\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}\n')
|
|
print(
|
|
f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)}\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)}')
|
|
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)
|
|
|
|
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)} ')
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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()
|