Run formatter

include/mplib/kinematics/pinocchio/pinocchio_model.h

@@ -263,7 +263,8 @@ class PinocchioModelTpl {
   void computeFullJacobian(const VectorX<S> &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

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)

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
 

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()

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):
         """

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)
         )

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):
         """

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)
 

src/kinematics/pinocchio/pinocchio_model.cpp

@@ -1,9 +1,9 @@
 #include "mplib/kinematics/pinocchio/pinocchio_model.h"
 
+#include <pinocchio/algorithm/frames.hpp>
 #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"
@@ -559,8 +559,8 @@ Matrix6X<S> PinocchioModelTpl<S>::computeSingleLinkJacobian(const VectorX<S> &qp
   Matrix6X<S> 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_;
 }