dartpy 0.1.0.post107__cp310-cp310-manylinux_2_28_x86_64.whl

tests/unit/dynamics/test_joint.py

@@ -0,0 +1,149 @@
+import math
+
+import dartpy as dart
+import numpy as np
+import pytest
+
+
+def kinematics_tester(joint):
+    num_tests = 2
+
+    joint.setTransformFromChildBodyNode(dart.math.expMap(np.random.rand(6)))
+    joint.setTransformFromParentBodyNode(dart.math.expMap(np.random.rand(6)))
+
+    dof = joint.getNumDofs()
+
+    q = np.zeros(dof)
+    dq = np.zeros(dof)
+
+    for _ in range(num_tests):
+        q_delta = 1e-5
+
+        for i in range(dof):
+            q[i] = dart.math.Random.uniform(-math.pi, math.pi)
+            dq[i] = dart.math.Random.uniform(-math.pi, math.pi)
+
+        joint.setPositions(q)
+        joint.setVelocities(dq)
+
+        if dof == 0:
+            return
+
+        T = joint.getRelativeTransform()
+        J = joint.getRelativeJacobian(q)
+        dJ = joint.getRelativeJacobianTimeDeriv()
+
+        # Verify transform
+        assert dart.math.verifyTransform(T)
+
+        # Test analytic Jacobian and numerical Jacobian
+        numericJ = np.zeros((6, dof))
+        for i in range(dof):
+            q_a = q.copy()
+            joint.setPositions(q_a)
+            T_a = joint.getRelativeTransform()
+
+            q_b = q.copy()
+            q_b[i] += q_delta
+            joint.setPositions(q_b)
+            T_b = joint.getRelativeTransform()
+
+            Tinv_a = T_a.inverse()
+
+            dTdq = (T_b.matrix() - T_a.matrix()) / q_delta
+
+            Ji_4x4matrix = np.matmul(Tinv_a.matrix(), dTdq)
+            Ji = np.zeros(6)
+            Ji[0] = Ji_4x4matrix[2, 1]
+            Ji[1] = Ji_4x4matrix[0, 2]
+            Ji[2] = Ji_4x4matrix[1, 0]
+            Ji[3] = Ji_4x4matrix[0, 3]
+            Ji[4] = Ji_4x4matrix[1, 3]
+            Ji[5] = Ji_4x4matrix[2, 3]
+            numericJ[:, i] = Ji
+
+        assert np.allclose(J, numericJ, atol=1e-5)
+
+
+def test_kinematics():
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createWeldJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createRevoluteJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createPrismaticJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createScrewJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createUniversalJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createTranslationalJoint2DAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createEulerJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createTranslationalJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createPlanarJointAndBodyNodePair()
+    kinematics_tester(joint)
+
+
+def test_access_to_parent_child_transforms():
+    skel = dart.dynamics.Skeleton()
+    joint, _ = skel.createRevoluteJointAndBodyNodePair()
+
+    parentToJointTf = dart.math.Isometry3.Identity()
+    parentToJointTf.set_translation(np.random.rand(3, 1))
+    childToJointTf = dart.math.Isometry3.Identity()
+    childToJointTf.set_translation(np.random.rand(3, 1))
+
+    joint.setTransformFromParentBodyNode(parentToJointTf)
+    joint.setTransformFromChildBodyNode(childToJointTf)
+
+    storedParentTf = joint.getTransformFromParentBodyNode()
+    storedChildTf = joint.getTransformFromChildBodyNode()
+
+    assert np.allclose(parentToJointTf.matrix(), storedParentTf.matrix())
+    assert np.allclose(childToJointTf.matrix(), storedChildTf.matrix())
+
+
+def test_BallJoint_positions_conversion():
+    assert np.allclose(
+        dart.dynamics.BallJoint.convertToPositions(np.eye(3)), np.zeros((1, 3))
+    )
+    assert np.allclose(
+        dart.dynamics.BallJoint.convertToPositions(
+            np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])
+        ),
+        np.array([0, 0, -np.pi / 2]),
+    )
+
+    for i in range(30):
+        ballJointPos = np.random.uniform(-np.pi / 2, np.pi / 2, 3)
+        assert np.allclose(
+            dart.dynamics.BallJoint.convertToRotation(
+                dart.dynamics.BallJoint.convertToPositions(
+                    dart.dynamics.BallJoint.convertToRotation(ballJointPos)
+                )
+            ),
+            dart.dynamics.BallJoint.convertToRotation(ballJointPos),
+        )
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/dynamics/test_meta_skeleton.py

