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.
- dartpy-0.2.0.post107.dist-info/LICENSE +31 -0
- dartpy-0.2.0.post107.dist-info/METADATA +47 -0
- dartpy-0.2.0.post107.dist-info/RECORD +64 -0
- dartpy-0.2.0.post107.dist-info/WHEEL +5 -0
- dartpy-0.2.0.post107.dist-info/top_level.txt +2 -0
- dartpy.cpython-311-x86_64-linux-gnu.so +0 -0
- dartpy.libs/libGLU-ff1477cc.so.1.3.1 +0 -0
- dartpy.libs/libOpenThreads-0250ab5c.so.3.3.1 +0 -0
- dartpy.libs/libXi-9178a6bd.so.6.1.0 +0 -0
- dartpy.libs/libXxf86vm-8446c95f.so.1.0.0 +0 -0
- dartpy.libs/libassimp-79671a85.so.5.2.4 +0 -0
- dartpy.libs/libccd-f7023733.so.2.0 +0 -0
- dartpy.libs/libconsole_bridge-757d7af3.so.1.0 +0 -0
- dartpy.libs/libfcl-fafce69a.so.0.7.0 +0 -0
- dartpy.libs/libglut-0c8fd390.so.3.10.0 +0 -0
- dartpy.libs/liboctomap-0c5b15c6.so.1.9.6 +0 -0
- dartpy.libs/liboctomath-31ce0045.so.1.9.6 +0 -0
- dartpy.libs/libosg-b45f9cbc.so.3.7.0 +0 -0
- dartpy.libs/libosgDB-209e5143.so.3.7.0 +0 -0
- dartpy.libs/libosgGA-260d5c6e.so.3.7.0 +0 -0
- dartpy.libs/libosgManipulator-05bcdf02.so.3.7.0 +0 -0
- dartpy.libs/libosgShadow-9971a879.so.3.7.0 +0 -0
- dartpy.libs/libosgText-16341674.so.3.7.0 +0 -0
- dartpy.libs/libosgUtil-e5d05863.so.3.7.0 +0 -0
- dartpy.libs/libosgViewer-7dd0f485.so.3.7.0 +0 -0
- dartpy.libs/libtinyxml-296537c6.so +0 -0
- dartpy.libs/liburdfdom_model-6cad1cb6.so.3.0 +0 -0
- dartpy.libs/liburdfdom_model_state-36524f5e.so.3.0 +0 -0
- dartpy.libs/liburdfdom_sensor-ad2c126c.so.3.0 +0 -0
- dartpy.libs/liburdfdom_world-538ea8a8.so.3.0 +0 -0
- tests/__init__.py +0 -0
- tests/integration/__init__.py +0 -0
- tests/integration/test_joint_force_torque.py +305 -0
- tests/unit/__init__.py +0 -0
- tests/unit/collision/__init__.py +0 -0
- tests/unit/collision/test_collision.py +249 -0
- tests/unit/common/__init__.py +0 -0
- tests/unit/common/test_logging.py +20 -0
- tests/unit/common/test_stopwatch.py +50 -0
- tests/unit/common/test_string.py +42 -0
- tests/unit/common/test_uri.py +20 -0
- tests/unit/constraint/__init__.py +0 -0
- tests/unit/constraint/test_constraint.py +42 -0
- tests/unit/dynamics/__init__.py +0 -0
- tests/unit/dynamics/test_aspect.py +15 -0
- tests/unit/dynamics/test_body_node.py +65 -0
- tests/unit/dynamics/test_inertia.py +76 -0
- tests/unit/dynamics/test_inverse_kinematics.py +98 -0
- tests/unit/dynamics/test_joint.py +149 -0
- tests/unit/dynamics/test_meta_skeleton.py +41 -0
- tests/unit/dynamics/test_simple_frame.py +54 -0
- tests/unit/dynamics/test_skeleton.py +23 -0
- tests/unit/math/__init__.py +0 -0
- tests/unit/math/test_random.py +40 -0
- tests/unit/optimizer/__init__.py +0 -0
- tests/unit/optimizer/test_optimizer.py +93 -0
- tests/unit/simulation/__init__.py +0 -0
- tests/unit/simulation/test_world.py +45 -0
- tests/unit/utils/__init__.py +0 -0
- tests/unit/utils/test_dart_loader.py +72 -0
- tests/unit/utils/test_mjcf_parser.py +19 -0
- tests/unit/utils/test_sdf_parser.py +21 -0
- tests/unit/utils/test_skel_parser.py +22 -0
- 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()
|