forked from ZhengLiu-cart/IK_qp
add urdf files.
aligh the function parameter names of qp and rm methods
This commit is contained in:
LiuzhengSJ
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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"):
|
||||
|
||||
@ -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
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
@ -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"""
|
||||
|
||||
9
kine_ctrl/urdf_rm75/RM75-B.csv
Normal file
9
kine_ctrl/urdf_rm75/RM75-B.csv
Normal file
@ -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,,,,,,,,
|
||||
|
453
kine_ctrl/urdf_rm75/RM75-B.urdf
Normal file
453
kine_ctrl/urdf_rm75/RM75-B.urdf
Normal file
@ -0,0 +1,453 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
</robot>
|
||||
BIN
kine_ctrl/urdf_rm75/meshes/base_link.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/base_link.STL
Normal file
Binary file not shown.
BIN
kine_ctrl/urdf_rm75/meshes/link_1.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/link_1.STL
Normal file
Binary file not shown.
BIN
kine_ctrl/urdf_rm75/meshes/link_2.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/link_2.STL
Normal file
Binary file not shown.
BIN
kine_ctrl/urdf_rm75/meshes/link_3.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/link_3.STL
Normal file
Binary file not shown.
BIN
kine_ctrl/urdf_rm75/meshes/link_4.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/link_4.STL
Normal file
Binary file not shown.
BIN
kine_ctrl/urdf_rm75/meshes/link_5.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/link_5.STL
Normal file
Binary file not shown.
BIN
kine_ctrl/urdf_rm75/meshes/link_6.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/link_6.STL
Normal file
Binary file not shown.
BIN
kine_ctrl/urdf_rm75/meshes/link_7.STL
Normal file
BIN
kine_ctrl/urdf_rm75/meshes/link_7.STL
Normal file
Binary file not shown.
Reference in New Issue
Block a user