diff --git a/include/mplib/kinematics/pinocchio/pinocchio_model.h b/include/mplib/kinematics/pinocchio/pinocchio_model.h index f2519ca5..372fda19 100644 --- a/include/mplib/kinematics/pinocchio/pinocchio_model.h +++ b/include/mplib/kinematics/pinocchio/pinocchio_model.h @@ -263,7 +263,8 @@ class PinocchioModelTpl { void computeFullJacobian(const VectorX &qpos); /** - * Get the jacobian of the given link. You must call ``compute_full_jacobian()`` first. + * Get the jacobian of the given link. You must call ``compute_full_jacobian()`` + * first. * * @param index: index of the link (in the order you passed to the constructor or the * default order) @@ -276,7 +277,7 @@ class PinocchioModelTpl { /** * Compute the jacobian of the given link. * Note you need to call computeForwardKinematics() first. - * + * * @param qpos: joint configuration. Needs to be full configuration, not just the * movegroup joints. * @param index: index of the link (in the order you passed to the constructor or the diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py index 1ae8f027..d2cdef9d 100644 --- a/mplib/examples/collision_avoidance.py +++ b/mplib/examples/collision_avoidance.py @@ -57,6 +57,7 @@ def __init__(self): def add_point_cloud(self): """We tell the planner about the obstacle through a point cloud""" import trimesh + # add_point_cloud ankor box = trimesh.creation.box([0.1, 0.4, 0.2]) points, _ = trimesh.sample.sample_surface(box, 1000) diff --git a/mplib/examples/constrained_planning.py b/mplib/examples/constrained_planning.py index 624e70a5..bf8047e0 100644 --- a/mplib/examples/constrained_planning.py +++ b/mplib/examples/constrained_planning.py @@ -48,12 +48,14 @@ def make_f(self): See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details. """ + # constraint function ankor def f(x, out): self.planner.robot.set_qpos(x) out[0] = ( self.get_eef_z().dot(np.array([0, 0, -1])) - 0.966 ) # maintain 15 degrees w.r.t. -z axis + # constraint function ankor end return f @@ -63,6 +65,7 @@ def make_j(self): This is needed because the planner uses the jacobian to project a random sample to the constraint manifold. """ + # constraint jacobian ankor def j(x, out): full_qpos = self.planner.pad_qpos(x) @@ -74,6 +77,7 @@ def j(x, out): out[i] = np.cross(rot_jac[:, i], self.get_eef_z()).dot( np.array([0, 0, -1]) ) + # constraint jacobian ankor end return j diff --git a/mplib/examples/demo.py b/mplib/examples/demo.py index eaa9c522..00d757ce 100644 --- a/mplib/examples/demo.py +++ b/mplib/examples/demo.py @@ -86,6 +86,7 @@ def demo(self): self.move_to_pose(pose) # execute motion ankor end + if __name__ == "__main__": demo = PlanningDemo() demo.demo() diff --git a/mplib/examples/demo_setup.py b/mplib/examples/demo_setup.py index 0133212a..d9e961d1 100644 --- a/mplib/examples/demo_setup.py +++ b/mplib/examples/demo_setup.py @@ -106,6 +106,7 @@ def setup_planner(self, **kwargs): srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"), move_group=kwargs.get("move_group", "panda_hand"), ) + # follow path ankor def follow_path(self, result): """Helper function to follow a path generated by the planner""" @@ -131,6 +132,7 @@ def follow_path(self, result): if i % 4 == 0: self.scene.update_render() self.viewer.render() + # follow path ankor end def set_gripper(self, pos): """ diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py index aec5c61f..162023fe 100644 --- a/mplib/examples/detect_collision.py +++ b/mplib/examples/detect_collision.py @@ -13,6 +13,7 @@ def __init__(self): """Only the planner is needed this time. No simulation env required""" super().__init__() self.setup_planner() + # print_collision ankor def print_collisions(self, collisions): """Helper function to abstract away the printing of collisions""" @@ -24,6 +25,7 @@ def print_collisions(self, collisions): f"{collision.link_name1} of entity {collision.object_name1} collides" f" with {collision.link_name2} of entity {collision.object_name2}" ) + # print_collision ankor end def demo(self): """ @@ -77,7 +79,7 @@ def demo(self): print("\n----- env-collision qpos -----") # this qpos causes several joints to dip below the floor - env_collision_qpos = [0,1.5,0,-1.5,0,0,0] + env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0] self.print_collisions( self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos) ) diff --git a/mplib/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py index c239de83..b87aee83 100644 --- a/mplib/examples/two_stage_motion.py +++ b/mplib/examples/two_stage_motion.py @@ -123,6 +123,7 @@ def plan_without_base(self, pose, has_attach=False): fixed_joint_indices=range(2), ) return result + # move_in_two_stage ankor def move_in_two_stage(self, pose, has_attach=False): """ @@ -148,6 +149,7 @@ def move_in_two_stage(self, pose, has_attach=False): result = self.plan_without_base(pose, has_attach) # execute the planned path self.follow_path(result) + # move_in_two_stage ankor end def demo(self): """ diff --git a/mplib/planner.py b/mplib/planner.py index d54aaecd..1fa35efd 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -93,7 +93,7 @@ def __init__( if self.srdf == "": self.generate_collision_pair() self.robot.update_SRDF(self.srdf) - + assert ( move_group in self.user_link_names ), f"end-effector not found as one of the links in {self.user_link_names}" @@ -676,7 +676,7 @@ def plan_qpos_to_pose( current_qpos[i] = self.joint_limits[i][1] - 1e-3 current_qpos = self.pad_qpos(current_qpos) - + if wrt_world: goal_pose = self.transform_goal_to_wrt_base(goal_pose) diff --git a/src/kinematics/pinocchio/pinocchio_model.cpp b/src/kinematics/pinocchio/pinocchio_model.cpp index af8bc8a6..acbc7460 100644 --- a/src/kinematics/pinocchio/pinocchio_model.cpp +++ b/src/kinematics/pinocchio/pinocchio_model.cpp @@ -1,9 +1,9 @@ #include "mplib/kinematics/pinocchio/pinocchio_model.h" +#include #include #include #include -#include #include #include "mplib/macros/assert.h" @@ -559,8 +559,8 @@ Matrix6X PinocchioModelTpl::computeSingleLinkJacobian(const VectorX &qp Matrix6X J(6, model_.nv); J.fill(0); auto rf = local ? pinocchio::LOCAL : pinocchio::WORLD; - pinocchio::computeFrameJacobian(model_, data_, qposUser2Pinocchio(qpos), frameId, - rf, J); + pinocchio::computeFrameJacobian(model_, data_, qposUser2Pinocchio(qpos), frameId, rf, + J); return J * v_map_user2pinocchio_; } diff --git a/tests/test_articulation.py b/tests/test_articulation.py index 0b432798..37ae7e9a 100644 --- a/tests/test_articulation.py +++ b/tests/test_articulation.py @@ -1,134 +1,146 @@ import os -import numpy as np import unittest + +import numpy as np +import trimesh from transforms3d.quaternions import mat2quat, quat2mat + import mplib from mplib.pymp.collision_detection import fcl -import trimesh FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) PANDA_SPEC = { - "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", - "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", + "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", + "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", } -ALL_LINKS = ['panda_link0', - 'panda_link1', - 'panda_link2', - 'panda_link3', - 'panda_link4', - 'panda_link5', - 'panda_link6', - 'panda_link7', - 'panda_link8', - 'panda_hand', - 'panda_leftfinger', - 'panda_rightfinger'] - -ALL_JOINTS = ['panda_joint1', - 'panda_joint2', - 'panda_joint3', - 'panda_joint4', - 'panda_joint5', - 'panda_joint6', - 'panda_joint7', - 'panda_finger_joint1', - 'panda_finger_joint2'] +ALL_LINKS = [ + "panda_link0", + "panda_link1", + "panda_link2", + "panda_link3", + "panda_link4", + "panda_link5", + "panda_link6", + "panda_link7", + "panda_link8", + "panda_hand", + "panda_leftfinger", + "panda_rightfinger", +] + +ALL_JOINTS = [ + "panda_joint1", + "panda_joint2", + "panda_joint3", + "panda_joint4", + "panda_joint5", + "panda_joint6", + "panda_joint7", + "panda_finger_joint1", + "panda_finger_joint2", +] + class TestArticulation(unittest.TestCase): - def setUp(self): - self.robot = mplib.ArticulatedModel( - PANDA_SPEC["urdf"], - PANDA_SPEC["srdf"], - gravity=[0, 0, -9.81], - joint_names=[], - link_names=[], - verbose=False, - convex=True, - ) - - self.target_pose = [0.4, 0.3, 0.12, 0, 1, 0, 0] - self.init_qpos = np.array([0, 0.2, 0, -2.6, 0, 3.0, 0.8, 0, 0]) - self.joint_limits = [[-2.8973, 2.8973], - [-1.7628, 1.7628], - [-2.8973, 2.8973], - [-3.0718, -0.0698], - [-2.8973, 2.8973], - [-0.0175, 3.7525], - [-2.8973, 2.8973], - [0, 0.04], - [0, 0.04]] - - def test_get_base_pose(self): - base_pose = self.robot.get_base_pose() - self.assertIsInstance(base_pose, np.ndarray) - self.assertTrue(np.allclose(base_pose, np.array([0, 0, 0, 1, 0, 0, 0]))) - - def test_get_fcl_model(self): - fcl_model = self.robot.get_fcl_model() - self.assertIsInstance(fcl_model, fcl.FCLModel) - - def test_get_move_group_end_effectors(self): - end_effectors = self.robot.get_move_group_end_effectors() - self.assertIsInstance(end_effectors, list) - self.assertEqual(end_effectors, ALL_LINKS) - - def test_get_move_group_joint_indices(self): - joint_indices = self.robot.get_move_group_joint_indices() - self.assertIsInstance(joint_indices, list) - self.assertEqual(joint_indices, list(range(9))) - self.robot.set_move_group("panda_hand") - joint_indices = self.robot.get_move_group_joint_indices() - self.assertEqual(joint_indices, list(range(7))) - - def test_get_move_group_joint_names(self): - joint_names = self.robot.get_move_group_joint_names() - self.assertIsInstance(joint_names, list) - self.assertEqual(joint_names, ALL_JOINTS) - self.robot.set_move_group("panda_link4") - joint_names = self.robot.get_move_group_joint_names() - self.assertEqual(joint_names, ALL_JOINTS[:4]) - - def test_get_move_group_qpos_dim(self): - qpos_dim = self.robot.get_move_group_qpos_dim() - self.assertIsInstance(qpos_dim, int) - self.assertEqual(qpos_dim, 9) - self.robot.set_move_group("panda_hand") - qpos_dim = self.robot.get_move_group_qpos_dim() - self.assertEqual(qpos_dim, 7) - - def test_get_pinocchio_model(self): - pinocchio_model = self.robot.get_pinocchio_model() - self.assertIsInstance(pinocchio_model, mplib.pymp.kinematics.pinocchio.PinocchioModel) - - def test_get_qpos(self): - qpos = self.robot.get_qpos() - self.assertIsInstance(qpos, np.ndarray) - self.assertEqual(len(qpos), len(self.robot.get_user_joint_names())) - self.robot.set_qpos(np.arange(len(qpos))/10.0, full=True) - qpos = self.robot.get_qpos() - self.assertTrue(np.allclose(qpos, np.arange(len(qpos))/10.0)) - - def test_get_user_joint_names(self): - joint_names = self.robot.get_user_joint_names() - self.assertIsInstance(joint_names, list) - self.assertEqual(joint_names, ALL_JOINTS) - - def test_get_user_link_names(self): - link_names = self.robot.get_user_link_names() - self.assertIsInstance(link_names, list) - self.assertEqual(link_names, ALL_LINKS) - - def test_set_base_pose(self): - pose = np.arange(7)/10.0 - pose[3:] /= np.linalg.norm(pose[3:]) - self.robot.set_base_pose(pose) - self.assertTrue(np.allclose(self.robot.get_base_pose(), pose)) - - def test_update_SRDF(self): - self.robot.update_SRDF("") # should not raise error - self.assertTrue(True) + def setUp(self): + self.robot = mplib.ArticulatedModel( + PANDA_SPEC["urdf"], + PANDA_SPEC["srdf"], + gravity=[0, 0, -9.81], + joint_names=[], + link_names=[], + verbose=False, + convex=True, + ) + + self.target_pose = [0.4, 0.3, 0.12, 0, 1, 0, 0] + self.init_qpos = np.array([0, 0.2, 0, -2.6, 0, 3.0, 0.8, 0, 0]) + self.joint_limits = [ + [-2.8973, 2.8973], + [-1.7628, 1.7628], + [-2.8973, 2.8973], + [-3.0718, -0.0698], + [-2.8973, 2.8973], + [-0.0175, 3.7525], + [-2.8973, 2.8973], + [0, 0.04], + [0, 0.04], + ] + + def test_get_base_pose(self): + base_pose = self.robot.get_base_pose() + self.assertIsInstance(base_pose, np.ndarray) + self.assertTrue(np.allclose(base_pose, np.array([0, 0, 0, 1, 0, 0, 0]))) + + def test_get_fcl_model(self): + fcl_model = self.robot.get_fcl_model() + self.assertIsInstance(fcl_model, fcl.FCLModel) + + def test_get_move_group_end_effectors(self): + end_effectors = self.robot.get_move_group_end_effectors() + self.assertIsInstance(end_effectors, list) + self.assertEqual(end_effectors, ALL_LINKS) + + def test_get_move_group_joint_indices(self): + joint_indices = self.robot.get_move_group_joint_indices() + self.assertIsInstance(joint_indices, list) + self.assertEqual(joint_indices, list(range(9))) + self.robot.set_move_group("panda_hand") + joint_indices = self.robot.get_move_group_joint_indices() + self.assertEqual(joint_indices, list(range(7))) + + def test_get_move_group_joint_names(self): + joint_names = self.robot.get_move_group_joint_names() + self.assertIsInstance(joint_names, list) + self.assertEqual(joint_names, ALL_JOINTS) + self.robot.set_move_group("panda_link4") + joint_names = self.robot.get_move_group_joint_names() + self.assertEqual(joint_names, ALL_JOINTS[:4]) + + def test_get_move_group_qpos_dim(self): + qpos_dim = self.robot.get_move_group_qpos_dim() + self.assertIsInstance(qpos_dim, int) + self.assertEqual(qpos_dim, 9) + self.robot.set_move_group("panda_hand") + qpos_dim = self.robot.get_move_group_qpos_dim() + self.assertEqual(qpos_dim, 7) + + def test_get_pinocchio_model(self): + pinocchio_model = self.robot.get_pinocchio_model() + self.assertIsInstance( + pinocchio_model, mplib.pymp.kinematics.pinocchio.PinocchioModel + ) + + def test_get_qpos(self): + qpos = self.robot.get_qpos() + self.assertIsInstance(qpos, np.ndarray) + self.assertEqual(len(qpos), len(self.robot.get_user_joint_names())) + self.robot.set_qpos(np.arange(len(qpos)) / 10.0, full=True) + qpos = self.robot.get_qpos() + self.assertTrue(np.allclose(qpos, np.arange(len(qpos)) / 10.0)) + + def test_get_user_joint_names(self): + joint_names = self.robot.get_user_joint_names() + self.assertIsInstance(joint_names, list) + self.assertEqual(joint_names, ALL_JOINTS) + + def test_get_user_link_names(self): + link_names = self.robot.get_user_link_names() + self.assertIsInstance(link_names, list) + self.assertEqual(link_names, ALL_LINKS) + + def test_set_base_pose(self): + pose = np.arange(7) / 10.0 + pose[3:] /= np.linalg.norm(pose[3:]) + self.robot.set_base_pose(pose) + self.assertTrue(np.allclose(self.robot.get_base_pose(), pose)) + + def test_update_SRDF(self): + self.robot.update_SRDF("") # should not raise error + self.assertTrue(True) + if __name__ == "__main__": - unittest.main() + unittest.main() diff --git a/tests/test_fcl_model.py b/tests/test_fcl_model.py index 52852268..1cf385b7 100644 --- a/tests/test_fcl_model.py +++ b/tests/test_fcl_model.py @@ -1,69 +1,79 @@ import os import unittest + import numpy as np from transforms3d.quaternions import mat2quat + from mplib.pymp.collision_detection.fcl import FCLModel from mplib.pymp.kinematics.pinocchio import PinocchioModel FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) PANDA_SPEC = { - "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", - "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", - "move_group": "panda_hand", + "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", + "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", + "move_group": "panda_hand", } + class TestFCLModel(unittest.TestCase): - def setUp(self): - # Create a FCLModel instance for testing - self.model = FCLModel(PANDA_SPEC["urdf"], verbose=False) - self.pinocchio_model = PinocchioModel(PANDA_SPEC["urdf"], [0, 0, -9.81], verbose=False) - self.collision_link_names = ['panda_link0', - 'panda_link1', - 'panda_link2', - 'panda_link3', - 'panda_link4', - 'panda_link5', - 'panda_link6', - 'panda_link7', - 'panda_hand', - 'panda_leftfinger', - 'panda_rightfinger'] - - qpos = np.zeros(len(self.pinocchio_model.get_joint_names())) - self.pinocchio_model.compute_forward_kinematics(qpos) - for i, link_name in enumerate(self.collision_link_names): - link_idx = self.pinocchio_model.get_link_names().index(link_name) - link_pose = self.pinocchio_model.get_link_pose(link_idx) - self.model.get_collision_objects()[i].set_transformation(link_pose) + def setUp(self): + # Create a FCLModel instance for testing + self.model = FCLModel(PANDA_SPEC["urdf"], verbose=False) + self.pinocchio_model = PinocchioModel( + PANDA_SPEC["urdf"], [0, 0, -9.81], verbose=False + ) + self.collision_link_names = [ + "panda_link0", + "panda_link1", + "panda_link2", + "panda_link3", + "panda_link4", + "panda_link5", + "panda_link6", + "panda_link7", + "panda_hand", + "panda_leftfinger", + "panda_rightfinger", + ] + + qpos = np.zeros(len(self.pinocchio_model.get_joint_names())) + self.pinocchio_model.compute_forward_kinematics(qpos) + for i, link_name in enumerate(self.collision_link_names): + link_idx = self.pinocchio_model.get_link_names().index(link_name) + link_pose = self.pinocchio_model.get_link_pose(link_idx) + self.model.get_collision_objects()[i].set_transformation(link_pose) + + def test_get_collision_objects(self): + self.assertEqual( + self.model.get_collision_link_names(), self.collision_link_names + ) + for i, link_name in enumerate(self.collision_link_names): + pinocchio_idx = self.pinocchio_model.get_link_names().index(link_name) + pos = self.model.get_collision_objects()[i].get_translation() + quat = mat2quat(self.model.get_collision_objects()[i].get_rotation()) + pinocchio_pose = self.pinocchio_model.get_link_pose(pinocchio_idx) + self.assertTrue(np.allclose(pos, pinocchio_pose[:3])) + self.assertAlmostEqual(abs(quat.dot(pinocchio_pose[3:])), 1) - def test_get_collision_objects(self): - self.assertEqual(self.model.get_collision_link_names(), self.collision_link_names) - for i, link_name in enumerate(self.collision_link_names): - pinocchio_idx = self.pinocchio_model.get_link_names().index(link_name) - pos = self.model.get_collision_objects()[i].get_translation() - quat = mat2quat(self.model.get_collision_objects()[i].get_rotation()) - pinocchio_pose = self.pinocchio_model.get_link_pose(pinocchio_idx) - self.assertTrue(np.allclose(pos, pinocchio_pose[:3])) - self.assertAlmostEqual(abs(quat.dot(pinocchio_pose[3:])), 1) + def test_remove_collision_pairs_from_srdf(self): + old_collision_pairs = self.model.get_collision_pairs() + self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"]) + new_collision_pairs = self.model.get_collision_pairs() + for pair in new_collision_pairs: + self.assertIn(pair, old_collision_pairs) - def test_remove_collision_pairs_from_srdf(self): - old_collision_pairs = self.model.get_collision_pairs() - self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"]) - new_collision_pairs = self.model.get_collision_pairs() - for pair in new_collision_pairs: - self.assertIn(pair, old_collision_pairs) + def test_collision(self): + collisions = self.model.collide_full() + self.assertGreater(len(collisions), 0) + # some of them are collisions + is_collision = [collision.is_collision() for collision in collisions] + self.assertTrue(any(is_collision)) + self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"]) + collisions = self.model.collide_full() + is_collision = [collision.is_collision() for collision in collisions] + self.assertFalse(any(is_collision)) - def test_collision(self): - collisions = self.model.collide_full() - self.assertGreater(len(collisions), 0) - # some of them are collisions - is_collision = [collision.is_collision() for collision in collisions] - self.assertTrue(any(is_collision)) - self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"]) - collisions = self.model.collide_full() - is_collision = [collision.is_collision() for collision in collisions] - self.assertFalse(any(is_collision)) if __name__ == "__main__": - unittest.main() + unittest.main() diff --git a/tests/test_pinocchio.py b/tests/test_pinocchio.py index 6ac8f497..fd025957 100644 --- a/tests/test_pinocchio.py +++ b/tests/test_pinocchio.py @@ -1,140 +1,162 @@ import os import unittest + import numpy as np -from transforms3d.quaternions import qmult, qinverse, quat2axangle +from transforms3d.quaternions import qinverse, qmult, quat2axangle + from mplib.pymp.kinematics.pinocchio import PinocchioModel FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) PANDA_SPEC = { - "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", - "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", - "move_group": "panda_hand", + "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", + "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", + "move_group": "panda_hand", } + class TestPinocchioModel(unittest.TestCase): - def setUp(self): - # Create a PinocchioModel instance for testing - self.model = PinocchioModel(PANDA_SPEC["urdf"], [0, 0, -9.81], verbose=False) - self.joint_limits = [[-2.8973, 2.8973], - [-1.7628, 1.7628], - [-2.8973, 2.8973], - [-3.0718, -0.0698], - [-2.8973, 2.8973], - [-0.0175, 3.7525], - [-2.8973, 2.8973], - [0, 0.04], - [0, 0.04]] - - def test_get_leaf_links(self): - # Test the getLeafLinks method - leaf_links = self.model.get_leaf_links() - self.assertEqual(len(leaf_links), 2) # two finger links - - def test_set_link_order(self): - # Test the setLinkOrder method - names = ['panda_link0', - 'panda_link1', - 'panda_link3', - 'panda_link2', - 'panda_link5', - 'panda_link4', - 'panda_link7', - 'panda_link8', - 'panda_link6', - 'panda_hand'] - self.model.set_link_order(names) - self.assertEqual(self.model.get_link_names(), names) - - def test_set_joint_order(self): - # Test the setJointOrder method - names = ['panda_joint1', - 'panda_joint2', - 'panda_finger_joint1', - 'panda_joint4', - 'panda_joint3', - 'panda_joint5', - 'panda_joint7', - 'panda_joint6', - 'panda_finger_joint2'] - self.model.set_joint_order(names) - self.assertEqual(self.model.get_joint_names(), names) - - def test_get_joint_dim(self): - # Test the getJointDim method - for i in range(len(self.model.get_joint_names())): - self.assertEqual(self.model.get_joint_dim(i), 1) # all joints are 1D - - def test_get_joint_parent(self): - names = ['panda_joint1', - 'panda_joint2', - 'panda_finger_joint1', - 'panda_joint4', - 'panda_joint3', - 'panda_joint5', - 'panda_joint7', - 'panda_joint6', - 'panda_finger_joint2'] - self.model.set_joint_order(names) - expected = [0, 1, 7, 3, 2, 4, 6, 5, 7] - for i in range(len(self.model.get_joint_names())): - self.assertEqual(self.model.get_joint_parent(i), expected[i]) - - def sample_qpos(self): - qpos = [] - for limit in self.joint_limits: - qpos.append(np.random.uniform(limit[0], limit[1])) - return np.array(qpos) - - def test_two_jacobian_methods(self): - ee_idx = self.model.get_link_names().index("panda_hand") - for local in [True, False]: - for _ in range(10): - qpos = self.sample_qpos() - self.model.compute_forward_kinematics(qpos) - self.model.compute_full_jacobian(qpos) - analytical_jacobian_method1 = self.model.get_link_jacobian(ee_idx, local=local) - analytical_jacobian_method2 = self.model.compute_single_link_jacobian(qpos, ee_idx, local=local) - self.assertTrue(np.allclose(analytical_jacobian_method1, analytical_jacobian_method2)) - - def get_numerical_jacobian(self, qpos, link_idx, epsilon=1e-3): - jacobian = np.zeros((6, len(qpos))) - for i in range(len(qpos)): - perturbation = np.zeros(len(qpos)) - perturbation[i] = epsilon - self.model.compute_forward_kinematics(qpos + perturbation) - pose_plus = self.model.get_link_pose(link_idx) - self.model.compute_forward_kinematics(qpos - perturbation) - pose_minus = self.model.get_link_pose(link_idx) - - position_diff = pose_plus[:3] - pose_minus[:3] - jacobian[:3, i] = position_diff / (2 * epsilon) - - orientation_plus = pose_plus[3:] - orientation_minus = pose_minus[3:] - # get the difference quaternion - orientation_diff = qmult(orientation_plus, qinverse(orientation_minus)) - # get the axis-angle representation - axis, angle = quat2axangle(orientation_diff) - jacobian[3:, i] = angle / (2 * epsilon) * axis - - self.model.compute_forward_kinematics(qpos) - curr_pos = self.model.get_link_pose(link_idx)[:3] - speed_due_to_rotation = np.cross(jacobian[3:, i], curr_pos) - jacobian[:3, i] -= speed_due_to_rotation - - return jacobian - - def test_link_jacobian(self): - # idea is to calculate the jacobian numerically by perturbing the joint angles one by one - # and comparing the result to the analytical jacobian - ee_idx = self.model.get_link_names().index("panda_hand") - for _ in range(10): - qpos = self.sample_qpos() - self.model.compute_forward_kinematics(qpos) - analytical_jacobian = self.model.compute_single_link_jacobian(qpos, ee_idx) - numerical_jacobian = self.get_numerical_jacobian(qpos, ee_idx) - self.assertTrue(np.allclose(analytical_jacobian, numerical_jacobian, atol=1e-3)) + def setUp(self): + # Create a PinocchioModel instance for testing + self.model = PinocchioModel(PANDA_SPEC["urdf"], [0, 0, -9.81], verbose=False) + self.joint_limits = [ + [-2.8973, 2.8973], + [-1.7628, 1.7628], + [-2.8973, 2.8973], + [-3.0718, -0.0698], + [-2.8973, 2.8973], + [-0.0175, 3.7525], + [-2.8973, 2.8973], + [0, 0.04], + [0, 0.04], + ] + + def test_get_leaf_links(self): + # Test the getLeafLinks method + leaf_links = self.model.get_leaf_links() + self.assertEqual(len(leaf_links), 2) # two finger links + + def test_set_link_order(self): + # Test the setLinkOrder method + names = [ + "panda_link0", + "panda_link1", + "panda_link3", + "panda_link2", + "panda_link5", + "panda_link4", + "panda_link7", + "panda_link8", + "panda_link6", + "panda_hand", + ] + self.model.set_link_order(names) + self.assertEqual(self.model.get_link_names(), names) + + def test_set_joint_order(self): + # Test the setJointOrder method + names = [ + "panda_joint1", + "panda_joint2", + "panda_finger_joint1", + "panda_joint4", + "panda_joint3", + "panda_joint5", + "panda_joint7", + "panda_joint6", + "panda_finger_joint2", + ] + self.model.set_joint_order(names) + self.assertEqual(self.model.get_joint_names(), names) + + def test_get_joint_dim(self): + # Test the getJointDim method + for i in range(len(self.model.get_joint_names())): + self.assertEqual(self.model.get_joint_dim(i), 1) # all joints are 1D + + def test_get_joint_parent(self): + names = [ + "panda_joint1", + "panda_joint2", + "panda_finger_joint1", + "panda_joint4", + "panda_joint3", + "panda_joint5", + "panda_joint7", + "panda_joint6", + "panda_finger_joint2", + ] + self.model.set_joint_order(names) + expected = [0, 1, 7, 3, 2, 4, 6, 5, 7] + for i in range(len(self.model.get_joint_names())): + self.assertEqual(self.model.get_joint_parent(i), expected[i]) + + def sample_qpos(self): + qpos = [] + for limit in self.joint_limits: + qpos.append(np.random.uniform(limit[0], limit[1])) + return np.array(qpos) + + def test_two_jacobian_methods(self): + ee_idx = self.model.get_link_names().index("panda_hand") + for local in [True, False]: + for _ in range(10): + qpos = self.sample_qpos() + self.model.compute_forward_kinematics(qpos) + self.model.compute_full_jacobian(qpos) + analytical_jacobian_method1 = self.model.get_link_jacobian( + ee_idx, local=local + ) + analytical_jacobian_method2 = self.model.compute_single_link_jacobian( + qpos, ee_idx, local=local + ) + self.assertTrue( + np.allclose( + analytical_jacobian_method1, analytical_jacobian_method2 + ) + ) + + def get_numerical_jacobian(self, qpos, link_idx, epsilon=1e-3): + jacobian = np.zeros((6, len(qpos))) + for i in range(len(qpos)): + perturbation = np.zeros(len(qpos)) + perturbation[i] = epsilon + self.model.compute_forward_kinematics(qpos + perturbation) + pose_plus = self.model.get_link_pose(link_idx) + self.model.compute_forward_kinematics(qpos - perturbation) + pose_minus = self.model.get_link_pose(link_idx) + + position_diff = pose_plus[:3] - pose_minus[:3] + jacobian[:3, i] = position_diff / (2 * epsilon) + + orientation_plus = pose_plus[3:] + orientation_minus = pose_minus[3:] + # get the difference quaternion + orientation_diff = qmult(orientation_plus, qinverse(orientation_minus)) + # get the axis-angle representation + axis, angle = quat2axangle(orientation_diff) + jacobian[3:, i] = angle / (2 * epsilon) * axis + + self.model.compute_forward_kinematics(qpos) + curr_pos = self.model.get_link_pose(link_idx)[:3] + speed_due_to_rotation = np.cross(jacobian[3:, i], curr_pos) + jacobian[:3, i] -= speed_due_to_rotation + + return jacobian + + def test_link_jacobian(self): + # idea is to calculate the jacobian numerically by perturbing the joint angles one by one + # and comparing the result to the analytical jacobian + ee_idx = self.model.get_link_names().index("panda_hand") + for _ in range(10): + qpos = self.sample_qpos() + self.model.compute_forward_kinematics(qpos) + analytical_jacobian = self.model.compute_single_link_jacobian(qpos, ee_idx) + numerical_jacobian = self.get_numerical_jacobian(qpos, ee_idx) + self.assertTrue( + np.allclose(analytical_jacobian, numerical_jacobian, atol=1e-3) + ) + if __name__ == "__main__": - unittest.main() + unittest.main()