diff --git a/python/py_package/wrapper/articulation_builder.py b/python/py_package/wrapper/articulation_builder.py index 98fe9db3..9dc6a9a9 100644 --- a/python/py_package/wrapper/articulation_builder.py +++ b/python/py_package/wrapper/articulation_builder.py @@ -182,8 +182,8 @@ def build( root = joint.parent_link articulation.create_fixed_tendon( [root, joint.child_link, mimic_joint.child_link], - [0, -multiplier, 1], - [0, -1 / multiplier, 1], + [0, 1, -multiplier], + [0, 1, -1 / multiplier], rest_length=offset, stiffness=1e5, ) diff --git a/unittest/test_physx/test_articulation.py b/unittest/test_physx/test_articulation.py index 88767d6b..5e8f3ea1 100644 --- a/unittest/test_physx/test_articulation.py +++ b/unittest/test_physx/test_articulation.py @@ -1,4 +1,5 @@ import os +import tempfile import unittest from pathlib import Path @@ -72,6 +73,56 @@ def test_urdf_loader(self): robot.set_qf(q) self.assertTrue(np.allclose(robot.get_qf(), q)) + def test_urdf_mimic_sibling_multiplier_offset(self): + inertial = ( + '' + '' + "" + ) + limit = '' + urdf = f""" + {inertial} + {inertial} + {inertial} + + + {limit} + + + + {limit} + + +""" + with tempfile.TemporaryDirectory() as d: + path = Path(d) / "mimic_sibling.urdf" + path.write_text(urdf) + + scene = sapien.Scene() + scene.set_timestep(1 / 240) + loader = scene.create_urdf_loader() + loader.fix_root_link = True + robot = loader.load(str(path)) + + joints = {j.get_name(): j for j in robot.get_active_joints()} + joints["source"].set_drive_property( + stiffness=5000, damping=500, force_limit=100000 + ) + joints["source"].set_drive_target(0.4) + joints["follower"].set_drive_property(stiffness=0, damping=0, force_limit=0) + for _ in range(720): + scene.step() + + qpos = { + j.get_name(): robot.get_qpos()[i] + for i, j in enumerate(robot.get_active_joints()) + } + self.assertAlmostEqual( + qpos["follower"], + 2.0 * qpos["source"] + 0.1, + delta=1e-3, + ) + def test_kinematics_dynamics(self): scene = sapien.Scene() loader = scene.create_urdf_loader()