diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index 6057566..71da75f 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -12,76 +12,18 @@ import time from math import radians, degrees, pi, cos, sin import numpy as np -def demo_position_control(): +def main(): """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() + robot_mjk = MuJoCoPositionController() - 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" - - + # ----------- rm75 qp based kine ------------ 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)} ') @@ -92,110 +34,80 @@ def demo_position_control(): # 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]) + + ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi 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 ]) + robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True) - 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.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True) - 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 + result = np.array([[0,0],[0,0]], dtype=np.int32) # to collect ik result 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) + joint_rand = np.random.uniform(ub, lb) 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_qp_p1 = robot_kine_qp.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name) - fk_rm_p1 = robot_kine_rm.forward_kinematics(q=(joint_rand*180/pi).tolist(), tool=tool_name) + fk_rm_p1 = robot_kine_rm.forward_kinematics(joint_angles=joint_rand.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) + d_fk = cal_pose_deviation(pose1=fk_rm_p1, pose2=fk_qp_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) + joint_rand_init = np.random.uniform(ub, lb) 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) ) + ret_qp, q = robot_kine_qp.inverse_kinematics( target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name) + + if ret_qp == 0: + fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name) + d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2) 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.send_command(q) 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}') + fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name) + d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2) + print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}') - 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}') + ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name) + if ret_rm == 0: + fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name) + d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2) + print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}') 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'== fail in the rm ik, ret = {ret_rm}, q = {q}') - if success or ret == 0: + if ret_qp == 0 or ret_rm == 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() +def cal_pose_deviation(pose1, pose2): + d_fk_p1 = np.array(pose1) - np.array(pose2) + 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) + return d_fk diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index 6057b17..6e78fd6 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/kine_ctrl/rm75_kine_qp.py @@ -12,7 +12,7 @@ import time class KinematicsSolver(): - def __init__(self,urdf_path="/home/zl/Downloads/urdf_rm75/RM75-B.urdf", mesh_dir="/home/zl/Downloads/meshes"): + def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"): """ for realman 75b Initialize robotic arm kinematics using Pinocchio (ROS2 version). @@ -90,18 +90,7 @@ class KinematicsSolver(): self.data = self.model.createData() - # Joint limits (radians) - expanded for better reachability - self.lower_limits = np.array([ - -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159 - ]) - self.upper_limits = np.array([ - 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159 - ]) - - # Set joint limits in the model - for i in range(7): - self.model.lowerPositionLimit[i] = self.lower_limits[i] - self.model.upperPositionLimit[i] = self.upper_limits[i] + self.cfg_j_limit() # ---------- for reused qp_solver ------------------ self.nv = 7 @@ -129,6 +118,19 @@ class KinematicsSolver(): self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4]) + def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True): + if min_j is None: + min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159] + if max_j is None: + max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159] + if rad_flag: + for i in range(7): + self.model.lowerPositionLimit[i] = min_j[i] + self.model.upperPositionLimit[i] = max_j[i] + else: + for i in range(7): + self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi + self.model.upperPositionLimit[i] = max_j[i] / 180 * pi def forward_kinematics(self, joint_angles, tool="ee"): """ @@ -167,15 +169,16 @@ class KinematicsSolver(): rpy = pin.rpy.matrixToRpy(rotation) # Compute quaternion - quat = pin.Quaternion(rotation) - - return { - 'position': position, - # 'rotation': rotation, - 'rpy': rpy, - 'quaternion': [quat.x, quat.y, quat.z, quat.w], - # 'transform': frame_transform - } + # quat = pin.Quaternion(rotation) + pose = np.concatenate([position, rpy], axis=0) + return pose + # return { + # 'position': position, + # # 'rotation': rotation, + # 'rpy': rpy, + # 'quaternion': [quat.x, quat.y, quat.z, quat.w], + # # 'transform': frame_transform + # } def inverse_kinematics(self, target_position, target_rpy=None, target_quat=None, initial_guess=None, @@ -360,9 +363,11 @@ class KinematicsSolver(): iter_count += 1 if best_solution is not None: - return best_solution, True, best_error, iter_count + # return best_solution, True, best_error, iter_count + return 0, best_solution else: - return q[:7].copy(), False, error_norm, iter_count + # return q[:7].copy(), False, error_norm, iter_count + return -1, q[:7].copy() # def invese_kinematics_velocity(self, target_position, target_rpy=None, # target_quat=None, initial_guess=None, tool="ee"): diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py index 5bacf26..218590c 100644 --- a/kine_ctrl/rm75_kine_rm.py +++ b/kine_ctrl/rm75_kine_rm.py @@ -12,6 +12,8 @@ class rm75_kine_api(): # Initialize the robotic arm model and sensor type in the algorithm self.robot_kine_rm = Algo(arm_model, force_type) + self.cfg_j_limit() + self.tool_frames = { 'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0), 'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.144, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72), @@ -24,15 +26,18 @@ class rm75_kine_api(): self.tool_name = "ee" self.work_name = "work" - def cfg_limit(self): - joint_max_limit = np.array([ - 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159 - ]) * 180 / math.pi - self.robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist()) - joint_min_limit = np.array([ - -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 - ]) * 180 / math.pi - self.robot_kine_rm.rm_algo_set_joint_min_limit(joint_min_limit.tolist()) + def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True): + if max_j is None: + max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159]) + if min_j is None: + min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ]) + + if rad_flag: + self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist()) + self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist()) + else: + self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist()) + self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist()) def cfg_work_frame(self , frame_name): self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name]) @@ -46,10 +51,11 @@ class rm75_kine_api(): def get_tool_frame(self): return self.robot_kine_rm.rm_algo_get_curr_toolframe() - def forward_kinematics(self, q, flag = 1 , tool="ee", work="work"): + def forward_kinematics(self, joint_angles, flag = 1 , tool="ee", work="work"): ''' - :param q: list of joint values, in degree - flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz] + :param joint_angles: list of joint values, in rad + :param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz] + :param return: [x,y,z,rx,ry,rz], m & rad ''' if tool != self.tool_name: self.tool_name = tool @@ -58,9 +64,19 @@ class rm75_kine_api(): self.work_name = work self.cfg_work_frame(work) - return self.robot_kine_rm.rm_algo_forward_kinematics(joint=q, flag=flag) + return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[q_s*180/math.pi for q_s in joint_angles] , flag=flag) def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="ee", work="work"): + ''' + :param target_position: list of position values, m + :param target_rpy: list of rpy values, rad + :param initial_guess: initial guess of angles, rad + :param tool: tool name, refer to self.tool_frames + :param work: work name, refer to self.work_frames + + return ret: state of ik calculation, 0:success, -2: out of workspace + [q_]: the ik calculated angles for joints, rad + ''' if tool != self.tool_name: self.tool_name = tool self.cfg_tool_frame(tool) @@ -71,11 +87,11 @@ class rm75_kine_api(): target = target_position + target_rpy if initial_guess is not None: - q_ref = initial_guess + q_ref = [ 180/math.pi * ig for ig in initial_guess ] else: q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) params = rm_inverse_kinematics_params_t(q_ref, target, 1) ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) - return ret, q_out \ No newline at end of file + return ret, [ q/180*math.pi for q in q_out] \ No newline at end of file diff --git a/kine_ctrl/rm75_mjc.py b/kine_ctrl/rm75_mjc.py index 77598e8..70ac134 100644 --- a/kine_ctrl/rm75_mjc.py +++ b/kine_ctrl/rm75_mjc.py @@ -18,7 +18,7 @@ class MuJoCoPositionController: No velocity commands, no forces - completely stable """ - def __init__(self, urdf_path, smoothness=0.2, enable_viewer=True): + def __init__(self, urdf_path="./urdf_rm75/RM75-B.urdf", smoothness=0.05, enable_viewer=True): """ Args: urdf_path: Path to URDF file @@ -73,8 +73,7 @@ class MuJoCoPositionController: print("Viewer launched") except Exception as e: print(f"Viewer warning: {e}") - - print("Robot controller ready - Pure Position Mode") + self.start() def start(self): """Start the simulation thread""" diff --git a/kine_ctrl/urdf_rm75/RM75-B.csv b/kine_ctrl/urdf_rm75/RM75-B.csv new file mode 100644 index 0000000..99f2a05 --- /dev/null +++ b/kine_ctrl/urdf_rm75/RM75-B.csv @@ -0,0 +1,9 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +base_link,0.00049987,5.2709E-05,0.060019,0,0,0,0.83887,0.0017232,-3.1058E-06,-3.7924E-05,0.0017051,1.3691E-06,0.00090158,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,,连杆1-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +link_1,1.4803E-07,-0.021108,-0.025186,0,0,0,0.59354,0.0012661,6.0354E-09,-6.3788E-09,0.0011817,-0.00021121,0.00056132,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,,连杆2-1,link_1,joint_1,joint_1,revolute,0,0,0.2405,0,0,0,base_link,0,0,1,60,3.14,-3.106,3.106,,,,,,,, +link_2,4.2145E-07,-0.076129,0.011078,0,0,0,0.43285,0.0012584,1.4694E-09,-5.7413E-09,0.00031747,0.000279,0.0012225,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,,连杆3-1,link_2,joint_2,joint_2,revolute,0,0,0,-1.5708,0,0,link_1,0,0,1,60,3.14,-2.2689,2.2689,,,,,,,, +link_3,-3.2093E-07,-0.023545,-0.027347,0,0,0,0.43132,0.00079433,1.02E-09,1.3908E-08,0.00073037,-0.00014262,0.00031507,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,,连杆4-1,link_3,joint_3,joint_3,revolute,0,-0.256,0,1.5708,0,0,link_2,0,0,1,30,3.14,-3.106,3.106,,,,,,,, +link_4,5.0722E-06,-0.059593,0.010569,0,0,0,0.28963,0.00063737,7.0681E-08,3.8708E-08,0.00015648,0.00014461,0.00061418,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,,连杆5-1,link_4,joint_4,joint_4,revolute,0,0,0,-1.5708,0,0,link_3,0,0,1,30,3.14,-2.356,2.356,,,,,,,, +link_5,2.7551E-07,-0.018042,-0.02154,0,0,0,0.23942,0.00028595,1.9823E-09,-1.192E-09,0.00026273,-4.424E-05,0.0001199,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,,连杆6-1,link_5,joint_5,joint_5,revolute,0,-0.21,0,1.5708,0,0,link_4,0,0,1,10,3.14,-3.106,3.106,,,,,,,, +link_6,3.4947E-06,-0.059381,0.0073681,0,0,0,0.2188,0.00035054,3.4456E-08,1.7975E-08,0.00010493,7.8243E-05,0.00033448,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,,连杆7-1,link_6,joint_6,joint_6,revolute,0,0,0,-1.5708,0,0,link_5,0,0,1,10,3.14,-2.234,2.234,,,,,,,, +link_7,0.00081557,1.3323E-05,-0.012705,0,0,0,0.065037,2.1144E-05,2.2774E-08,2.5471E-08,1.8109E-05,1.019E-08,3.19E-05,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,,末端法兰件 方案一-1,link_7,joint_7,joint_7,revolute,0,-0.144,0,1.5708,0,0,link_6,0,0,1,10,3.14,-6.28,6.28,,,,,,,, diff --git a/kine_ctrl/urdf_rm75/RM75-B.urdf b/kine_ctrl/urdf_rm75/RM75-B.urdf new file mode 100644 index 0000000..6ecc903 --- /dev/null +++ b/kine_ctrl/urdf_rm75/RM75-B.urdf @@ -0,0 +1,453 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kine_ctrl/urdf_rm75/meshes/base_link.STL b/kine_ctrl/urdf_rm75/meshes/base_link.STL new file mode 100644 index 0000000..d5a35d5 Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/base_link.STL differ diff --git a/kine_ctrl/urdf_rm75/meshes/link_1.STL b/kine_ctrl/urdf_rm75/meshes/link_1.STL new file mode 100644 index 0000000..8b2bc5f Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/link_1.STL differ diff --git a/kine_ctrl/urdf_rm75/meshes/link_2.STL b/kine_ctrl/urdf_rm75/meshes/link_2.STL new file mode 100644 index 0000000..83d21b3 Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/link_2.STL differ diff --git a/kine_ctrl/urdf_rm75/meshes/link_3.STL b/kine_ctrl/urdf_rm75/meshes/link_3.STL new file mode 100644 index 0000000..5f6baba Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/link_3.STL differ diff --git a/kine_ctrl/urdf_rm75/meshes/link_4.STL b/kine_ctrl/urdf_rm75/meshes/link_4.STL new file mode 100644 index 0000000..949dc0f Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/link_4.STL differ diff --git a/kine_ctrl/urdf_rm75/meshes/link_5.STL b/kine_ctrl/urdf_rm75/meshes/link_5.STL new file mode 100644 index 0000000..bfaf4f9 Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/link_5.STL differ diff --git a/kine_ctrl/urdf_rm75/meshes/link_6.STL b/kine_ctrl/urdf_rm75/meshes/link_6.STL new file mode 100644 index 0000000..3ed9d50 Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/link_6.STL differ diff --git a/kine_ctrl/urdf_rm75/meshes/link_7.STL b/kine_ctrl/urdf_rm75/meshes/link_7.STL new file mode 100644 index 0000000..412c292 Binary files /dev/null and b/kine_ctrl/urdf_rm75/meshes/link_7.STL differ