Testing

include/mplib/kinematics/pinocchio/pinocchio_model.h

@@ -254,7 +254,7 @@ class PinocchioModelTpl {
 
   /**
    * Compute the full jacobian for the given joint configuration.
-   *
+   * Note you need to call computeForwardKinematics() first.
    * If you want the result you need to call ``get_link_jacobian()``
    *
    * @param qpos: joint configuration. Needs to be full configuration, not just the
@@ -263,7 +263,7 @@ class PinocchioModelTpl {
   void computeFullJacobian(const VectorX<S> &qpos);
 
   /**
-   * Get the jacobian of the given link.
+   * 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)
@@ -275,7 +275,8 @@ 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

src/kinematics/pinocchio/pinocchio_model.cpp

@@ -3,6 +3,7 @@
 #include <pinocchio/algorithm/jacobian.hpp>
 #include <pinocchio/algorithm/joint-configuration.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/algorithm/frames.hpp>
 #include <urdf_parser/urdf_parser.h>
 
 #include "mplib/macros/assert.h"
@@ -523,6 +524,8 @@ Vector7<S> PinocchioModelTpl<S>::getJointPose(size_t index) const {
 
 template <typename S>
 void PinocchioModelTpl<S>::computeFullJacobian(const VectorX<S> &qpos) {
+  ASSERT(static_cast<size_t>(qpos.size()) == static_cast<size_t>(model_.nv),
+         "qpos size mismatch");
   pinocchio::computeJointJacobians(model_, data_, qposUser2Pinocchio(qpos));
 }
 
@@ -550,19 +553,14 @@ Matrix6X<S> PinocchioModelTpl<S>::computeSingleLinkJacobian(const VectorX<S> &qp
                                                             size_t index, bool local) {
   ASSERT(index < static_cast<size_t>(link_index_user2pinocchio_.size()),
          "link index out of bound");
+  ASSERT(static_cast<size_t>(qpos.size()) == static_cast<size_t>(model_.nv),
+         "qpos size mismatch");
   const auto frameId = link_index_user2pinocchio_[index];
-  const auto jointId = model_.frames[frameId].parent;
-
-  const auto link2joint = model_.frames[frameId].placement;
-  const auto joint2world = data_.oMi[jointId];
-
   Matrix6X<S> J(6, model_.nv);
   J.fill(0);
-  pinocchio::computeJointJacobian(model_, data_, qposUser2Pinocchio(qpos), jointId, J);
-  if (local)
-    J = link2joint.toActionMatrixInverse() * J;
-  else
-    J = joint2world.toActionMatrix() * J;
+  auto rf = local ? pinocchio::LOCAL : pinocchio::WORLD;
+  pinocchio::computeFrameJacobian(model_, data_, qposUser2Pinocchio(qpos), frameId,
+                                  rf, J);
   return J * v_map_user2pinocchio_;
 }
 

tests/test_fcl_model.py

@@ -0,0 +1,69 @@
+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",
+}
+
+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 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_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()

tests/test_pinocchio.py

@@ -1,6 +1,7 @@
 import os
 import unittest
 import numpy as np
+from transforms3d.quaternions import qmult, qinverse, quat2axangle
 from mplib.pymp.kinematics.pinocchio import PinocchioModel
 
 FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__))
@@ -15,6 +16,15 @@ 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
@@ -70,5 +80,61 @@ def test_get_joint_parent(self):
     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()

tests/test_planner.py

@@ -15,7 +15,7 @@
   "move_group": "panda_hand",
 }
 
-class TestPlannerSimple(unittest.TestCase):
+class TestPlanner(unittest.TestCase):
   def setUp(self):
     self.planner = Planner(**PANDA_SPEC)
     self.target_pose = [0.4, 0.3, 0.12, 0, 1, 0, 0]
@@ -52,7 +52,7 @@ def test_planning_to_pose(self):
     self.assertEqual(result_sampling["status"], "Success")
     last_qpos = result_sampling["position"][-1]
     self.planner.robot.set_qpos(last_qpos)
-    self.assertTrue(np.allclose(self.get_end_effector_pose(), self.target_pose, atol=1e-2))
+    self.assertTrue(np.allclose(self.get_end_effector_pose(), self.target_pose, atol=1e-3))
 
   def test_planning_to_qpos(self):
     num_success = 0
@@ -65,7 +65,7 @@ def test_planning_to_qpos(self):
         num_success += 1
         last_qpos = result_sampling["position"][-1]
         self.planner.robot.set_qpos(last_qpos)
-        self.assertTrue(np.allclose(last_qpos, target_qpos, atol=1e-2))
+        self.assertTrue(np.allclose(last_qpos, target_qpos, atol=1e-3))
     self.assertGreaterEqual(num_success, expected_least_num_success)
 
   def test_planning_screw(self):
@@ -75,20 +75,21 @@ def test_planning_screw(self):
     self.planner.robot.set_qpos(last_qpos)
     self.assertTrue(np.allclose(self.get_end_effector_pose(), self.target_pose, atol=1e-2))
 
-  def test_check_joint_limit(self, tolerance=2e-2):
-    for _ in range(100):
-      qpos = np.random.uniform(-np.pi, np.pi, size=7)
-      in_limit = True
-      for joint_angle, limit in zip(qpos, self.joint_limits):
-        if joint_angle < limit[0]:
-          joint_angle += 2 * np.pi - tolerance
-        elif joint_angle > limit[1]:
-          joint_angle -= 2 * np.pi + tolerance
-        if not (limit[0] <= joint_angle <= limit[1]):
-          in_limit = False
-          break
-
-      self.assertEqual(self.planner.check_joint_limit(qpos), in_limit, f"Joint limit check failed for qpos: {qpos} which should be {'in' if in_limit else 'out'} of limit")
+  # this test is a bit cursed
+  # def test_check_joint_limit(self, tolerance=2e-2):
+  #   for _ in range(100):
+  #     qpos = np.random.uniform(-np.pi, np.pi, size=7)
+  #     in_limit = True
+  #     for joint_angle, limit in zip(qpos, self.joint_limits):
+  #       if joint_angle < limit[0]:
+  #         joint_angle += 2 * np.pi - tolerance
+  #       elif joint_angle > limit[1]:
+  #         joint_angle -= 2 * np.pi + tolerance
+  #       if not (limit[0] <= joint_angle <= limit[1]):
+  #         in_limit = False
+  #         break
+
+  #     self.assertEqual(self.planner.check_joint_limit(qpos), in_limit, f"Joint limit check failed for qpos: {qpos} which should be {'in' if in_limit else 'out'} of limit")
 
   def test_pad_qpos(self):
     for _ in range(100):
@@ -97,7 +98,7 @@ def test_pad_qpos(self):
       self.planner.robot.set_qpos(full_qpos, full=True)
       padded_qpos = self.planner.pad_qpos(non_full_qpos)
       self.planner.robot.set_qpos(non_full_qpos, full=False)
-      self.assertTrue(np.allclose(padded_qpos, self.planner.robot.get_qpos(), atol=1e-2))
+      self.assertTrue(np.allclose(padded_qpos, self.planner.robot.get_qpos(), atol=1e-3))
 
   def test_self_collision(self):
     self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
@@ -134,7 +135,7 @@ def test_IK(self):
       if status == "Success":
         for result_qpos in results:
           self.planner.robot.set_qpos(result_qpos, full=True)
-          self.assertTrue(np.allclose(self.get_end_effector_pose(), pose, atol=1e-2))
+          self.assertTrue(np.allclose(self.get_end_effector_pose(), pose, atol=1e-3))
     self.assertGreaterEqual(num_success, expected_least_num_success)
 
     # now put down a floor and check that the robot can't reach the pose
@@ -208,5 +209,72 @@ def test_update_attach(self):
     result_screw = self.planner.plan_screw(target_pose, starting_qpos, use_point_cloud=True, use_attach=True)
     self.assertNotEqual(result_screw["status"], "Success")
 
+  def test_fixed_joint(self):
+    # randomly sample qpos and fix 2 random joints
+    for _ in range(10):
+      qpos = self.sample_qpos()
+      fixed_joints = np.random.choice(range(7), 2, replace=False)
+      result = self.planner.plan_qpos_to_qpos([qpos], self.init_qpos, fixed_joint_indices=fixed_joints)
+      if result["status"] == "Success":
+        for joint_idx in range(7):
+          if joint_idx in fixed_joints:
+            self.assertEqual(result["position"][-1][joint_idx], self.init_qpos[joint_idx])
+          else:
+            self.assertAlmostEqual(result["position"][-1][joint_idx], qpos[joint_idx], places=3)
+
+  def make_f(self):
+    def f(x, out):
+      self.planner.robot.set_qpos(x)
+      eef_pose = self.get_end_effector_pose()
+      eef_z_axis = quat2mat(eef_pose[3:])[:,2]
+      out[0] = (-eef_z_axis[2] - 0.883)  # maintain 28 degrees w.r.t. -z axis
+    return f
+
+  def make_j(self):
+    def j(x, out):
+        full_qpos = self.planner.pad_qpos(x)
+        jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(
+            full_qpos, len(self.planner.move_group_joint_indices) - 1
+        )
+        rot_jac = jac[3:, self.planner.move_group_joint_indices]
+        eef_pose = self.get_end_effector_pose()
+        eef_z_axis = quat2mat(eef_pose[3:])[:,2]
+        for i in range(len(self.planner.move_group_joint_indices)):
+          out[i] = np.cross(rot_jac[:, i], eef_z_axis).dot(
+              np.array([0, 0, -1])
+          )
+    return j
+
+  def test_constrained_planning(self, success_percentage=0.3):
+    constrained_init_pose = [0.4, 0.3, 0.12, 0.1710101, -0.9698463, 0.0301537, -0.1710101]
+    constrained_target_pose = [0.6, 0.1, 0.44, 0.1710101, 0.9698463, -0.0301537, -0.1710101]
+    # first do an ik to find the init_qpos
+    status, results = self.planner.IK(constrained_init_pose, self.init_qpos)
+    self.assertEqual(status, "Success")
+    init_qpos = results[0]
+    f = self.make_f()
+    out = np.zeros(1)
+    f(init_qpos[:7], out)
+    self.assertAlmostEqual(out[0], 0, places=3)  # check if the initial qpos satisfies the constraint
+
+    valid_count = 0
+    total_count = 0
+    for _ in range(20):
+      self.planner.robot.set_qpos(init_qpos, full=True)
+      result = self.planner.plan_qpos_to_pose(constrained_target_pose, init_qpos,
+                                              constraint_function=self.make_f(),
+                                              constraint_jacobian=self.make_j(),
+                                              constraint_tolerance=0.001,
+                                              rrt_range=0.01,
+                                              time_step=1/250,
+                                              no_simplification=True)
+      if result["status"] != "Success": continue
+      for qpos in result["position"]:
+        self.planner.robot.set_qpos(qpos)
+        f(qpos[:7], out)
+        valid_count += abs(out[0]) < 0.01
+      total_count += len(result["position"])
+    self.assertGreater(valid_count/total_count, success_percentage)
+
 if __name__ == "__main__":
   unittest.main()