Debian Codegraph @

python/tests/integration/test_joint_force_torque.py

raw
1 import pytest
2 import math
3 import numpy as np
4 import dartpy as dart
5
6
7 def read_world(uri: dart.common.Uri):
8 options = dart.utils.SdfParser.Options()
9 options.mDefaultRootJointType = dart.utils.SdfParser.RootJointType.FIXED
10 world = dart.utils.SdfParser.readWorld(uri, options)
11 for i in range(world.getNumSkeletons()):
12 skel = world.getSkeleton(i)
13 for j in range(skel.getNumJoints()):
14 joint = skel.getJoint(j)
15 joint.setLimitEnforcement(True)
16 return world
17
18
19 def test_static():
20 # Load world
21 world = read_world("dart://sample/sdf/test/force_torque_test.world")
22 assert world is not None
23 assert world.getNumSkeletons() == 1
24
25 # Check if the world is correctly loaded
26 model_1 = world.getSkeleton(0)
27 assert model_1 is not None
28 assert model_1.getName() == "model_1"
29 assert model_1.getNumBodyNodes() == 2
30 assert model_1.getNumJoints() == 2
31
32 world.setGravity([0, 0, -50])
33
34 # Simulate 1 step
35 world.step()
36 t = world.getTime()
37
38 # Get time step size
39 dt = world.getTimeStep()
40 assert dt > 0
41 assert t == pytest.approx(dt)
42
43 # Get joint and get force torque
44 link_1 = model_1.getBodyNode("link_1")
45 assert link_1 is not None
46 link_2 = model_1.getBodyNode("link_2")
47 assert link_2 is not None
48 joint_01 = model_1.getJoint("joint_01")
49 assert joint_01 is not None
50 joint_12 = model_1.getJoint("joint_12")
51 assert joint_12 is not None
52
53 tf = dart.math.Isometry3()
54
55 # Run 10 steps
56 for _ in range(10):
57 world.step()
58
59 #----------------------
60 # Test joint_01 wrench
61 #----------------------
62
63 # Reference adjustment for the difference of the joint frame conventions
64 # between Gazebo and DART
65 tf.set_identity()
66 tf.set_translation(joint_01.getTransformFromParentBodyNode().translation())
67 parent_frame01 = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), "parent_frame01", tf)
68 tf.set_identity()
69 tf.set_translation(joint_01.getTransformFromChildBodyNode().translation())
70 child_frame01 = dart.dynamics.SimpleFrame(link_1, "child_frame01", tf)
71
72 parent_f01 = joint_01.getWrenchToParentBodyNode(parent_frame01)
73 assert (parent_f01 == [0, 0, 0, 0, 0, 1000]).all()
74
75 child_f01 = joint_01.getWrenchToChildBodyNode(child_frame01)
76 assert (child_f01 == -parent_f01).all()
77
78 #----------------------
79 # Test joint_12 wrench
80 #----------------------
81
82 # Reference adjustment for the difference of the joint frame conventions
83 # between Gazebo and DART
84 tf.set_identity()
85 tf.set_translation(joint_12.getTransformFromParentBodyNode().translation())
86 parent_frame12 = dart.dynamics.SimpleFrame(link_1, "parent_frame12", tf)
87 tf.set_identity()
88 tf.set_translation(joint_12.getTransformFromChildBodyNode().translation())
89 child_frame12 = dart.dynamics.SimpleFrame(link_2, "child_frame12", tf)
90
91 parent_f12 = joint_12.getWrenchToParentBodyNode(parent_frame12)
92 assert (parent_f12 == [0, 0, 0, 0, 0, 500]).all()
93
94 child_f12 = joint_12.getWrenchToChildBodyNode(child_frame12)
95 assert (child_f12 == -parent_f12).all()
96
97
98 def test_force_torque_at_joint_limits():
99 # Load world
100 world = read_world("dart://sample/sdf/test/force_torque_test.world")
101 assert world is not None
102 assert world.getNumSkeletons() == 1
103
104 # Check if the world is correctly loaded
105 model_1 = world.getSkeleton(0)
106 assert model_1 is not None
107 assert model_1.getName() == "model_1"
108 assert model_1.getNumBodyNodes() == 2
109 assert model_1.getNumJoints() == 2
110
111 world.setGravity([0, 0, -50])
112
113 # Simulate 1 step
114 world.step()
115 t = world.getTime()
116
117 # Get time step size
118 dt = world.getTimeStep()
119 assert dt > 0
120 assert t == pytest.approx(dt)
121
122 # Get joint and get force torque
123 link_1 = model_1.getBodyNode("link_1")
124 assert link_1 is not None
125 link_2 = model_1.getBodyNode("link_2")
126 assert link_2 is not None
127 joint_01 = model_1.getJoint("joint_01")
128 assert joint_01 is not None
129 joint_12 = model_1.getJoint("joint_12")
130 assert joint_12 is not None
131
132 # Change gravity so that the top link topples over, then remeasure
133 world.setGravity([-30, 10, -50])
134
135 # Wait for dynamics to be stabilized
136 for i in range(2000):
137 world.step()
138
139 tf = dart.math.Isometry3()
140
141 # Run 5 steps
142 for _ in range(5):
143 world.step()
144
145 #----------------------
146 # Test joint_01 wrench
147 #----------------------
148
149 # Reference adjustment for the difference of the joint frame conventions
150 # between Gazebo and DART
151 tf.set_identity()
152 tf.set_translation(joint_01.getTransformFromParentBodyNode().translation())
153 parent_frame01 = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), "parent_frame01", tf)
154 tf.set_identity()
155 tf.set_translation(joint_01.getTransformFromChildBodyNode().translation())
156 child_frame01 = dart.dynamics.SimpleFrame(link_1, "child_frame01", tf)
157
158 parent_f01 = joint_01.getWrenchToParentBodyNode(parent_frame01)
159 assert np.isclose(parent_f01, [750, 0, -450, 600, -200, 1000], rtol=0.1, atol=4.5).all()
160
161 child_f01 = joint_01.getWrenchToChildBodyNode(child_frame01)
162 assert np.isclose(child_f01, [-750, -450, 0, -600, 1000, 200], rtol=0.1, atol=4.5).all()
163
164 #----------------------
165 # Test joint_12 wrench
166 #----------------------
167
168 # Reference adjustment for the difference of the joint frame conventions
169 # between Gazebo and DART
170 tf.set_identity()
171 tf.set_translation(joint_12.getTransformFromParentBodyNode().translation())
172 parent_frame12 = dart.dynamics.SimpleFrame(link_1, "parent_frame12", tf)
173 tf.set_identity()
174 tf.set_translation(joint_12.getTransformFromChildBodyNode().translation())
175 child_frame12 = dart.dynamics.SimpleFrame(link_2, "child_frame12", tf)
176
177 parent_f12 = joint_12.getWrenchToParentBodyNode(parent_frame12)
178 assert np.isclose(parent_f12, [250, 150, 0, 300, -500, -100], rtol=0.1, atol=0.1).all()
179
180 child_f12 = joint_12.getWrenchToChildBodyNode(child_frame12)
181 assert np.isclose(child_f12, [-250, -150, 0, -300, 500, 100], rtol=0.1, atol=0.1).all()
182
183
184 def test_force_torque_at_joint_limits_with_external_forces():
185 # Load world
186 world = read_world("dart://sample/sdf/test/force_torque_test2.world")
187 assert world is not None
188 assert world.getNumSkeletons() == 1
189
190 # Check if the world is correctly loaded
191 model_1 = world.getSkeleton(0)
192 assert model_1 is not None
193 assert model_1.getName() == "boxes"
194 assert model_1.getNumBodyNodes() == 3
195 assert model_1.getNumJoints() == 3
196 assert model_1.getNumDofs() == 2
197
198 # The first joint is fixed joint
199 assert model_1.getRootJoint().getType() == dart.dynamics.WeldJoint.getStaticType()
200
201 world.setGravity([0, 0, -50])
202
203 # Simulate 1 step
204 world.step()
205 t = world.getTime()
206
207 # Get time step size
208 dt = world.getTimeStep()
209 assert dt > 0
210 assert t == pytest.approx(dt)
211
212 # Get joint and get force torque
213 link_1 = model_1.getBodyNode("link1")
214 assert link_1 is not None
215 link_2 = model_1.getBodyNode("link2")
216 assert link_2 is not None
217 link_3 = model_1.getBodyNode("link3")
218 assert link_3 is not None
219 joint_12 = model_1.getJoint("joint1")
220 assert joint_12 is not None
221 joint_23 = model_1.getJoint("joint2")
222 assert joint_23 is not None
223
224 tf = dart.math.Isometry3()
225
226 # Run 45005 steps
227 kp1 = 5e+4
228 kp2 = 1e+4
229 target1 = 0
230 target2 = -0.25 * math.pi
231 steps = 4500
232 for _ in range(steps):
233 # PD control
234 j1_state = joint_12.getPosition(0)
235 j2_state = joint_23.getPosition(0)
236 p1_error = target1 - j1_state
237 p2_error = target2 - j2_state
238 effort1 = kp1 * p1_error
239 effort2 = kp2 * p2_error
240 joint_12.setForce(0, effort1)
241 joint_23.setForce(0, effort2)
242
243 world.step()
244
245 assert joint_12.getPosition(0) == pytest.approx(target1, abs=1e-1)
246 assert joint_23.getPosition(0) == pytest.approx(target2, abs=1e-1)
247
248 tol = 2
249
250 #----------------------
251 # Test joint_12 wrench
252 #----------------------
253
254 # Reference adjustment for the difference of the joint frame conventions
255 # between Gazebo and DART
256 tf.set_identity()
257 tf.set_translation(joint_12.getTransformFromParentBodyNode().translation())
258 parent_frame01 = dart.dynamics.SimpleFrame(link_1, "parent_frame01", tf)
259 tf.set_identity()
260 tf.set_translation(joint_12.getTransformFromChildBodyNode().translation())
261 child_frame01 = dart.dynamics.SimpleFrame(link_2, "child_frame01", tf)
262
263 parent_f01 = joint_12.getWrenchToParentBodyNode(parent_frame01)
264 assert np.isclose(parent_f01, [25, -175, 0, 0, 0, 300], rtol=0.01, atol=tol).all()
265
266 child_f01 = joint_12.getWrenchToChildBodyNode(child_frame01)
267 assert np.isclose(child_f01, [-25, 175, 0, 0, 0, -300], rtol=0.01, atol=tol).all()
268
269 #----------------------
270 # Test joint_23 wrench
271 #----------------------
272
273 # Reference adjustment for the difference of the joint frame conventions
274 # between Gazebo and DART
275 tf.set_identity()
276 tf.set_translation(joint_23.getTransformFromParentBodyNode().translation())
277 parent_frame12 = dart.dynamics.SimpleFrame(link_2, "parent_frame12", tf)
278 tf.set_identity()
279 tf.set_translation(joint_23.getTransformFromChildBodyNode().translation())
280 child_frame12 = dart.dynamics.SimpleFrame(link_3, "child_frame12", tf)
281
282 parent_f12 = joint_23.getWrenchToParentBodyNode(parent_frame12)
283 assert np.isclose(parent_f12, [25, 0, 0, 0, 0, 50], rtol=0.01, atol=tol).all()
284
285 child_f12 = joint_23.getWrenchToChildBodyNode(child_frame12)
286 assert np.isclose(child_f12, [-17.678, 0, 17.679, -35.355, 0, -35.355], rtol=0.01, atol=tol).all()
287
288
289
290
291 if __name__ == "__main__":
292 pytest.main()