dartpy 0.1.0.post107__cp312-cp312-win_amd64.whl

dartpy-0.1.0.post107.dist-info/LICENSE

@@ -0,0 +1,31 @@
+BSD 2-Clause License
+
+Copyright (c) 2011-2016, Humanoid Lab, Georgia Tech Research Corporation
+Copyright (c) 2016-2017, Personal Robotics Lab, Carnegie Mellon University
+Copyright (c) 2011-2019, Graphics Lab, Georgia Tech Research Corporation
+Copyright (c) 2017-2019, Personal Robotics Lab, University of Washington
+Copyright (c) 2017-2019, Open Source Robotics Foundation
+Copyright (c) 2019-2024, The Movement Lab, Stanford University
+Copyright (c) 2019-2024, Jeongseok Lee
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

dartpy-0.1.0.post107.dist-info/METADATA

@@ -0,0 +1,70 @@
+Metadata-Version: 2.1
+Name: dartpy
+Version: 0.1.0.post107
+Summary: Python API of Dynamic Animation and Robotics Toolkit.
+Home-page: https://github.com/dartsim/dart.git
+Author: Jeongseok Lee
+Author-email: jslee02@gmail.com
+License: BSD 2-Clause
+Keywords: dartsim robotics
+Classifier: Development Status :: 2 - Pre-Alpha
+Classifier: Framework :: Robot Framework
+Classifier: Intended Audience :: Science/Research
+Classifier: License :: OSI Approved :: BSD License
+Classifier: Topic :: Scientific/Engineering
+Classifier: Operating System :: Microsoft :: Windows
+Classifier: Operating System :: MacOS
+Classifier: Operating System :: POSIX :: Linux
+Requires-Python: >=3.7
+Description-Content-Type: text/markdown
+License-File: LICENSE
+Requires-Dist: numpy
+Provides-Extra: test
+Requires-Dist: pytest >=6.0 ; extra == 'test'
+
+# DART
+
+
+
+<br>
+
+<p align="center">
+
+  <img src="https://raw.githubusercontent.com/dartsim/dart/master/docs/dart_logo_377x107.jpg" alt="DART: Dynamic Animation and Robotics Toolkit">
+
+</p>
+
+
+
+## Documentation
+
+
+
+For more information on DART, please visit the DART documentation: [English](https://dart.readthedocs.io/) | [한국어](https://dart-ko.readthedocs.io/) (WIP)
+
+
+
+## Project Status
+
+
+
+| Item                  | Status                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  |
+
+| --------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+
+| Build                 | [![CI Ubuntu](https://github.com/dartsim/dart/actions/workflows/ci_ubuntu.yml/badge.svg)](https://github.com/dartsim/dart/actions/workflows/ci_ubuntu.yml) [![CI macOS](https://github.com/dartsim/dart/actions/workflows/ci_macos.yml/badge.svg)](https://github.com/dartsim/dart/actions/workflows/ci_macos.yml) [![CI Windows](https://github.com/dartsim/dart/actions/workflows/ci_windows.yml/badge.svg)](https://github.com/dartsim/dart/actions/workflows/ci_windows.yml)                                                                                                                                                                                                        |
+
+| Doc, Coverage, Linter | [![API Documentation](https://github.com/dartsim/dart/actions/workflows/api_doc.yml/badge.svg)](https://github.com/dartsim/dart/actions/workflows/api_doc.yml)  [![Documentation Status](https://readthedocs.org/projects/dart/badge/?version=latest)](https://dart.readthedocs.io/en/latest/?badge=latest) [![codecov](https://codecov.io/gh/dartsim/dart/branch/main/graph/badge.svg)](https://codecov.io/gh/dartsim/dart)   [![Codacy Badge](https://app.codacy.com/project/badge/Grade/2d95a9b951be4b73a71097670ec351e8)](https://www.codacy.com/gh/dartsim/dart/dashboard?utm_source=github.com&amp;utm_medium=referral&amp;utm_content=dartsim/dart&amp;utm_campaign=Badge_Grade) |
+
+| Packages              | [![Packaging status](https://repology.org/badge/vertical-allrepos/dartsim.svg)](https://repology.org/project/dartsim/versions)    [![Publish dartpy](https://github.com/dartsim/dart/actions/workflows/publish_dartpy.yml/badge.svg)](https://github.com/dartsim/dart/actions/workflows/publish_dartpy.yml)                                                                                                                                                                                                                                                                                                                                                                             |
+
+| Maintenance           | [![Average time to resolve an issue](http://isitmaintained.com/badge/resolution/dartsim/dart.svg)](http://isitmaintained.com/project/dartsim/dart "Average time to resolve an issue") [![Percentage of issues still open](http://isitmaintained.com/badge/open/dartsim/dart.svg)](http://isitmaintained.com/project/dartsim/dart "Percentage of issues still open")                                                                                                                                                                                                                                                                                                                     |
+
+
+
+## Citation
+
+
+
+If you use DART in an academic publication, please consider citing this [JOSS Paper](https://doi.org/10.21105/joss.00500) [[BibTeX](https://gist.github.com/jslee02/998b8809e3ae1b7aef6ef04dd2ad5e27)]
+

dartpy-0.1.0.post107.dist-info/RECORD

@@ -0,0 +1,68 @@
+OpenThreads.dll,sha256=xwRI1vSevKegySdNMySXpkZQa1Ne8PZ17bSXTTeTrhI,32768
+assimp-vc143-mt.dll,sha256=qF3gmfMIuttJmISSleHE-5QjWrM93riIBcEykQ-h0EY,5400064
+brotlicommon.dll,sha256=9I4VL8B3ZwH6BGeBQAXJyahU1Cp3-csJmi3EelCSDR4,137216
+brotlidec.dll,sha256=GpXQcJl_qvKCQO8BDYOuK-TMbODwMpe-wNn4MWqfEaA,50176
+bz2.dll,sha256=ZCoakaZjkFYPhkGXrq5Ri6UkCjDD_ffRDIGse6dM0hQ,75264
+ccd.dll,sha256=IJ8ibQVhScsryJp2HrvEA8XPInaqUmrWcluBlU7b5J8,37888
+console_bridge.dll,sha256=LOgqiEg0RZE3bTTSdEVaupsjsz-huiFBqDC0QVCdsEM,24576
+dartpy.cp312-win_amd64.pyd,sha256=Nz8udYsE0woyicTw5ANEXQ5udCYwDa4h8Dl6B6xc4rw,9481216
+fontconfig-1.dll,sha256=8XoIgDiM5NBNNRbRIQw1L0qcNlEJnA-J8j8AA3JmAas,285184
+freetype.dll,sha256=adw4ak368AhMSgmtPBaHq94VHfjWSfE06pbWu2_jTZc,691200
+libexpat.dll,sha256=D2ovnnD4NzVZu3JoNrL1oOx2CoutRJfKimjmZ2WX2kg,157184
+libpng16.dll,sha256=Oxw_87pVsNU-MieE69o1i9ZyAiVsBaePspF78HMvYhg,199680
+minizip.dll,sha256=eWR8UJkJ2xistIlpbpr6zQqfYkleGSLfZFykhREXxgI,47104
+nlopt.dll,sha256=BXc7lG6FTds_S3elgI07rdhqHmOYDUZZRSd1KR25BoA,525824
+ode_double.dll,sha256=gd_PMf4-4xe2_vr40CxpDAztDTIk6Nlj6xA3TKleGY4,707072
+osg.dll,sha256=AE5X8PMikzDSmlqBYiBo6pd6Ep5EEPYtGnsqK0mY8pE,2738688
+osgDB.dll,sha256=6CsezZMSLkXVZ9vGna9U3d6H7F1-bYT1CVZFfYdpv9A,953344
+osgGA.dll,sha256=BWNaYe3vBnIKT9sHCfkKmyD8PvAOyMD0G6LHa1xIGqE,325632
+osgShadow.dll,sha256=mquNEjRLmX4-G2DpxfNbSh6bBgwgbbx77uiSMUswyQM,470528
+osgText.dll,sha256=9STzTG7mA_ZJzujTGlchau4ZHvhJzKLYMgSVQYKGvY8,321536
+osgUtil.dll,sha256=9d_drEtVaKfxWyERxgQwT5-DtaE6HX38ng8LWwVBn8E,1334784
+osgViewer.dll,sha256=a00rDlOEC7dps8FVzTZRvH2HdSxTEca1hyG8Ij1XJ4A,743936
+poly2tri.dll,sha256=5twzwTrV8ToYbCWqayQV14OeBoIA3qqXRIrr0dB8qtg,43008
+pugixml.dll,sha256=DBJ7utB7LfagqIVm_OblDib-B8RItpbELxEhgF9O73Q,190464
+tinyxml2.dll,sha256=qxsav7kiFUBWw3NGg2kvL9u_GhsaQEsbbX7d43WqnkY,95232
+urdfdom_world.dll,sha256=AriC9bXiamguF9ARxh5zDXuM5drsZQrUJqv_WF5puQ8,161792
+zlib1.dll,sha256=DmCyg4ruoOCnnNgTarQfAI6I7Bd5v-iStj2CuhF0ffg,89600
+tests/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/util.py,sha256=54Hn0E0aD61sKl5xKZ7iM2TtH-OT-ecPvepucfViROc,490
+tests/integration/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/integration/test_joint_force_torque.py,sha256=lPFlYMGZ--hP-8IOnbkdPejzDLlRmtth8wDvyQGVuyI,10551
+tests/unit/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/collision/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/collision/test_collision.py,sha256=7dlkIxl5-l7H9bt3P24ntsmcu8VCgggWrzNKMdMLTEU,7804
+tests/unit/common/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/common/test_logging.py,sha256=YRTY-ScNizYeqmpxb2VRhY4uH98m-Lb5WE7VBb0n2WY,435
+tests/unit/common/test_stopwatch.py,sha256=fNWNy-AzUuz3yhn7KHrhqwjxJ8Q18F5RFolCpwsBJzs,1280
+tests/unit/common/test_string.py,sha256=CiXjAhTBMKH6_CuVwGteyflq2uKClxx-QZm8h_SGiew,1562
+tests/unit/common/test_uri.py,sha256=HyXNYoW7p332zNUQRFaAN_-Xbh62MJjHYSP0MEeXjH8,765
+tests/unit/constraint/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/constraint/test_constraint.py,sha256=nw0vv0hGV5Jx7gU3flLqUiK3SXUihJtCgs_V1qXsvmk,1403
+tests/unit/dynamics/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/dynamics/test_aspect.py,sha256=s9e5GlIJZ4OEO8c85yUPqvCNlZV9W-fktn7XYwnzc_U,395
+tests/unit/dynamics/test_body_node.py,sha256=TlzI6jM20cPTzlc47x9bKF3zSewoG2kYaOXmLKbYcuw,2197
+tests/unit/dynamics/test_inertia.py,sha256=SdAT2KNQ3fW27x8pgq13T9dXyQIcZ1CEvH8b1A55acg,2202
+tests/unit/dynamics/test_inverse_kinematics.py,sha256=DdCruJpuPnj2HO59gkF5Z6hpYm1ZvVeuq3DyPodXzfI,2798
+tests/unit/dynamics/test_joint.py,sha256=XJv2fHYvrZZTLI2RiLCZ85ZgE00nwSFcTF8luWchmWQ,4588
+tests/unit/dynamics/test_meta_skeleton.py,sha256=KCqZRXx0UnPh8nptCfM_5OszL_YlDzStSdOu8a_m9x4,1107
+tests/unit/dynamics/test_simple_frame.py,sha256=YjIp8b-R3qXLncvw-JyIwD_ClQC-4lku4eAvQGIk9d4,1991
+tests/unit/dynamics/test_skeleton.py,sha256=C-wY5Der2cVl7Y4pHBV-lB8eLpgFBU2p764cSlYKG8A,548
+tests/unit/math/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/math/test_random.py,sha256=2WoH-3lv2iQQrCKZXN914GNuCpmhC5wXafHUr7LJ_Wk,828
+tests/unit/optimizer/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/optimizer/test_optimizer.py,sha256=fegkKe0DR5vLqFBlxwWOEK9HrZcglWGrv7RL05InwP0,2584
+tests/unit/simulation/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/simulation/test_world.py,sha256=yT0TknYuLF1kl-hhC3COnOs0ivfXUZ9gABSeoQqrXN0,1362
+tests/unit/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/utils/test_dart_loader.py,sha256=Oq_5mfogGFFUXkNFTAgPBE1yAjk7Vgkl0DRl93hjlkI,2288
+tests/unit/utils/test_mjcf_parser.py,sha256=RD-5ZLK0IeqtA2Zth07-1SznjSSuLlBzhOfZUZbhJho,357
+tests/unit/utils/test_sdf_parser.py,sha256=Tiea-pVHC2ZE7dsMhPPykbc5L2IHxtQTYanuFLgAQXw,491
+tests/unit/utils/test_skel_parser.py,sha256=a_nTYkT5yBPWNqOSow5bEMPALgJUb3Owvu8PBzkZzyI,577
+tests/unit/v7/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+tests/unit/v7/test_import.py,sha256=ih2zs8JCYUEf1mBPTbjVZLSPrL3miQQ5HI0OWGfC5bo,154
+dartpy-0.1.0.post107.dist-info/LICENSE,sha256=4DWlDbIdXGvZMkvRwozhkDtn4QjniOiacm_E6R1b8Js,1771
+dartpy-0.1.0.post107.dist-info/METADATA,sha256=tYp7VHbWbcrqxx6ozkt0h82k4aCp47BZCU4RiDYhCzM,5657
+dartpy-0.1.0.post107.dist-info/WHEEL,sha256=fZWyj_84lK0cA-ZNCsdwhbJl0OTrpWkxInEn424qrSs,102
+dartpy-0.1.0.post107.dist-info/top_level.txt,sha256=uS54APWVBb-a3RHZ0OUaZMgYA3_YFTZ-6m_1uYJFlls,18
+dartpy-0.1.0.post107.dist-info/RECORD,,

dartpy-0.1.0.post107.dist-info/WHEEL

@@ -0,0 +1,5 @@
+Wheel-Version: 1.0
+Generator: bdist_wheel (0.43.0)
+Root-Is-Purelib: false
+Tag: cp312-cp312-win_amd64
+

dartpy-0.1.0.post107.dist-info/top_level.txt

@@ -0,0 +1,2 @@
+dartpy_math
+tests

tests/integration/test_joint_force_torque.py

@@ -0,0 +1,305 @@
+import math
+
+import dartpy as dart
+import numpy as np
+import pytest
+
+
+def read_world(uri: dart.common.Uri):
+    options = dart.utils.SdfParser.Options()
+    options.mDefaultRootJointType = dart.utils.SdfParser.RootJointType.FIXED
+    world = dart.utils.SdfParser.readWorld(uri, options)
+    for i in range(world.getNumSkeletons()):
+        skel = world.getSkeleton(i)
+        for j in range(skel.getNumJoints()):
+            joint = skel.getJoint(j)
+            joint.setLimitEnforcement(True)
+    return world
+
+
+def test_static():
+    # Load world
+    world = read_world("dart://sample/sdf/test/force_torque_test.world")
+    assert world is not None
+    assert world.getNumSkeletons() == 1
+
+    # Check if the world is correctly loaded
+    model_1 = world.getSkeleton(0)
+    assert model_1 is not None
+    assert model_1.getName() == "model_1"
+    assert model_1.getNumBodyNodes() == 2
+    assert model_1.getNumJoints() == 2
+
+    world.setGravity([0, 0, -50])
+
+    # Simulate 1 step
+    world.step()
+    t = world.getTime()
+
+    # Get time step size
+    dt = world.getTimeStep()
+    assert dt > 0
+    assert t == pytest.approx(dt)
+
+    # Get joint and get force torque
+    link_1 = model_1.getBodyNode("link_1")
+    assert link_1 is not None
+    link_2 = model_1.getBodyNode("link_2")
+    assert link_2 is not None
+    joint_01 = model_1.getJoint("joint_01")
+    assert joint_01 is not None
+    joint_12 = model_1.getJoint("joint_12")
+    assert joint_12 is not None
+
+    tf = dart.math.Isometry3()
+
+    # Run 10 steps
+    for _ in range(10):
+        world.step()
+
+        # ----------------------
+        # Test joint_01 wrench
+        # ----------------------
+
+        # Reference adjustment for the difference of the joint frame conventions
+        # between Gazebo and DART
+        tf.set_identity()
+        tf.set_translation(joint_01.getTransformFromParentBodyNode().translation())
+        parent_frame01 = dart.dynamics.SimpleFrame(
+            dart.dynamics.Frame.World(), "parent_frame01", tf
+        )
+        tf.set_identity()
+        tf.set_translation(joint_01.getTransformFromChildBodyNode().translation())
+        child_frame01 = dart.dynamics.SimpleFrame(link_1, "child_frame01", tf)
+
+        parent_f01 = joint_01.getWrenchToParentBodyNode(parent_frame01)
+        assert (parent_f01 == [0, 0, 0, 0, 0, 1000]).all()
+
+        child_f01 = joint_01.getWrenchToChildBodyNode(child_frame01)
+        assert (child_f01 == -parent_f01).all()
+
+        # ----------------------
+        # Test joint_12 wrench
+        # ----------------------
+
+        # Reference adjustment for the difference of the joint frame conventions
+        # between Gazebo and DART
+        tf.set_identity()
+        tf.set_translation(joint_12.getTransformFromParentBodyNode().translation())
+        parent_frame12 = dart.dynamics.SimpleFrame(link_1, "parent_frame12", tf)
+        tf.set_identity()
+        tf.set_translation(joint_12.getTransformFromChildBodyNode().translation())
+        child_frame12 = dart.dynamics.SimpleFrame(link_2, "child_frame12", tf)
+
+        parent_f12 = joint_12.getWrenchToParentBodyNode(parent_frame12)
+        assert (parent_f12 == [0, 0, 0, 0, 0, 500]).all()
+
+        child_f12 = joint_12.getWrenchToChildBodyNode(child_frame12)
+        assert (child_f12 == -parent_f12).all()
+
+
+def test_force_torque_at_joint_limits():
+    # Load world
+    world = read_world("dart://sample/sdf/test/force_torque_test.world")
+    assert world is not None
+    assert world.getNumSkeletons() == 1
+
+    # Check if the world is correctly loaded
+    model_1 = world.getSkeleton(0)
+    assert model_1 is not None
+    assert model_1.getName() == "model_1"
+    assert model_1.getNumBodyNodes() == 2
+    assert model_1.getNumJoints() == 2
+
+    world.setGravity([0, 0, -50])
+
+    # Simulate 1 step
+    world.step()
+    t = world.getTime()
+
+    # Get time step size
+    dt = world.getTimeStep()
+    assert dt > 0
+    assert t == pytest.approx(dt)
+
+    # Get joint and get force torque
+    link_1 = model_1.getBodyNode("link_1")
+    assert link_1 is not None
+    link_2 = model_1.getBodyNode("link_2")
+    assert link_2 is not None
+    joint_01 = model_1.getJoint("joint_01")
+    assert joint_01 is not None
+    joint_12 = model_1.getJoint("joint_12")
+    assert joint_12 is not None
+
+    # Change gravity so that the top link topples over, then remeasure
+    world.setGravity([-30, 10, -50])
+
+    # Wait for dynamics to be stabilized
+    for i in range(2000):
+        world.step()
+
+    tf = dart.math.Isometry3()
+
+    # Run 5 steps
+    for _ in range(5):
+        world.step()
+
+        # ----------------------
+        # Test joint_01 wrench
+        # ----------------------
+
+        # Reference adjustment for the difference of the joint frame conventions
+        # between Gazebo and DART
+        tf.set_identity()
+        tf.set_translation(joint_01.getTransformFromParentBodyNode().translation())
+        parent_frame01 = dart.dynamics.SimpleFrame(
+            dart.dynamics.Frame.World(), "parent_frame01", tf
+        )
+        tf.set_identity()
+        tf.set_translation(joint_01.getTransformFromChildBodyNode().translation())
+        child_frame01 = dart.dynamics.SimpleFrame(link_1, "child_frame01", tf)
+
+        parent_f01 = joint_01.getWrenchToParentBodyNode(parent_frame01)
+        assert np.isclose(
+            parent_f01, [750, 0, -450, 600, -200, 1000], rtol=0.1, atol=4.5
+        ).all()
+
+        child_f01 = joint_01.getWrenchToChildBodyNode(child_frame01)
+        assert np.isclose(
+            child_f01, [-750, -450, 0, -600, 1000, 200], rtol=0.1, atol=4.5
+        ).all()
+
+        # ----------------------
+        # Test joint_12 wrench
+        # ----------------------
+
+        # Reference adjustment for the difference of the joint frame conventions
+        # between Gazebo and DART
+        tf.set_identity()
+        tf.set_translation(joint_12.getTransformFromParentBodyNode().translation())
+        parent_frame12 = dart.dynamics.SimpleFrame(link_1, "parent_frame12", tf)
+        tf.set_identity()
+        tf.set_translation(joint_12.getTransformFromChildBodyNode().translation())
+        child_frame12 = dart.dynamics.SimpleFrame(link_2, "child_frame12", tf)
+
+        parent_f12 = joint_12.getWrenchToParentBodyNode(parent_frame12)
+        assert np.isclose(
+            parent_f12, [250, 150, 0, 300, -500, -100], rtol=0.1, atol=0.1
+        ).all()
+
+        child_f12 = joint_12.getWrenchToChildBodyNode(child_frame12)
+        assert np.isclose(
+            child_f12, [-250, -150, 0, -300, 500, 100], rtol=0.1, atol=0.1
+        ).all()
+
+
+def test_force_torque_at_joint_limits_with_external_forces():
+    # Load world
+    world = read_world("dart://sample/sdf/test/force_torque_test2.world")
+    assert world is not None
+    assert world.getNumSkeletons() == 1
+
+    # Check if the world is correctly loaded
+    model_1 = world.getSkeleton(0)
+    assert model_1 is not None
+    assert model_1.getName() == "boxes"
+    assert model_1.getNumBodyNodes() == 3
+    assert model_1.getNumJoints() == 3
+    assert model_1.getNumDofs() == 2
+
+    # The first joint is fixed joint
+    assert model_1.getRootJoint().getType() == dart.dynamics.WeldJoint.getStaticType()
+
+    world.setGravity([0, 0, -50])
+
+    # Simulate 1 step
+    world.step()
+    t = world.getTime()
+
+    # Get time step size
+    dt = world.getTimeStep()
+    assert dt > 0
+    assert t == pytest.approx(dt)
+
+    # Get joint and get force torque
+    link_1 = model_1.getBodyNode("link1")
+    assert link_1 is not None
+    link_2 = model_1.getBodyNode("link2")
+    assert link_2 is not None
+    link_3 = model_1.getBodyNode("link3")
+    assert link_3 is not None
+    joint_12 = model_1.getJoint("joint1")
+    assert joint_12 is not None
+    joint_23 = model_1.getJoint("joint2")
+    assert joint_23 is not None
+
+    tf = dart.math.Isometry3()
+
+    # Run 45005 steps
+    kp1 = 5e4
+    kp2 = 1e4
+    target1 = 0
+    target2 = -0.25 * math.pi
+    steps = 4500
+    for _ in range(steps):
+        # PD control
+        j1_state = joint_12.getPosition(0)
+        j2_state = joint_23.getPosition(0)
+        p1_error = target1 - j1_state
+        p2_error = target2 - j2_state
+        effort1 = kp1 * p1_error
+        effort2 = kp2 * p2_error
+        joint_12.setForce(0, effort1)
+        joint_23.setForce(0, effort2)
+
+        world.step()
+
+    assert joint_12.getPosition(0) == pytest.approx(target1, abs=1e-1)
+    assert joint_23.getPosition(0) == pytest.approx(target2, abs=1e-1)
+
+    tol = 2
+
+    # ----------------------
+    # Test joint_12 wrench
+    # ----------------------
+
+    # Reference adjustment for the difference of the joint frame conventions
+    # between Gazebo and DART
+    tf.set_identity()
+    tf.set_translation(joint_12.getTransformFromParentBodyNode().translation())
+    parent_frame01 = dart.dynamics.SimpleFrame(link_1, "parent_frame01", tf)
+    tf.set_identity()
+    tf.set_translation(joint_12.getTransformFromChildBodyNode().translation())
+    child_frame01 = dart.dynamics.SimpleFrame(link_2, "child_frame01", tf)
+
+    parent_f01 = joint_12.getWrenchToParentBodyNode(parent_frame01)
+    assert np.isclose(parent_f01, [25, -175, 0, 0, 0, 300], rtol=0.01, atol=tol).all()
+
+    child_f01 = joint_12.getWrenchToChildBodyNode(child_frame01)
+    assert np.isclose(child_f01, [-25, 175, 0, 0, 0, -300], rtol=0.01, atol=tol).all()
+
+    # ----------------------
+    # Test joint_23 wrench
+    # ----------------------
+
+    # Reference adjustment for the difference of the joint frame conventions
+    # between Gazebo and DART
+    tf.set_identity()
+    tf.set_translation(joint_23.getTransformFromParentBodyNode().translation())
+    parent_frame12 = dart.dynamics.SimpleFrame(link_2, "parent_frame12", tf)
+    tf.set_identity()
+    tf.set_translation(joint_23.getTransformFromChildBodyNode().translation())
+    child_frame12 = dart.dynamics.SimpleFrame(link_3, "child_frame12", tf)
+
+    parent_f12 = joint_23.getWrenchToParentBodyNode(parent_frame12)
+    assert np.isclose(parent_f12, [25, 0, 0, 0, 0, 50], rtol=0.01, atol=tol).all()
+
+    child_f12 = joint_23.getWrenchToChildBodyNode(child_frame12)
+    assert np.isclose(
+        child_f12, [-17.678, 0, 17.679, -35.355, 0, -35.355], rtol=0.01, atol=tol
+    ).all()
+
+
+if __name__ == "__main__":
+    pytest.main()