@@ -0,0 +1,41 @@
+import platform
+
+import dartpy as dart
+import pytest
+
+# TODO(JS): Move this to integration category once created
+
+
+def test_basic():
+    urdfParser = dart.utils.DartLoader()
+    kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
+    assert kr5 is not None
+
+    shoulder = kr5.getBodyNode("shoulder")
+    assert shoulder is not None
+
+    elbow = kr5.getBodyNode("elbow")
+    assert elbow is not None
+
+    chain1 = dart.dynamics.Chain(shoulder, elbow, False, "midchain")
+    assert chain1 is not None
+    assert chain1.getNumBodyNodes() is 2
+
+    chain2 = dart.dynamics.Chain(shoulder, elbow, True, "midchain")
+    assert chain2 is not None
+    assert chain2.getNumBodyNodes() is 3
+
+    assert len(kr5.getPositions()) is not 0
+    assert kr5.getNumJoints() is not 0
+    assert kr5.getRootJoint() is not None
+    assert len(kr5.getRootJoint().getPositions()) is 0
+
+    rootBody = kr5.getBodyNode(0)
+    assert rootBody is not None
+
+    rootJoint = kr5.getJoint(0)
+    assert rootJoint is not None
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/dynamics/test_simple_frame.py

@@ -0,0 +1,54 @@
+import platform
+
+import dartpy as dart
+import numpy as np
+import pytest
+
+
+def test_basic():
+    world_frame = dart.dynamics.Frame.World()
+    assert world_frame.isWorld()
+
+    frame1 = dart.dynamics.SimpleFrame()
+    assert not frame1.isWorld()
+    assert not frame1.isShapeNode()
+    assert frame1.isShapeFrame()
+
+    assert frame1.getTransform().translation()[0] == pytest.approx(0)
+    assert frame1.getTransform().translation()[1] == pytest.approx(0)
+    assert frame1.getTransform().translation()[2] == pytest.approx(0)
+
+    frame1.setTranslation([1, 2, 3])
+    assert frame1.getTransform().translation()[0] == pytest.approx(1)
+    assert frame1.getTransform().translation()[1] == pytest.approx(2)
+    assert frame1.getTransform().translation()[2] == pytest.approx(3)
+
+    assert frame1.getParentFrame().isWorld()
+    assert frame1.descendsFrom(None)
+    assert frame1.descendsFrom(world_frame)
+    assert frame1.descendsFrom(frame1)
+
+    frame2 = frame1.spawnChildSimpleFrame()
+    assert not frame2.isWorld()
+    assert frame2.descendsFrom(None)
+    assert frame2.descendsFrom(world_frame)
+    assert frame2.descendsFrom(frame1)
+    assert frame2.descendsFrom(frame2)
+
+    assert frame2.getTransform().translation()[0] == pytest.approx(1)
+    assert frame2.getTransform().translation()[1] == pytest.approx(2)
+    assert frame2.getTransform().translation()[2] == pytest.approx(3)
+
+    frame2.setRelativeTranslation([1, 2, 3])
+    assert frame2.getTransform().translation()[0] == pytest.approx(2)
+    assert frame2.getTransform().translation()[1] == pytest.approx(4)
+    assert frame2.getTransform().translation()[2] == pytest.approx(6)
+
+    frame2.setTranslation([1, 2, 3])
+    assert frame2.getTransform().translation()[0] == pytest.approx(1)
+    assert frame2.getTransform().translation()[1] == pytest.approx(2)
+    assert frame2.getTransform().translation()[2] == pytest.approx(3)
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/dynamics/test_skeleton.py

