Skip to content

Commit 1582d67

Browse files
LemonPiLemonPi
committed
Add UR5 FK test case
1 parent 5288100 commit 1582d67

File tree

2 files changed

+334
-6
lines changed

2 files changed

+334
-6
lines changed

‎tests/test_kinematics.py

+29-6
Original file line numberDiff line numberDiff line change
@@ -150,9 +150,9 @@ def test_fk_simple_arm():
150150
# print(chain.get_joint_parameter_names())
151151
ret = chain.forward_kinematics({
152152
'arm_shoulder_pan_joint': 0.,
153-
'arm_elbow_pan_joint': math.pi / 2.0,
154-
'arm_wrist_lift_joint': -0.5,
155-
'arm_wrist_roll_joint': 0.,
153+
'arm_elbow_pan_joint': math.pi / 2.0,
154+
'arm_wrist_lift_joint': -0.5,
155+
'arm_wrist_roll_joint': 0.,
156156
})
157157
tg = ret['arm_wrist_roll']
158158
pos, rot = quat_pos_from_transform3d(tg)
@@ -189,9 +189,9 @@ def test_cuda():
189189

190190
ret = chain.forward_kinematics({
191191
'arm_shoulder_pan_joint': 0,
192-
'arm_elbow_pan_joint': math.pi / 2.0,
193-
'arm_wrist_lift_joint': -0.5,
194-
'arm_wrist_roll_joint': 0,
192+
'arm_elbow_pan_joint': math.pi / 2.0,
193+
'arm_wrist_lift_joint': -0.5,
194+
'arm_wrist_roll_joint': 0,
195195
})
196196
tg = ret['arm_wrist_roll']
197197
pos, rot = quat_pos_from_transform3d(tg)
@@ -280,6 +280,28 @@ def test_fk_partial_batched():
280280
tg = chain.forward_kinematics(th)
281281

282282

283+
def test_ur5_fk():
284+
pk_chain = pk.build_serial_chain_from_urdf(open('ur5.urdf').read(), 'ee_link', 'base_link')
285+
th = [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0]
286+
287+
try:
288+
import ikpy.chain
289+
ik_chain = ikpy.chain.Chain.from_urdf_file('ur5.urdf',
290+
active_links_mask=[False, True, True, True, True, True, True, False])
291+
ik_ret = ik_chain.forward_kinematics([0, *th, 0])
292+
except ImportError:
293+
ik_ret = [[-6.44330720e-18, 3.58979314e-09, -1.00000000e+00, 5.10955359e-01],
294+
[1.00000000e+00, 1.79489651e-09, 0.00000000e+00, 1.91450000e-01],
295+
[1.79489651e-09, -1.00000000e+00, -3.58979312e-09, 6.00114361e-01],
296+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]
297+
298+
ret = pk_chain.forward_kinematics(th, end_only=True)
299+
print(ret.get_matrix())
300+
ik_ret = torch.tensor(ik_ret, dtype=ret.dtype)
301+
print(ik_ret)
302+
assert torch.allclose(ik_ret, ret.get_matrix(), atol=1e-6)
303+
304+
283305
if __name__ == "__main__":
284306
test_fk_partial_batched()
285307
test_fk_partial_batched_dict()
@@ -293,3 +315,4 @@ def test_fk_partial_batched():
293315
test_urdf()
294316
# test_fk_mjcf_humanoid()
295317
test_mjcf_slide_joint_parsing()
318+
test_ur5_fk()

‎tests/ur5.urdf

