Compare commits
2
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 LiuzhengSJ
|
9513dfa4ce | ||
|
LiuzhengSJ
|
e1d833812b |
+11
-4
@@ -8,6 +8,10 @@ from rm75_kine_rm import rm75_kine_api as kine_rm
|
|||||||
from rm75_mjc import MuJoCoPositionController
|
from rm75_mjc import MuJoCoPositionController
|
||||||
from Robotic_Arm.rm_robot_interface import *
|
from Robotic_Arm.rm_robot_interface import *
|
||||||
|
|
||||||
|
import os
|
||||||
|
cwd = os.getcwd()
|
||||||
|
|
||||||
|
|
||||||
import time
|
import time
|
||||||
from math import radians, degrees, pi, cos, sin
|
from math import radians, degrees, pi, cos, sin
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -35,14 +39,17 @@ def main():
|
|||||||
"""Demonstrate pure position control"""
|
"""Demonstrate pure position control"""
|
||||||
|
|
||||||
# Create controller
|
# Create controller
|
||||||
robot_mjk = MuJoCoPositionController()
|
robot_mjk = MuJoCoPositionController(urdf_path="./urdf_rm75/RM75-SCI.urdf")
|
||||||
|
|
||||||
|
|
||||||
# ----------- rm75 qp based kine ------------
|
# ----------- 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-SCI.urdf', mesh_dir='./urdf_rm75')
|
||||||
robot_kine_qp.add_tool_frames(tools_in_ee)
|
robot_kine_qp.add_tool_frames(tools_in_ee)
|
||||||
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||||
|
|
||||||
|
fp = robot_kine_qp.forward_kinematics(np.zeros(7))
|
||||||
|
print(f'forward kine res = {fp}')
|
||||||
|
|
||||||
# ---------- rm75 official algorithm -----------
|
# ---------- rm75 official algorithm -----------
|
||||||
robot_kine_rm = kine_rm()
|
robot_kine_rm = kine_rm()
|
||||||
robot_kine_rm.add_tool_frames(tools_in_ee)
|
robot_kine_rm.add_tool_frames(tools_in_ee)
|
||||||
@@ -100,8 +107,8 @@ def main():
|
|||||||
if d_p_ik < 0.01:
|
if d_p_ik < 0.01:
|
||||||
result[0][1] += 1
|
result[0][1] += 1
|
||||||
|
|
||||||
while 1:
|
# while 1:
|
||||||
time.sleep(1)
|
# time.sleep(1)
|
||||||
|
|
||||||
robot_mjk.send_command(q)
|
robot_mjk.send_command(q)
|
||||||
robot_mjk.wait_until_reached()
|
robot_mjk.wait_until_reached()
|
||||||
|
|||||||
@@ -71,6 +71,13 @@ class KinematicsSolver():
|
|||||||
0.3, 0.3, 0.2
|
0.3, 0.3, 0.2
|
||||||
])
|
])
|
||||||
|
|
||||||
|
self.scissor_frame_id = self.model.getFrameId("scissor_tcp")
|
||||||
|
self.camera_frame_id = self.model.getFrameId("camera_tcp")
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def add_frame(self,frame_name, position, rotationXYZ):
|
def add_frame(self,frame_name, position, rotationXYZ):
|
||||||
'''
|
'''
|
||||||
:param frame_name: str
|
:param frame_name: str
|
||||||
@@ -135,7 +142,8 @@ class KinematicsSolver():
|
|||||||
pin.updateFramePlacements(self.model, self.data)
|
pin.updateFramePlacements(self.model, self.data)
|
||||||
|
|
||||||
# Get frame transform
|
# Get frame transform
|
||||||
frame_id = self.tool_frames[tool]
|
# frame_id = self.tool_frames[tool]
|
||||||
|
frame_id = self.scissor_frame_id
|
||||||
frame_transform = self.data.oMf[frame_id]
|
frame_transform = self.data.oMf[frame_id]
|
||||||
|
|
||||||
# Extract results
|
# Extract results
|
||||||
|
|||||||
@@ -0,0 +1,516 @@
|
|||||||
|
<?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>
|
||||||
|
<!-- Scissor end-effector -->
|
||||||
|
<link name="scissor_link">
|
||||||
|
<inertial>
|
||||||
|
<!-- Replace these values with CAD-derived mass properties -->
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.1" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.0001"
|
||||||
|
ixy="0"
|
||||||
|
ixz="0"
|
||||||
|
iyy="0.0001"
|
||||||
|
iyz="0"
|
||||||
|
izz="0.0001" />
|
||||||
|
</inertial>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/scissor.stl"
|
||||||
|
scale="1 1 1" />
|
||||||
|
</geometry>
|
||||||
|
<material name="scissor_material">
|
||||||
|
<color rgba="0.7 0.7 0.7 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/scissor.stl"
|
||||||
|
scale="1 1 1" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||||
|
<joint name="scissor_fixed_joint" type="fixed">
|
||||||
|
<parent link="link_7" />
|
||||||
|
<child link="scissor_link" />
|
||||||
|
<origin xyz="0 -0.12 0.03" rpy="1.5707963 0 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="scissor_tcp"/>
|
||||||
|
|
||||||
|
<joint name="scissor_tcp_fixed" type="fixed">
|
||||||
|
<parent link="scissor_link"/>
|
||||||
|
<child link="scissor_tcp"/>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="camera_tcp"/>
|
||||||
|
|
||||||
|
<joint name="camera_tcp_fixed" type="fixed">
|
||||||
|
<parent link="scissor_link"/>
|
||||||
|
<child link="camera_tcp"/>
|
||||||
|
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.5707963"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
</robot>
|
||||||
@@ -0,0 +1,516 @@
|
|||||||
|
<?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>
|
||||||
|
<!-- Scissor end-effector -->
|
||||||
|
<link name="scissor_link">
|
||||||
|
<inertial>
|
||||||
|
<!-- Replace these values with CAD-derived mass properties -->
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<mass value="0.1" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.0001"
|
||||||
|
ixy="0"
|
||||||
|
ixz="0"
|
||||||
|
iyy="0.0001"
|
||||||
|
iyz="0"
|
||||||
|
izz="0.0001" />
|
||||||
|
</inertial>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/scissor.stl"
|
||||||
|
scale="1 1 1" />
|
||||||
|
</geometry>
|
||||||
|
<material name="scissor_material">
|
||||||
|
<color rgba="0.7 0.7 0.7 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="meshes/scissor.stl"
|
||||||
|
scale="1 1 1" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||||
|
<joint name="scissor_fixed_joint" type="fixed">
|
||||||
|
<parent link="link_7" />
|
||||||
|
<child link="scissor_link" />
|
||||||
|
<origin xyz="-0.12 0 0.063" rpy="0 -1.5707963 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="scissor_tcp"/>
|
||||||
|
|
||||||
|
<joint name="scissor_tcp_fixed" type="fixed">
|
||||||
|
<parent link="scissor_link"/>
|
||||||
|
<child link="scissor_tcp"/>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="camera_tcp"/>
|
||||||
|
|
||||||
|
<joint name="camera_tcp_fixed" type="fixed">
|
||||||
|
<parent link="scissor_link"/>
|
||||||
|
<child link="camera_tcp"/>
|
||||||
|
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
</robot>
|
||||||
@@ -469,7 +469,7 @@
|
|||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh
|
<mesh
|
||||||
filename="meshes/sci.stl"
|
filename="meshes/scissor.stl"
|
||||||
scale="1 1 1" />
|
scale="1 1 1" />
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="scissor_material">
|
<material name="scissor_material">
|
||||||
@@ -481,7 +481,7 @@
|
|||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh
|
<mesh
|
||||||
filename="meshes/sci.stl"
|
filename="meshes/scissor.stl"
|
||||||
scale="1 1 1" />
|
scale="1 1 1" />
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
@@ -491,6 +491,26 @@
|
|||||||
<joint name="scissor_fixed_joint" type="fixed">
|
<joint name="scissor_fixed_joint" type="fixed">
|
||||||
<parent link="link_7" />
|
<parent link="link_7" />
|
||||||
<child link="scissor_link" />
|
<child link="scissor_link" />
|
||||||
<origin xyz="0 0 0" rpy="3.14159 0 0" />
|
<origin xyz="0 0 0.165" rpy="0 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="scissor_tcp"/>
|
||||||
|
|
||||||
|
<joint name="scissor_tcp_fixed" type="fixed">
|
||||||
|
<parent link="scissor_link"/>
|
||||||
|
<child link="scissor_tcp"/>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="camera_tcp"/>
|
||||||
|
|
||||||
|
<joint name="camera_tcp_fixed" type="fixed">
|
||||||
|
<parent link="scissor_link"/>
|
||||||
|
<child link="camera_tcp"/>
|
||||||
|
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
</robot>
|
</robot>
|
||||||
|
|||||||
Binary file not shown.
Reference in New Issue
Block a user