diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index ebf9717..2dbe49d 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -8,6 +8,10 @@ from rm75_kine_rm import rm75_kine_api as kine_rm from rm75_mjc import MuJoCoPositionController from Robotic_Arm.rm_robot_interface import * +import os +cwd = os.getcwd() + + import time from math import radians, degrees, pi, cos, sin import numpy as np @@ -35,11 +39,11 @@ def main(): """Demonstrate pure position control""" # Create controller - robot_mjk = MuJoCoPositionController() + robot_mjk = MuJoCoPositionController(urdf_path="./urdf_rm75/RM75-SCI.urdf") # ----------- rm75 qp based kine ------------ - robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75') + robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-B.urdf', mesh_dir='./urdf_rm75') robot_kine_qp.add_tool_frames(tools_in_ee) robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True) @@ -100,8 +104,8 @@ def main(): if d_p_ik < 0.01: result[0][1] += 1 - while 1: - time.sleep(1) + # while 1: + # time.sleep(1) robot_mjk.send_command(q) robot_mjk.wait_until_reached()