@@ -0,0 +1,23 @@
+import platform
+
+import dartpy as dart
+import numpy as np
+import pytest
+
+
+def test_basic():
+    skel = dart.dynamics.Skeleton()
+
+    joint_prop = dart.dynamics.FreeJointProperties()
+    joint_prop.mName = "joint0"
+    assert joint_prop.mName == "joint0"
+
+    [joint1, body1] = skel.createFreeJointAndBodyNodePair(None, joint_prop)
+    assert joint1.getType() == "FreeJoint"
+    assert joint1.getName() == "joint0"
+
+    assert skel.getBodyNodes()[0].getName() == body1.getName()
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/math/test_random.py

@@ -0,0 +1,40 @@
+import platform
+
+import pytest
+from dartpy.math import Random
+
+
+def test_create():
+    rand = Random()
+
+
+def test_seed():
+    rand = Random()
+
+    N = 10
+
+    min = -10
+    max = 10
+
+    first = []
+    second = []
+    third = []
+
+    tol = 1e-6
+
+    for i in range(N):
+        Random.setSeed(i)
+        first.append(Random.uniform(min, max))
+        second.append(Random.uniform(min, max))
+        third.append(Random.uniform(min, max))
+
+    for i in range(N):
+        Random.setSeed(i)
+        assert Random.getSeed() is i
+        assert Random.uniform(min, max) == pytest.approx(first[i], tol)
+        assert Random.uniform(min, max) == pytest.approx(second[i], tol)
+        assert Random.uniform(min, max) == pytest.approx(third[i], tol)
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/optimizer/test_optimizer.py

@@ -0,0 +1,93 @@
+import math
+
+import dartpy as dart
+import pytest
+
+
+# Problem reference: http://ab-initio.mit.edu/wiki/index.php/NLopt_Tutorial
+class SampleObjFunc(dart.optimizer.Function):
+    def eval(self, x):
+        return math.sqrt(x[1])
+
+    def evalGradient(self, x, grad):
+        grad[0] = 0
+        grad[1] = 0.5 / (math.sqrt(x[1]) + 0.000001)
+
+
+class SampleConstFunc(dart.optimizer.Function):
+    def __init__(self, a, b):
+        super(SampleConstFunc, self).__init__()
+        self.a = a
+        self.b = b
+
+    def eval(self, x):
+        return (self.a * x[0] + self.b) * (self.a * x[0] + self.b) * (
+            self.a * x[0] + self.b
+        ) - x[1]
+
+    def evalGradient(self, x, grad):
+        grad[0] = 3 * self.a * (self.a * x[0] + self.b) * (self.a * x[0] + self.b)
+        grad[1] = -1.0
+
+
+def test_gradient_descent_solver():
+    prob = dart.optimizer.Problem(2)
+    assert prob.getDimension() is 2
+
+    prob.setLowerBounds([-1e100, 0])
+    prob.setInitialGuess([1.234, 5.678])
+
+    assert prob.getInitialGuess()[0] == pytest.approx(1.234)
+
+    obj = SampleObjFunc()
+    prob.setObjective(obj)
+
+    solver = dart.optimizer.GradientDescentSolver(prob)
+    success = solver.solve()
+    assert success is True
+
+    min_f = prob.getOptimumValue()
+    opt_x = prob.getOptimalSolution()
+
+    assert min_f == pytest.approx(0, 1e-6)
+    assert len(opt_x) == prob.getDimension()
+    assert opt_x[0] == pytest.approx(1.234, 0.0)
+    assert opt_x[1] == pytest.approx(0, solver.getTolerance())
+
+
+def test_nlopt_solver():
+    if not hasattr(dart.optimizer, "NloptSolver"):
+        return
+
+    prob = dart.optimizer.Problem(2)
+    assert prob.getDimension() is 2
+
+    prob.setLowerBounds([-1e100, 0])
+    prob.setInitialGuess([1.234, 5.678])
+
+    assert prob.getInitialGuess()[0] == pytest.approx(1.234)
+
+    obj = SampleObjFunc()
+    prob.setObjective(obj)
+
+    const1 = SampleConstFunc(2, 0)
+    const2 = SampleConstFunc(-1, 1)
+    prob.addIneqConstraint(const1)
+    prob.addIneqConstraint(const2)
+
+    solver = dart.optimizer.NloptSolver(prob)
+    solver.setAlgorithm(dart.optimizer.NloptSolver.Algorithm.LD_MMA)
+    success = solver.solve()
+    assert success is True
+
+    min_f = prob.getOptimumValue()
+    opt_x = prob.getOptimalSolution()
+
+    assert min_f == pytest.approx(0.544330847, 1e-5)
+    assert len(opt_x) == prob.getDimension()
+    assert opt_x[0] == pytest.approx(0.333334, 1e-5)
+    assert opt_x[1] == pytest.approx(0.296296, 1e-5)
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/simulation/test_world.py

