dartpy 0.2.0.post107__cp311-cp311-manylinux_2_28_x86_64.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.

Potentially problematic release.


This version of dartpy might be problematic. Click here for more details.

Files changed (64) hide show
  1. dartpy-0.2.0.post107.dist-info/LICENSE +31 -0
  2. dartpy-0.2.0.post107.dist-info/METADATA +47 -0
  3. dartpy-0.2.0.post107.dist-info/RECORD +64 -0
  4. dartpy-0.2.0.post107.dist-info/WHEEL +5 -0
  5. dartpy-0.2.0.post107.dist-info/top_level.txt +2 -0
  6. dartpy.cpython-311-x86_64-linux-gnu.so +0 -0
  7. dartpy.libs/libGLU-ff1477cc.so.1.3.1 +0 -0
  8. dartpy.libs/libOpenThreads-0250ab5c.so.3.3.1 +0 -0
  9. dartpy.libs/libXi-9178a6bd.so.6.1.0 +0 -0
  10. dartpy.libs/libXxf86vm-8446c95f.so.1.0.0 +0 -0
  11. dartpy.libs/libassimp-79671a85.so.5.2.4 +0 -0
  12. dartpy.libs/libccd-f7023733.so.2.0 +0 -0
  13. dartpy.libs/libconsole_bridge-757d7af3.so.1.0 +0 -0
  14. dartpy.libs/libfcl-fafce69a.so.0.7.0 +0 -0
  15. dartpy.libs/libglut-0c8fd390.so.3.10.0 +0 -0
  16. dartpy.libs/liboctomap-0c5b15c6.so.1.9.6 +0 -0
  17. dartpy.libs/liboctomath-31ce0045.so.1.9.6 +0 -0
  18. dartpy.libs/libosg-b45f9cbc.so.3.7.0 +0 -0
  19. dartpy.libs/libosgDB-209e5143.so.3.7.0 +0 -0
  20. dartpy.libs/libosgGA-260d5c6e.so.3.7.0 +0 -0
  21. dartpy.libs/libosgManipulator-05bcdf02.so.3.7.0 +0 -0
  22. dartpy.libs/libosgShadow-9971a879.so.3.7.0 +0 -0
  23. dartpy.libs/libosgText-16341674.so.3.7.0 +0 -0
  24. dartpy.libs/libosgUtil-e5d05863.so.3.7.0 +0 -0
  25. dartpy.libs/libosgViewer-7dd0f485.so.3.7.0 +0 -0
  26. dartpy.libs/libtinyxml-296537c6.so +0 -0
  27. dartpy.libs/liburdfdom_model-6cad1cb6.so.3.0 +0 -0
  28. dartpy.libs/liburdfdom_model_state-36524f5e.so.3.0 +0 -0
  29. dartpy.libs/liburdfdom_sensor-ad2c126c.so.3.0 +0 -0
  30. dartpy.libs/liburdfdom_world-538ea8a8.so.3.0 +0 -0
  31. tests/__init__.py +0 -0
  32. tests/integration/__init__.py +0 -0
  33. tests/integration/test_joint_force_torque.py +305 -0
  34. tests/unit/__init__.py +0 -0
  35. tests/unit/collision/__init__.py +0 -0
  36. tests/unit/collision/test_collision.py +249 -0
  37. tests/unit/common/__init__.py +0 -0
  38. tests/unit/common/test_logging.py +20 -0
  39. tests/unit/common/test_stopwatch.py +50 -0
  40. tests/unit/common/test_string.py +42 -0
  41. tests/unit/common/test_uri.py +20 -0
  42. tests/unit/constraint/__init__.py +0 -0
  43. tests/unit/constraint/test_constraint.py +42 -0
  44. tests/unit/dynamics/__init__.py +0 -0
  45. tests/unit/dynamics/test_aspect.py +15 -0
  46. tests/unit/dynamics/test_body_node.py +65 -0
  47. tests/unit/dynamics/test_inertia.py +76 -0
  48. tests/unit/dynamics/test_inverse_kinematics.py +98 -0
  49. tests/unit/dynamics/test_joint.py +149 -0
  50. tests/unit/dynamics/test_meta_skeleton.py +41 -0
  51. tests/unit/dynamics/test_simple_frame.py +54 -0
  52. tests/unit/dynamics/test_skeleton.py +23 -0
  53. tests/unit/math/__init__.py +0 -0
  54. tests/unit/math/test_random.py +40 -0
  55. tests/unit/optimizer/__init__.py +0 -0
  56. tests/unit/optimizer/test_optimizer.py +93 -0
  57. tests/unit/simulation/__init__.py +0 -0
  58. tests/unit/simulation/test_world.py +45 -0
  59. tests/unit/utils/__init__.py +0 -0
  60. tests/unit/utils/test_dart_loader.py +72 -0
  61. tests/unit/utils/test_mjcf_parser.py +19 -0
  62. tests/unit/utils/test_sdf_parser.py +21 -0
  63. tests/unit/utils/test_skel_parser.py +22 -0
  64. tests/util.py +18 -0