+305
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,305 @@
1+
<?xml version="1.0" ?>
2+
<!-- =================================================================================== -->
3+
<!-- | This document was autogenerated by xacro from ur5_joint_limited_box_robot_robotiq3.urdf.xacro | -->
4+
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
5+
<!-- =================================================================================== -->
6+
<robot name="ur5" xmlns:xacro="http://ros.org/wiki/xacro">
7+
<gazebo>
8+
<plugin filename="libgazebo_ros_control.so" name="ros_control">
9+
<!--robotNamespace>/</robotNamespace-->
10+
<!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
11+
</plugin>
12+
<!--
13+
<plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
14+
<alwaysOn>true</alwaysOn>
15+
<updateRate>1.0</updateRate>
16+
<timeout>5</timeout>
17+
<powerStateTopic>power_state</powerStateTopic>
18+
<powerStateRate>10.0</powerStateRate>
19+
<fullChargeCapacity>87.78</fullChargeCapacity>
20+
<dischargeRate>-474</dischargeRate>
21+
<chargeRate>525</chargeRate>
22+
<dischargeVoltage>15.52</dischargeVoltage>
23+
<chargeVoltage>16.41</chargeVoltage>
24+
</plugin>
25+
-->
26+
</gazebo>
27+
<!-- measured from model -->
28+
<!--property name="shoulder_height" value="0.089159" /-->
29+
<!--property name="shoulder_offset" value="0.13585" /-->
30+
<!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
31+
<!--property name="upper_arm_length" value="0.42500" /-->
32+
<!--property name="elbow_offset" value="0.1197" /-->
33+
<!-- CAD measured -->
34+
<!--property name="forearm_length" value="0.39225" /-->
35+
<!--property name="wrist_1_length" value="0.093" /-->
36+
<!-- CAD measured -->
37+
<!--property name="wrist_2_length" value="0.09465" /-->
38+
<!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
39+
<!--property name="wrist_3_length" value="0.0823" /-->
40+
<!-- manually measured -->
41+
<link name="base_link">
42+
<visual>
43+
<geometry>
44+
<mesh filename="package://ur_description/meshes/ur5/visual/base.dae"/>
45+
</geometry>
46+
<material name="LightGrey">
47+
<color rgba="0.7 0.7 0.7 1.0"/>
48+
</material>
49+
</visual>
50+
<collision>
51+
<geometry>
52+
<mesh filename="package://ur_description/meshes/ur5/collision/base.stl"/>
53+
</geometry>
54+
</collision>
55+
<inertial>
56+
<mass value="4.0"/>
57+
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
58+
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
59+
</inertial>
60+
</link>
61+
<joint name="shoulder_pan_joint" type="revolute">
62+
<parent link="base_link"/>
63+
<child link="shoulder_link"/>
64+
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
65+
<axis xyz="0 0 1"/>
66+
<limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
67+
<dynamics damping="0.0" friction="0.0"/>
68+
</joint>
69+
<link name="shoulder_link">
70+
<visual>
71+
<geometry>
72+
<mesh filename="package://ur_description/meshes/ur5/visual/shoulder.dae"/>
73+
</geometry>
74+
<material name="LightGrey">
75+
<color rgba="0.7 0.7 0.7 1.0"/>
76+
</material>
77+
</visual>
78+
<collision>
79+
<geometry>
80+
<mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl"/>
81+
</geometry>
82+
</collision>
83+
<inertial>
84+
<mass value="3.7"/>
85+
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
86+
<inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
87+
</inertial>
88+
</link>
89+
<joint name="shoulder_lift_joint" type="revolute">
90+
<parent link="shoulder_link"/>
91+
<child link="upper_arm_link"/>
92+
<origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
93+
<axis xyz="0 1 0"/>
94+
<limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
95+
<!-- <limit effort="150.0" lower="-3.14159265" upper="-0.8" velocity="3.15"/> -->
96+
<dynamics damping="0.0" friction="0.0"/>
97+
</joint>
98+
<link name="upper_arm_link">
99+
<visual>
100+
<geometry>
101+
<mesh filename="package://ur_description/meshes/ur5/visual/upperarm.dae"/>
102+
</geometry>
103+
<material name="LightGrey">
104+
<color rgba="0.7 0.7 0.7 1.0"/>
105+
</material>
106+
</visual>
107+
<collision>
108+
<geometry>
109+
<mesh filename="package://ur_description/meshes/ur5/collision/upperarm.stl"/>
110+
</geometry>
111+
</collision>
112+
<inertial>
113+
<mass value="8.393"/>
114+
<origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
115+
<inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
116+
</inertial>
117+
</link>
118+
<joint name="elbow_joint" type="revolute">
119+
<parent link="upper_arm_link"/>
120+
<child link="forearm_link"/>
121+
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
122+
<axis xyz="0 1 0"/>
123+
<limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
124+
<dynamics damping="0.0" friction="0.0"/>
125+
</joint>
126+
<link name="forearm_link">
127+
<visual>
128+
<geometry>
129+
<mesh filename="package://ur_description/meshes/ur5/visual/forearm.dae"/>
130+
</geometry>
131+
<material name="LightGrey">
132+
<color rgba="0.7 0.7 0.7 1.0"/>
133+
</material>
134+
</visual>
135+
<collision>
136+
<geometry>
137+
<mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl"/>
138+
</geometry>
139+
</collision>
140+
<inertial>
141+
<mass value="2.275"/>
142+
<origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
143+
<inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
144+
</inertial>
145+
</link>
146+
<joint name="wrist_1_joint" type="revolute">
147+
<parent link="forearm_link"/>
148+
<child link="wrist_1_link"/>
149+
<origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/>
150+
<axis xyz="0 1 0"/>
151+
<limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
152+
<dynamics damping="0.0" friction="0.0"/>
153+
</joint>
154+
<link name="wrist_1_link">
155+
<visual>
156+
<geometry>
157+
<mesh filename="package://ur_description/meshes/ur5/visual/wrist1.dae"/>
158+
</geometry>
159+
<material name="LightGrey">
160+
<color rgba="0.7 0.7 0.7 1.0"/>
161+
</material>
162+
</visual>
163+
<collision>
164+
<geometry>
165+
<mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl"/>
166+
</geometry>
167+
</collision>
168+
<inertial>
169+
<mass value="1.219"/>
170+
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
171+
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
172+
</inertial>
173+
</link>
174+
<joint name="wrist_2_joint" type="revolute">
175+
<parent link="wrist_1_link"/>
176+
<child link="wrist_2_link"/>
177+
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
178+
<axis xyz="0 0 1"/>
179+
<limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
180+
<dynamics damping="0.0" friction="0.0"/>
181+
</joint>
182+
<link name="wrist_2_link">
183+
<visual>
184+
<geometry>
185+
<mesh filename="package://ur_description/meshes/ur5/visual/wrist2.dae"/>
186+
</geometry>
187+
<material name="LightGrey">
188+
<color rgba="0.7 0.7 0.7 1.0"/>
189+
</material>
190+
</visual>
191+
<collision>
192+
<geometry>
193+
<mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl"/>
194+
</geometry>
195+
</collision>
196+
<inertial>
197+
<mass value="1.219"/>
198+
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
199+
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
200+
</inertial>
201+
</link>
202+
<joint name="wrist_3_joint" type="revolute">
203+
<parent link="wrist_2_link"/>
204+
<child link="wrist_3_link"/>
205+
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
206+
<axis xyz="0 1 0"/>
207+
<limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
208+
<dynamics damping="0.0" friction="0.0"/>
209+
</joint>
210+
<link name="wrist_3_link">
211+
<visual>
212+
<geometry>
213+
<mesh filename="package://ur_description/meshes/ur5/visual/wrist3.dae"/>
214+
</geometry>
215+
<material name="LightGrey">
216+
<color rgba="0.7 0.7 0.7 1.0"/>
217+
</material>
218+
</visual>
219+
<collision>
220+
<geometry>
221+
<mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl"/>
222+
</geometry>
223+
</collision>
224+
<inertial>
225+
<mass value="0.1879"/>
226+
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
227+
<inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
228+
</inertial>
229+
</link>
230+
<joint name="ee_fixed_joint" type="fixed">
231+
<parent link="wrist_3_link"/>
232+
<child link="ee_link"/>
233+
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0823 0.0"/>
234+
</joint>
235+
<link name="ee_link">
236+
<collision>
237+
<geometry>
238+
<box size="0.01 0.01 0.01"/>
239+
</geometry>
240+
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
241+
</collision>
242+
</link>
243+
<!-- Frame coincident with all-zeros TCP on UR controller -->
244+
<link name="tool0"/>
245+
<joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
246+
<origin rpy="-1.570796325 0 0" xyz="0 0.0823 0"/>
247+
<parent link="wrist_3_link"/>
248+
<child link="tool0"/>
249+
</joint>
250+
<transmission name="shoulder_pan_trans">
251+
<type>transmission_interface/SimpleTransmission</type>
252+
<joint name="shoulder_pan_joint">
253+
<hardwareInterface>PositionJointInterface</hardwareInterface>
254+
</joint>
255+
<actuator name="shoulder_pan_motor">
256+
<mechanicalReduction>1</mechanicalReduction>
257+
</actuator>
258+
</transmission>
259+
<transmission name="shoulder_lift_trans">
260+
<type>transmission_interface/SimpleTransmission</type>
261+
<joint name="shoulder_lift_joint">
262+
<hardwareInterface>PositionJointInterface</hardwareInterface>
263+
</joint>
264+
<actuator name="shoulder_lift_motor">
265+
<mechanicalReduction>1</mechanicalReduction>
266+
</actuator>
267+
</transmission>
268+
<transmission name="elbow_trans">
269+
<type>transmission_interface/SimpleTransmission</type>
270+
<joint name="elbow_joint">
271+
<hardwareInterface>PositionJointInterface</hardwareInterface>
272+
</joint>
273+
<actuator name="elbow_motor">
274+
<mechanicalReduction>1</mechanicalReduction>
275+
</actuator>
276+
</transmission>
277+
<transmission name="wrist_1_trans">
278+
<type>transmission_interface/SimpleTransmission</type>
279+
<joint name="wrist_1_joint">
280+
<hardwareInterface>PositionJointInterface</hardwareInterface>
281+
</joint>
282+
<actuator name="wrist_1_motor">
283+
<mechanicalReduction>1</mechanicalReduction>
284+
</actuator>
285+
</transmission>
286+
<transmission name="wrist_2_trans">
287+
<type>transmission_interface/SimpleTransmission</type>
288+
<joint name="wrist_2_joint">
289+
<hardwareInterface>PositionJointInterface</hardwareInterface>
290+
</joint>
291+
<actuator name="wrist_2_motor">
292+
<mechanicalReduction>1</mechanicalReduction>
293+
</actuator>
294+
</transmission>
295+
<transmission name="wrist_3_trans">
296+
<type>transmission_interface/SimpleTransmission</type>
297+
<joint name="wrist_3_joint">
298+
<hardwareInterface>PositionJointInterface</hardwareInterface>
299+
</joint>
300+
<actuator name="wrist_3_motor">
301+
<mechanicalReduction>1</mechanicalReduction>
302+
</actuator>
303+
</transmission>
304+
</robot>
305+

0 commit comments

Comments
 (0)