@@ -0,0 +1,45 @@
+import platform
+
+import dartpy as dart
+import pytest
+
+
+def test_empty_world():
+    world = dart.simulation.World("my world")
+    assert world.getNumSkeletons() is 0
+    assert world.getNumSimpleFrames() is 0
+
+
+def test_collision_detector_change():
+    world = dart.simulation.World("world")
+    solver = world.getConstraintSolver()
+    assert solver is not None
+
+    assert (
+        solver.getCollisionDetector().getType()
+        == dart.collision.FCLCollisionDetector().getStaticType()
+    )
+
+    solver.setCollisionDetector(dart.collision.DARTCollisionDetector())
+    assert (
+        solver.getCollisionDetector().getType()
+        == dart.collision.DARTCollisionDetector().getStaticType()
+    )
+
+    if hasattr(dart.collision, "BulletCollisionDetector"):
+        solver.setCollisionDetector(dart.collision.BulletCollisionDetector())
+        assert (
+            solver.getCollisionDetector().getType()
+            == dart.collision.BulletCollisionDetector().getStaticType()
+        )
+
+    if hasattr(dart.collision, "OdeCollisionDetector"):
+        solver.setCollisionDetector(dart.collision.OdeCollisionDetector())
+        assert (
+            solver.getCollisionDetector().getType()
+            == dart.collision.OdeCollisionDetector().getStaticType()
+        )
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/utils/test_dart_loader.py