@@ -0,0 +1,249 @@
1
+ import platform
2
+
3
+ import dartpy as dart
4
+ import numpy as np
5
+ import pytest
6
+
7
+
8
+ def collision_groups_tester(cd):
9
+ size = [1, 1, 1]
10
+ pos1 = [0, 0, 0]
11
+ pos2 = [0.5, 0, 0]
12
+
13
+ simple_frame1 = dart.dynamics.SimpleFrame()
14
+ simple_frame2 = dart.dynamics.SimpleFrame()
15
+
16
+ sphere1 = dart.dynamics.SphereShape(1)
17
+ sphere2 = dart.dynamics.SphereShape(1)
18
+
19
+ simple_frame1.setShape(sphere1)
20
+ simple_frame2.setShape(sphere2)
21
+
22
+ group = cd.createCollisionGroup()
23
+ group.addShapeFrame(simple_frame1)
24
+ group.addShapeFrame(simple_frame2)
25
+ assert group.getNumShapeFrames() is 2
26
+
27
+ #
28
+ # ( s1,s2 ) collision!
29
+ # ---+---|---+---+---+---+--->
30
+ # -1 0 +1 +2 +3 +4
31
+ #
32
+ assert group.collide()
33
+
34
+ #
35
+ # ( s1 ) ( s2 ) no collision
36
+ # ---+---|---+---+---+---+--->
37
+ # -1 0 +1 +2 +3 +4
38
+ #
39
+ simple_frame2.setTranslation([3, 0, 0])
40
+ assert not group.collide()
41
+
42
+ option = dart.collision.CollisionOption()
43
+ result = dart.collision.CollisionResult()
44
+
45
+ group.collide(option, result)
46
+ assert not result.isCollision()
47
+ assert result.getNumContacts() is 0
48
+
49
+ option.enableContact = True
50
+ simple_frame2.setTranslation([1.99, 0, 0])
51
+
52
+ group.collide(option, result)
53
+ assert result.isCollision()
54
+ assert result.getNumContacts() is not 0
55
+
56
+ # Repeat the same test with BodyNodes instead of SimpleFrames
57
+
58
+ group.removeAllShapeFrames()
59
+ assert group.getNumShapeFrames() is 0
60
+
61
+ skel1 = dart.dynamics.Skeleton()
62
+ skel2 = dart.dynamics.Skeleton()
63
+
64
+ [joint1, body1] = skel1.createFreeJointAndBodyNodePair(None)
65
+ [joint2, body2] = skel2.createFreeJointAndBodyNodePair(None)
66
+
67
+ shape_node1 = body1.createShapeNode(sphere1)
68
+ shape_node1.createVisualAspect()
69
+ shape_node1.createCollisionAspect()
70
+
71
+ shape_node2 = body2.createShapeNode(sphere2)
72
+ shape_node2.createVisualAspect()
73
+ shape_node2.createCollisionAspect()
74
+
75
+ group.addShapeFramesOf(body1)
76
+ group.addShapeFramesOf(body2)
77
+
78
+ assert group.getNumShapeFrames() is 2
79
+
80
+ assert group.collide()
81
+
82
+ joint2.setPosition(3, 3)
83
+ assert not group.collide()
84
+
85
+ # Repeat the same test with BodyNodes and two groups
86
+
87
+ joint2.setPosition(3, 0)
88
+
89
+ group.removeAllShapeFrames()
90
+ assert group.getNumShapeFrames() is 0
91
+ group2 = cd.createCollisionGroup()
92
+
93
+ group.addShapeFramesOf(body1)
94
+ group2.addShapeFramesOf(body2)
95
+
96
+ assert group.getNumShapeFrames() is 1
97
+ assert group2.getNumShapeFrames() is 1
98
+
99
+ assert group.collide(group2)
100
+
101
+ joint2.setPosition(3, 3)
102
+ assert not group.collide(group2)
103
+
104
+
105
+ def test_collision_groups():
106
+ cd = dart.collision.FCLCollisionDetector()
107
+ collision_groups_tester(cd)
108
+
109
+ cd = dart.collision.DARTCollisionDetector()
110
+ collision_groups_tester(cd)
111
+
112
+ if hasattr(dart.collision, "BulletCollisionDetector"):
113
+ cd = dart.collision.BulletCollisionDetector()
114
+ collision_groups_tester(cd)
115
+
116
+ if hasattr(dart.collision, "OdeCollisionDetector"):
117
+ cd = dart.collision.OdeCollisionDetector()
118
+ collision_groups_tester(cd)
119
+
120
+
121
+ # TODO: Add more collision detectors
122
+ @pytest.mark.parametrize("cd", [dart.collision.FCLCollisionDetector()])
123
+ def test_filter(cd):
124
+ # Create two bodies skeleton. The two bodies are placed at the same position
125
+ # with the same size shape so that they collide by default.
126
+ skel = dart.dynamics.Skeleton()
127
+
128
+ shape = dart.dynamics.BoxShape(np.ones(3))
129
+
130
+ _, body0 = skel.createRevoluteJointAndBodyNodePair()
131
+ shape_node0 = body0.createShapeNode(shape)
132
+ shape_node0.createVisualAspect()
133
+ shape_node0.createCollisionAspect()
134
+
135
+ _, body1 = skel.createRevoluteJointAndBodyNodePair(body0)
136
+ shape_node1 = body1.createShapeNode(shape)
137
+ shape_node1.createVisualAspect()
138
+ shape_node1.createCollisionAspect()
139
+
140
+ # Create a world and add the created skeleton
141
+ world = dart.simulation.World()
142
+ world.addSkeleton(skel)
143
+
144
+ # Set a new collision detector
145
+ constraint_solver = world.getConstraintSolver()
146
+ constraint_solver.setCollisionDetector(cd)
147
+
148
+ # Get the collision group from the constraint solver
149
+ group = constraint_solver.getCollisionGroup()
150
+ assert group.getNumShapeFrames() == 2
151
+
152
+ # Create BodyNodeCollisionFilter
153
+ option = constraint_solver.getCollisionOption()
154
+ body_node_filter = dart.collision.BodyNodeCollisionFilter()
155
+ option.collisionFilter = body_node_filter
156
+
157
+ skel.enableSelfCollisionCheck()
158
+ skel.enableAdjacentBodyCheck()
159
+ assert skel.isEnabledSelfCollisionCheck()
160
+ assert skel.isEnabledAdjacentBodyCheck()
161
+ assert group.collide()
162
+ assert group.collide(option)
163
+
164
+ skel.enableSelfCollisionCheck()
165
+ skel.disableAdjacentBodyCheck()
166
+ assert skel.isEnabledSelfCollisionCheck()
167
+ assert not skel.isEnabledAdjacentBodyCheck()
168
+ assert group.collide()
169
+ assert not group.collide(option)
170
+
171
+ skel.disableSelfCollisionCheck()
172
+ skel.enableAdjacentBodyCheck()
173
+ assert not skel.isEnabledSelfCollisionCheck()
174
+ assert skel.isEnabledAdjacentBodyCheck()
175
+ assert group.collide()
176
+ assert not group.collide(option)
177
+
178
+ skel.disableSelfCollisionCheck()
179
+ skel.disableAdjacentBodyCheck()
180
+ assert not skel.isEnabledSelfCollisionCheck()
181
+ assert not skel.isEnabledAdjacentBodyCheck()
182
+ assert group.collide()
183
+ assert not group.collide(option)
184
+
185
+ # Test collision body filtering
186
+ skel.enableSelfCollisionCheck()
187
+ skel.enableAdjacentBodyCheck()
188
+ body_node_filter.addBodyNodePairToBlackList(body0, body1)
189
+ assert not group.collide(option)
190
+ body_node_filter.removeBodyNodePairFromBlackList(body0, body1)
191
+ assert group.collide(option)
192
+ body_node_filter.addBodyNodePairToBlackList(body0, body1)
193
+ assert not group.collide(option)
194
+ body_node_filter.removeAllBodyNodePairsFromBlackList()
195
+ assert group.collide(option)
196
+
197
+
198
+ def test_raycast():
199
+ cd = dart.collision.BulletCollisionDetector()
200
+
201
+ simple_frame = dart.dynamics.SimpleFrame()
202
+ sphere = dart.dynamics.SphereShape(1)
203
+ simple_frame.setShape(sphere)
204
+
205
+ group = cd.createCollisionGroup()
206
+ group.addShapeFrame(simple_frame)
207
+ assert group.getNumShapeFrames() == 1
208
+
209
+ option = dart.collision.RaycastOption()
210
+ option.mEnableAllHits = False
211
+
212
+ result = dart.collision.RaycastResult()
213
+ assert not result.hasHit()
214
+
215
+ ray_hit = dart.collision.RayHit()
216
+
217
+ result.clear()
218
+ simple_frame.setTranslation(np.zeros(3))
219
+ assert group.raycast([-2, 0, 0], [2, 0, 0], option, result)
220
+ assert result.hasHit()
221
+ assert len(result.mRayHits) == 1
222
+ ray_hit = result.mRayHits[0]
223
+ assert np.isclose(ray_hit.mPoint, [-1, 0, 0]).all()
224
+ assert np.isclose(ray_hit.mNormal, [-1, 0, 0]).all()
225
+ assert ray_hit.mFraction == pytest.approx(0.25)
226
+
227
+ result.clear()
228
+ simple_frame.setTranslation(np.zeros(3))
229
+ assert group.raycast([2, 0, 0], [-2, 0, 0], option, result)
230
+ assert result.hasHit()
231
+ assert len(result.mRayHits) == 1
232
+ ray_hit = result.mRayHits[0]
233
+ assert np.isclose(ray_hit.mPoint, [1, 0, 0]).all()
234
+ assert np.isclose(ray_hit.mNormal, [1, 0, 0]).all()
235
+ assert ray_hit.mFraction == pytest.approx(0.25)
236
+
237
+ result.clear()
238
+ simple_frame.setTranslation([1, 0, 0])
239
+ assert group.raycast([-2, 0, 0], [2, 0, 0], option, result)
240
+ assert result.hasHit()
241
+ assert len(result.mRayHits) == 1
242
+ ray_hit = result.mRayHits[0]
243
+ assert np.isclose(ray_hit.mPoint, [0, 0, 0]).all()
244
+ assert np.isclose(ray_hit.mNormal, [-1, 0, 0]).all()
245
+ assert ray_hit.mFraction == pytest.approx(0.5)
246
+
247
+
248
+ if __name__ == "__main__":
249
+ pytest.main()
File without changes
@@ -0,0 +1,20 @@
1
+ import dartpy as dart
2
+ import pytest
3
+
4
+
5
+ def test_basics():
6
+ dart.common.trace("trace log")
7
+ dart.common.debug("debug log")
8
+ dart.common.info("info log")
9
+ dart.common.warn("warn log")
10
+ dart.common.error("error log")
11
+ dart.common.fatal("fatal log")
12
+
13
+
14
+ def test_arguments():
15
+ val = 10
16
+ dart.common.info("Log with param '{}' and '{}'".format(1, val))
17
+
18
+
19
+ if __name__ == "__main__":
20
+ pytest.main()
@@ -0,0 +1,50 @@
1
+ import dartpy as dart
2
+ import pytest
3
+
4
+
5
+ def test_basics():
6
+ sw = dart.common.Stopwatch()
7
+
8
+ # Stopwatch is started by default
9
+ assert sw.isStarted()
10
+ assert dart.common.Stopwatch(True).isStarted()
11
+ assert not dart.common.Stopwatch(False).isStarted()
12
+
13
+ # Stop the stopwatch
14
+ sw.stop()
15
+ assert not sw.isStarted()
16
+
17
+ # Elapsed time should be the same
18
+ elapsed1 = sw.elapsedS()
19
+ assert elapsed1 == sw.elapsedS()
20
+ assert elapsed1 == sw.elapsedS()
21
+
22
+ # Elapsed time monotonically increase while the stopwatch is running
23
+ sw.start()
24
+ assert sw.elapsedS() >= elapsed1
25
+ assert sw.elapsedS() >= elapsed1
26
+
27
+ # Starting a stopwatch already started doesn't have any effect
28
+ sw.start()
29
+ assert sw.isStarted()
30
+
31
+ # Restting a started stopwatch resets the elapsed time but doesn't stop the
32
+ # stopwatch
33
+ sw.start()
34
+ sw.reset()
35
+ assert sw.isStarted()
36
+ assert sw.elapsedS() >= 0.0
37
+ assert sw.elapsedS() >= 0.0
38
+
39
+ # Restting a stopped stopwatch resets the elapsed time but doesn't start the
40
+ # stopwatch
41
+ sw.stop()
42
+ sw.reset()
43
+ assert not sw.isStarted()
44
+ assert sw.elapsedS() == pytest.approx(0.0)
45
+
46
+ sw.print()
47
+
48
+
49
+ if __name__ == "__main__":
50
+ pytest.main()
@@ -0,0 +1,42 @@
1
+ # Copyright (c) 2011-2024, The DART development contributors
2
+ # All rights reserved.
3
+ #
4
+ # The list of contributors can be found at:
5
+ # https://github.com/dartsim/dart/blob/main/LICENSE
6
+ #
7
+ # This file is provided under the "BSD-style" License
8
+
9
+ import dartpy as dart
10
+ import pytest
11
+
12
+
13
+ def test_case_conversions():
14
+ assert dart.common.toUpper("to UppEr") == "TO UPPER"
15
+ assert dart.common.toLower("to LowEr") == "to lower"
16
+
17
+
18
+ def test_trim():
19
+ assert dart.common.trimLeft(" trim ThIs ") == "trim ThIs "
20
+ assert dart.common.trimRight(" trim ThIs ") == " trim ThIs"
21
+ assert dart.common.trim(" trim ThIs ") == "trim ThIs"
22
+
23
+ assert dart.common.trimLeft("\n trim ThIs ", " ") == "\n trim ThIs "
24
+ assert dart.common.trimLeft("\n trim ThIs ", "\n") == " trim ThIs "
25
+ assert dart.common.trimRight(" trim ThIs \n", " ") == " trim ThIs \n"
26
+ assert dart.common.trimRight(" trim ThIs \n", "\n") == " trim ThIs "
27
+ assert dart.common.trim("\n trim ThIs \n", " ") == "\n trim ThIs \n"
28
+ assert dart.common.trim("\n trim ThIs \n", "\n") == " trim ThIs "
29
+
30
+ assert dart.common.trimLeft("\n trim ThIs \n", " \n") == "trim ThIs \n"
31
+ assert dart.common.trimRight("\n trim ThIs \n", " \n") == "\n trim ThIs"
32
+ assert dart.common.trim("\n trim ThIs \n", " \n") == "trim ThIs"
33
+
34
+
35
+ def test_split():
36
+ assert len(dart.common.split(" trim ThIs ")) == 2
37
+ assert dart.common.split(" trim ThIs ")[0] == "trim"
38
+ assert dart.common.split(" trim ThIs ")[1] == "ThIs"
39
+
40
+
41
+ if __name__ == "__main__":
42
+ pytest.main()
@@ -0,0 +1,20 @@
1
+ import platform
2
+
3
+ import pytest
4
+ from dartpy.common import Uri
5
+
6
+
7
+ def test_from_string_valid_uri_returns_true():
8
+ uri = Uri()
9
+ assert uri.fromString("ftp://ftp.is.co.za/rfc/rfc1808.txt") is True
10
+ assert uri.fromString("http://www.ietf.org/rfc/rfc2396.txt") is True
11
+ assert uri.fromString("ldap://[2001:db8::7]/c=GB?objectClass?one") is True
12
+ assert uri.fromString("mailto:John.Doe@example.com") is True
13
+ assert uri.fromString("news:comp.infosystems.www.servers.unix") is True
14
+ assert uri.fromString("tel:+1-816-555-1212") is True
15
+ assert uri.fromString("telnet://192.0.2.16:80/") is True
16
+ assert uri.fromString("urn:oasis:names:specification:docbook:dtd:xml:4.1.2") is True
17
+
18
+
19
+ if __name__ == "__main__":
20
+ pytest.main()
File without changes
@@ -0,0 +1,42 @@
1
+ import platform
2
+
3
+ import dartpy as dart
4
+ import numpy as np
5
+ import pytest
6
+
7
+
8
+ def test_ball_joint_constraint():
9
+ world = dart.utils.SkelParser.readWorld("dart://sample/skel/chain.skel")
10
+ world.setGravity([0, -9.81, 0])
11
+ world.setTimeStep(1.0 / 2000)
12
+
13
+ # Set joint damping
14
+ chain = world.getSkeleton(0)
15
+ for i in range(chain.getNumJoints()):
16
+ joint = chain.getJoint(i)
17
+ for j in range(joint.getNumDofs()):
18
+ joint.setDampingCoefficient(j, 0.01)
19
+
20
+ # Create a ball joint contraint
21
+ bd1 = chain.getBodyNode("link 6")
22
+ bd2 = chain.getBodyNode("link 10")
23
+ offset1 = [0, 0.025, 0]
24
+ joint_pos = bd1.getTransform().multiply(offset1)
25
+ offset2 = bd2.getTransform().inverse().multiply(joint_pos)
26
+ constraint = dart.constraint.BallJointConstraint(bd1, bd2, joint_pos)
27
+ assert constraint.getType() == dart.constraint.BallJointConstraint.getStaticType()
28
+
29
+ # Add ball joint constraint to the constraint solver
30
+ constraint_solver = world.getConstraintSolver()
31
+ constraint_solver.addConstraint(constraint)
32
+
33
+ # Check if the ball joint constraint is being satisfied
34
+ for _ in range(100):
35
+ world.step()
36
+ pos1 = bd1.getTransform().multiply(offset1)
37
+ pos2 = bd2.getTransform().multiply(offset2)
38
+ assert np.isclose(pos1, pos2).all()
39
+
40
+
41
+ if __name__ == "__main__":
42
+ pytest.main()
File without changes
@@ -0,0 +1,15 @@
1
+ import dartpy as dart
2
+ import pytest
3
+
4
+
5
+ def test_simple_frame():
6
+ shape_frame = dart.dynamics.SimpleFrame()
7
+ assert not shape_frame.hasVisualAspect()
8
+ assert shape_frame.getVisualAspect() is None
9
+ assert shape_frame.getVisualAspect(False) is None
10
+ visual = shape_frame.createVisualAspect()
11
+ assert visual is not None
12
+
13
+
14
+ if __name__ == "__main__":
15
+ pytest.main()
@@ -0,0 +1,65 @@
1
+ import platform
2
+
3
+ import dartpy as dart
4
+ import numpy as np
5
+ import pytest
6
+
7
+
8
+ def test_basic():
9
+ urdfParser = dart.utils.DartLoader()
10
+ kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
11
+ assert kr5 is not None
12
+
13
+ for i in range(kr5.getNumBodyNodes()):
14
+ body = kr5.getBodyNode(i)
15
+ body_force = body.getBodyForce()
16
+ assert body_force.size == 6
17
+ bodyPtr = body.getBodyNodePtr()
18
+ assert body == bodyPtr
19
+ assert body.getName() == bodyPtr.getName()
20
+ assert np.array_equal(np.array(body.getSpatialVelocity()), np.zeros(6)) is True
21
+ shape_nodes = body.getShapeNodes()
22
+ for shape_node in shape_nodes:
23
+ print(shape_node)
24
+ if shape_node.hasVisualAspect():
25
+ visual = shape_node.getVisualAspect()
26
+ visual.getRGBA()
27
+ if shape_node.hasCollisionAspect():
28
+ collision = shape_node.getCollisionAspect()
29
+ if shape_node.hasDynamicsAspect():
30
+ dynamics = shape_node.getDynamicsAspect()
31
+
32
+
33
+ def test_get_child_methods():
34
+ urdfParser = dart.utils.DartLoader()
35
+ kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
36
+ assert kr5 is not None
37
+
38
+ currentBodyNode = kr5.getRootBodyNode()
39
+ assert currentBodyNode is not None
40
+
41
+ for i in range(1, kr5.getNumBodyNodes()):
42
+ childBodyNode = currentBodyNode.getChildBodyNode(0)
43
+ childJoint = currentBodyNode.getChildJoint(0)
44
+
45
+ assert childBodyNode is not None
46
+ assert childJoint is not None
47
+ assert childBodyNode.getName() == kr5.getBodyNode(i).getName()
48
+ assert childJoint.getName() == kr5.getJoint(i).getName()
49
+
50
+ currentBodyNode = childBodyNode
51
+
52
+
53
+ def test_get_inertia():
54
+ urdfParser = dart.utils.DartLoader()
55
+ kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
56
+ assert kr5 is not None
57
+
58
+ inertias = [
59
+ kr5.getBodyNode(i).getInertia() for i in range(1, kr5.getNumBodyNodes())
60
+ ]
61
+ assert all([inertia is not None for inertia in inertias])
62
+
63
+
64
+ if __name__ == "__main__":
65
+ pytest.main()
@@ -0,0 +1,76 @@
1
+ import math
2
+ import platform
3
+
4
+ import dartpy as dart
5
+ import numpy as np
6
+ import pytest
7
+
8
+
9
+ def test_inertia_init():
10
+ """
11
+ Test basic functionality for the `dartpy.dynamics.Inertia` class.
12
+ """
13
+ # test default values
14
+ i1 = dart.dynamics.Inertia()
15
+ assert i1 is not None
16
+
17
+ # initialize with parameters
18
+ i2 = dart.dynamics.Inertia(0.1, [0, 0, 0], 1.3 * np.eye(3))
19
+ assert i1 is not None
20
+
21
+ newMass = 1.5
22
+ i2.setMass(newMass)
23
+ assert i2.getMass() == newMass
24
+
25
+ newCOM = np.array((0.1, 0, 0))
26
+ i2.setLocalCOM(newCOM)
27
+ assert np.allclose(i2.getLocalCOM(), newCOM)
28
+
29
+ newMoment = 0.4 * newMass * 0.1**2 * np.eye(3)
30
+ i2.setMoment(newMoment)
31
+ assert np.allclose(i2.getMoment(), newMoment)
32
+
33
+ i2.setSpatialTensor(0.3 * i2.getSpatialTensor())
34
+
35
+ assert i2.verify()
36
+
37
+ for i in range(10): # based on the C++ tests
38
+ mass = np.random.uniform(0.1, 10.0)
39
+ com = np.random.uniform(-5, 5, 3)
40
+ I = np.random.rand(3, 3) - 0.5 + np.diag(np.random.uniform(0.6, 1, 3), 0)
41
+ I = (I + I.T) / 2
42
+
43
+ inertia = dart.dynamics.Inertia(mass, com, I)
44
+ assert inertia.verify()
45
+
46
+
47
+ def test_inertia_static_methods():
48
+ """
49
+ Test the class methods `verifyMoment`and `verifySpatialTensor`.
50
+ """
51
+ assert dart.dynamics.Inertia.verifyMoment(np.eye(3), printWarnings=False)
52
+ for i in range(10):
53
+ I = np.random.rand(3, 3) - 0.5 + np.diag(np.random.uniform(1, 10, 3), 0)
54
+ I = (I + I.T) / 2
55
+ assert dart.dynamics.Inertia.verifyMoment(I)
56
+
57
+ assert dart.dynamics.Inertia.verifySpatialTensor(np.eye(6), printWarnings=False)
58
+
59
+
60
+ def test_failing_moment_and_spatial():
61
+ """
62
+ Test some failure cases of the verify methods.
63
+ """
64
+
65
+ for i in range(10):
66
+ I = np.random.rand(3, 3) - 0.5 - np.diag(np.random.uniform(1, 10, 3), 0)
67
+ assert not dart.dynamics.Inertia.verifyMoment(I, printWarnings=False)
68
+
69
+ # fails e.g. due to off diagonal values in translational part.
70
+ assert not dart.dynamics.Inertia.verifySpatialTensor(
71
+ np.random.rand(6, 6), printWarnings=False
72
+ )
73
+
74
+
75
+ if __name__ == "__main__":
76
+ pytest.main()
@@ -0,0 +1,98 @@
1
+ import math
2
+ import platform
3
+
4
+ import dartpy as dart
5
+ import numpy as np
6
+ import pytest
7
+
8
+
9
+ def test_solve_for_free_joint():
10
+ """
11
+ Very simple test of InverseKinematics module, applied to a FreeJoint to
12
+ ensure that the target is reachable
13
+ """
14
+
15
+ skel = dart.dynamics.Skeleton()
16
+ [joint0, body0] = skel.createFreeJointAndBodyNodePair()
17
+
18
+ ik = body0.getOrCreateIK()
19
+ assert ik.isActive()
20
+
21
+ tf = dart.math.Isometry3()
22
+ tf.set_translation([0, 0, 0.8])
23
+ tf.set_rotation(dart.math.AngleAxis(math.pi / 8.0, [0, 1, 0]).to_rotation_matrix())
24
+ ik.getTarget().setTransform(tf)
25
+
26
+ error_method = ik.getErrorMethod()
27
+ assert error_method.getMethodName() == "TaskSpaceRegion"
28
+ [lb, ub] = error_method.getBounds()
29
+ assert len(lb) is 6
30
+ assert len(ub) is 6
31
+ error_method.setBounds(np.ones(6) * -1e-8, np.ones(6) * 1e-8)
32
+ [lb, ub] = error_method.getBounds()
33
+ assert lb == pytest.approx(-1e-8)
34
+ assert ub == pytest.approx(1e-8)
35
+
36
+ solver = ik.getSolver()
37
+ solver.setNumMaxIterations(100)
38
+
39
+ prob = ik.getProblem()
40
+
41
+ tf_actual = ik.getTarget().getTransform().matrix()
42
+ tf_expected = body0.getTransform().matrix()
43
+ assert not np.isclose(tf_actual, tf_expected).all()
44
+
45
+ success = solver.solve()
46
+ assert success
47
+
48
+ tf_actual = ik.getTarget().getTransform().matrix()
49
+ tf_expected = body0.getTransform().matrix()
50
+ assert np.isclose(tf_actual, tf_expected).all()
51
+
52
+
53
+ class FailingSolver(dart.optimizer.Solver):
54
+ def __init__(self, constant):
55
+ super(FailingSolver, self).__init__()
56
+ self.constant = constant
57
+
58
+ def solve(self):
59
+ problem = self.getProblem()
60
+ if problem is None:
61
+ print(
62
+ "[FailingSolver::solve] Attempting to solve a nullptr problem! We will return false."
63
+ )
64
+ return False
65
+
66
+ dim = problem.getDimension()
67
+ wrong_solution = np.ones(dim) * self.constant
68
+ problem.setOptimalSolution(wrong_solution)
69
+
70
+ return False
71
+
72
+ def getType(self):
73
+ return "FailingSolver"
74
+
75
+ def clone(self):
76
+ return FailingSolver(self.constant)
77
+
78
+
79
+ def test_do_not_apply_solution_on_failure():
80
+ skel = dart.dynamics.Skeleton()
81
+ [joint, body] = skel.createFreeJointAndBodyNodePair()
82
+
83
+ ik = body.getIK(True)
84
+ solver = FailingSolver(10)
85
+ ik.setSolver(solver)
86
+
87
+ dofs = skel.getNumDofs()
88
+ skel.resetPositions()
89
+
90
+ assert not ik.solveAndApply(allowIncompleteResult=False)
91
+ assert np.isclose(skel.getPositions(), np.zeros(dofs)).all()
92
+
93
+ assert not ik.solveAndApply(allowIncompleteResult=True)
94
+ assert not np.isclose(skel.getPositions(), np.zeros(dofs)).all()
95
+
96
+
97
+ if __name__ == "__main__":
98
+ pytest.main()