@@ -0,0 +1,72 @@
+import os
+import platform
+
+import dartpy
+import pytest
+from dartpy.utils import DartLoader
+from tests.util import get_asset_path
+
+
+def test_parse_skeleton_non_existing_path_returns_null():
+    assert os.path.isfile(get_asset_path("skel/cubes.skel")) is True
+    loader = DartLoader()
+    assert loader.parseSkeleton(get_asset_path("skel/test/does_not_exist.urdf")) is None
+
+
+def test_parse_skeleton_invalid_urdf_returns_null():
+    loader = DartLoader()
+    assert loader.parseSkeleton(get_asset_path("urdf/invalid.urdf")) is None
+
+
+def test_parse_skeleton_missing_mesh_returns_null():
+    loader = DartLoader()
+    assert loader.parseSkeleton(get_asset_path("urdf/missing_mesh.urdf")) is None
+
+
+def test_parse_skeleton_invalid_mesh_returns_null():
+    loader = DartLoader()
+    assert loader.parseSkeleton(get_asset_path("urdf/invalid_mesh.urdf")) is None
+
+
+def test_parse_skeleton_missing_package_returns_null():
+    loader = DartLoader()
+    assert loader.parseSkeleton(get_asset_path("urdf/missing_package.urdf")) is None
+
+
+def test_parse_skeleton_loads_primitive_geometry():
+    loader = DartLoader()
+    assert (
+        loader.parseSkeleton(get_asset_path("urdf/test/primitive_geometry.urdf"))
+        is not None
+    )
+
+
+# Failing with following errors:
+# TypeError: No to_python (by-value) converter found for C++ type: std::shared_ptr<dart::simulation::World>
+#
+# def test_parse_world():
+#     loader = DartLoader()
+#     assert loader.parseWorld(get_asset_path('urdf/testWorld.urdf')) is not None
+
+
+def test_parse_joint_properties():
+    loader = DartLoader()
+    robot = loader.parseSkeleton(get_asset_path("urdf/test/joint_properties.urdf"))
+    assert robot is not None
+
+
+#    joint1 = robot.getJoint(1)
+#    assert joint1 is not None
+#    assert joint1.getDampingCoefficient(0) == pytest.approx(1.2, 1e-12)
+#    assert joint1.getCoulombFriction(0) == pytest.approx(2.3, 1e-12)
+
+#    joint2 = robot.getJoint(2)
+#    assert joint2 is not None
+#    assert joint2.getPositionLowerLimit(0) == -float("inf")
+#    assert joint2.getPositionUpperLimit(0) == float("inf")
+#    if not platform.linux_distribution()[1] == '14.04':
+#        assert joint2.isCyclic(0)
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/utils/test_mjcf_parser.py

@@ -0,0 +1,19 @@
+import os
+import platform
+
+import dartpy as dart
+import pytest
+from tests.util import get_asset_path
+
+
+def test_parse_fetch():
+    assert (
+        dart.utils.MjcfParser.readWorld(
+            "dart://sample/mjcf/openai/robotics/fetch/pick_and_place.xml"
+        )
+        is not None
+    )
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/utils/test_sdf_parser.py

@@ -0,0 +1,21 @@
+# Copyright (c) 2011-2024, The DART development contributors
+# All rights reserved.
+#
+# The list of contributors can be found at:
+#   https://github.com/dartsim/dart/blob/main/LICENSE
+#
+# This file is provided under the "BSD-style" License
+
+import dartpy as dart
+import pytest
+
+
+def test_read_world():
+    assert (
+        dart.utils.SdfParser.readWorld("dart://sample/sdf/double_pendulum.world")
+        is not None
+    )
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/utils/test_skel_parser.py

@@ -0,0 +1,22 @@
+# Copyright (c) 2011-2024, The DART development contributors
+# All rights reserved.
+#
+# The list of contributors can be found at:
+#   https://github.com/dartsim/dart/blob/main/LICENSE
+#
+# This file is provided under the "BSD-style" License
+
+import dartpy as dart
+import pytest
+
+
+def test_read_world():
+    assert dart.utils.SkelParser.readWorld("dart://sample/skel/cubes.skel") is not None
+    assert (
+        dart.utils.SkelParser.readWorld("dart://sample/skel/cubes.skel", None)
+        is not None
+    )
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/unit/v7/test_import.py

@@ -0,0 +1,11 @@
+import dartpy as dart
+import pytest
+
+
+def test_import():
+    import dartpy.v7 as v7
+    dir(v7)
+
+
+if __name__ == "__main__":
+    pytest.main()

tests/util.py

@@ -0,0 +1,18 @@
+import inspect
+import os
+import os.path
+
+
+def get_path_to_this_file():
+    return inspect.getfile(inspect.currentframe())
+
+
+def get_asset_path(rel_path, check_existing=False):
+    if rel_path.startswith("/"):
+        full_path = rel_path
+    else:
+        full_path = os.path.join(os.path.dirname(__file__), "../../data/", rel_path)
+    if check_existing and not os.path.exists(full_path):
+        raise IOError("File %s does not exist" % full_path)
+
+    return full_path