Unify pose; Support multi-shape Physx components in SapienPlanningWorld conversion

docs/source/reference/index.rst

@@ -8,6 +8,7 @@ API Reference
    PlanningWorld
    core/ArticulatedModel
    core/AttachedBody
+   utils/pose
    utils/random
 
 .. toctree::

docs/source/reference/utils/pose.rst

@@ -0,0 +1,7 @@
+``Pose``
+-------------------------
+
+.. autoclass:: mplib.pymp.Pose
+   :members:
+   :undoc-members:
+   :show-inheritance:

docs/source/tutorials/plan_a_path.rst

@@ -92,7 +92,7 @@ In this example, we create some boxes inside the simulation like so:
    :start-after: # boxes ankor
    :end-before: # boxes ankor end
 
-We then find the target poses needed to reach the boxes. The poses are specified in [x,y,z,qw,qx,qy,qz]:
+We then find the target poses needed to reach the boxes.
 
 .. literalinclude:: ../../../mplib/examples/demo.py
    :dedent: 0

include/mplib/collision_detection/fcl/collision_common.h

@@ -9,6 +9,7 @@
 #include "mplib/collision_detection/fcl/types.h"
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
+#include "mplib/utils/pose.h"
 
 namespace mplib::collision_detection::fcl {
 
@@ -42,9 +43,9 @@ struct FCLObject {
    * @param shapes: all collision shapes as a vector of ``fcl::CollisionObjectPtr``
    * @param shape_poses: relative poses from this FCLObject to each collision shape
    */
-  FCLObject(const std::string &name, const Isometry3<S> &pose,
-            const std::vector<fcl::CollisionObjectPtr<S>> &shapes,
-            const std::vector<Isometry3<S>> &shape_poses);
+  FCLObject(const std::string &name_, const Pose<S> &pose_,
+            const std::vector<fcl::CollisionObjectPtr<S>> &shapes_,
+            const std::vector<Pose<S>> &shape_poses_);
 
   std::string name;   ///< Name of this FCLObject
   Isometry3<S> pose;  ///< Pose of this FCLObject. All shapes are relative to this pose

include/mplib/collision_detection/fcl/fcl_model.h

@@ -12,7 +12,7 @@
 
 #include "mplib/collision_detection/fcl/collision_common.h"
 #include "mplib/macros/class_forward.h"
-#include "mplib/types.h"
+#include "mplib/utils/pose.h"
 
 namespace mplib::collision_detection::fcl {
 
@@ -125,14 +125,7 @@ class FCLModelTpl {
    *
    * @param link_poses: list of link poses in the order of the link order
    */
-  void updateCollisionObjects(const std::vector<Vector7<S>> &link_poses) const;
-
-  /**
-   * Update the collision objects of the FCL model.
-   *
-   * @param link_poses: list of link poses in the order of the link order
-   */
-  void updateCollisionObjects(const std::vector<Isometry3<S>> &link_poses) const;
+  void updateCollisionObjects(const std::vector<Pose<S>> &link_poses) const;
 
   /**
    * Perform self-collision checking.

include/mplib/core/articulated_model.h

@@ -9,7 +9,7 @@
 #include "mplib/kinematics/types.h"
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
-#include "mplib/utils/conversion.h"
+#include "mplib/utils/pose.h"
 
 namespace mplib {
 
@@ -66,6 +66,7 @@ class ArticulatedModelTpl {
    * @param verbose: print debug information. Default: ``false``.
    * @return: a unique_ptr to ArticulatedModel
    */
+  // TODO(merge): remove pair (link name is not needed)
   static std::unique_ptr<ArticulatedModelTpl<S>> createFromURDFString(
       const std::string &urdf_string, const std::string &srdf_string,
       const std::vector<std::pair<std::string, collision_detection::FCLObjectPtr<S>>>
@@ -176,7 +177,7 @@ class ArticulatedModelTpl {
   const VectorX<S> &getQpos() const { return current_qpos_; }
 
   /**
-   * Let the planner know the current joint positions.
+   * Set the current joint positions and update all collision links in the ``FCLModel``.
    *
    * @param qpos: current qpos of all active joints or just the move group joints
    * @param full: whether to set the full qpos or just the move group qpos.
@@ -186,18 +187,21 @@ class ArticulatedModelTpl {
   void setQpos(const VectorX<S> &qpos, bool full = false);
 
   /**
-   * Get the base pose of the robot.
+   * Get the base (root) pose of the robot.
    *
-   * @return: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
+   * @return: base pose of the robot
    */
-  Vector7<S> getBasePose() const { return toPoseVec<S>(base_pose_); }
+  Pose<S> getBasePose() const { return Pose<S>(base_pose_); }
 
   /**
-   * Set the base pose of the robot.
+   * Set the base pose of the robot and update all collision links in the ``FCLModel``.
    *
-   * @param pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
+   * @param pose: base pose of the robot
    */
-  void setBasePose(const Vector7<S> &pose);
+  void setBasePose(const Pose<S> &pose) {
+    base_pose_ = pose.toIsometry();
+    setQpos(current_qpos_, true);  // update all collision links in the fcl_model_
+  }
 
   /**
    * Update the SRDF file to disable self-collisions.

include/mplib/core/attached_body.h

@@ -8,7 +8,7 @@
 #include "mplib/kinematics/types.h"
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
-#include "mplib/utils/conversion.h"
+#include "mplib/utils/pose.h"
 
 namespace mplib {
 
@@ -42,7 +42,7 @@ class AttachedBodyTpl {
    */
   AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object,
                   const ArticulatedModelPtr &attached_articulation,
-                  int attached_link_id, const Isometry3<S> &pose,
+                  int attached_link_id, const Pose<S> &pose,
                   const std::vector<std::string> &touch_links = {});
 
   /// @brief Gets the attached object name
@@ -51,10 +51,10 @@ class AttachedBodyTpl {
   /// @brief Gets the attached object (``FCLObjectPtr``)
   FCLObjectPtr getObject() const { return object_; }
 
-  /// @brief Gets the articulation this body is attached to
+  /// @brief Gets the articulation that this body is attached to
   ArticulatedModelPtr getAttachedArticulation() const { return attached_articulation_; }
 
-  /// @brief Gets the articulation this body is attached to
+  /// @brief Gets the articulation link id that this body is attached to
   int getAttachedLinkId() const { return attached_link_id_; }
 
   /// @brief Gets the attached pose (relative pose from attached link to object)
@@ -63,11 +63,14 @@ class AttachedBodyTpl {
   /// @brief Sets the attached pose (relative pose from attached link to object)
   void setPose(const Isometry3<S> &pose) { pose_ = pose; }
 
-  /// @brief Gets the global pose of the attached object
-  Isometry3<S> getGlobalPose() const {
-    return toIsometry(pinocchio_model_->getLinkPose(attached_link_id_)) * pose_;
+  /// @brief Gets the global pose of the articulation link that this body is attached to
+  Isometry3<S> getAttachedLinkGlobalPose() const {
+    return pinocchio_model_->getLinkPose(attached_link_id_);
   }
 
+  /// @brief Gets the global pose of the attached object
+  Isometry3<S> getGlobalPose() const { return getAttachedLinkGlobalPose() * pose_; }
+
   /// @brief Updates the global pose of the attached object using current state
   void updatePose() const;
 

include/mplib/kinematics/kdl/kdl_model.h

@@ -9,6 +9,7 @@
 
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
+#include "mplib/utils/pose.h"
 
 namespace mplib::kinematics::kdl {
 
@@ -30,19 +31,18 @@ class KDLModelTpl {
   const std::string &getTreeRootName() const { return tree_root_name_; }
 
   std::tuple<VectorX<S>, int> chainIKLMA(size_t index, const VectorX<S> &q0,
-                                         const Vector7<S> &pose) const;
+                                         const Pose<S> &pose) const;
 
   std::tuple<VectorX<S>, int> chainIKNR(size_t index, const VectorX<S> &q0,
-                                        const Vector7<S> &pose) const;
+                                        const Pose<S> &pose) const;
 
   std::tuple<VectorX<S>, int> chainIKNRJL(size_t index, const VectorX<S> &q0,
-                                          const Vector7<S> &pose,
-                                          const VectorX<S> &q_min,
+                                          const Pose<S> &pose, const VectorX<S> &q_min,
                                           const VectorX<S> &q_max) const;
 
   std::tuple<VectorX<S>, int> TreeIKNRJL(const std::vector<std::string> endpoints,
                                          const VectorX<S> &q0,
-                                         const std::vector<Vector7<S>> &poses,
+                                         const std::vector<Pose<S>> &poses,
                                          const VectorX<S> &q_min,
                                          const VectorX<S> &q_max) const;
 

include/mplib/kinematics/pinocchio/pinocchio_model.h

@@ -14,6 +14,7 @@
 #include "mplib/kinematics/pinocchio/types.h"
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
+#include "mplib/utils/pose.h"
 
 namespace mplib::kinematics::pinocchio {
 
@@ -266,15 +267,15 @@ class PinocchioModelTpl {
   void computeForwardKinematics(const VectorX<S> &qpos);
 
   /**
-   * Get the pose of the given link.
+   * Get the pose of the given link in robot's base (root) frame.
    *
    * @param index: index of the link (in the order you passed to the constructor or the
    *    default order)
-   * @return: pose of the link [x, y, z, qw, qx, qy, qz]
+   * @return: pose of the link in robot's base (root) frame.
    */
-  Vector7<S> getLinkPose(size_t index) const;
+  Isometry3<S> getLinkPose(size_t index) const;
 
-  Vector7<S> getJointPose(size_t index) const;  // TODO: not same as sapien
+  Isometry3<S> getJointPose(size_t index) const;  // TODO: not same as sapien
 
   /**
    * Compute the full jacobian for the given joint configuration.
@@ -318,7 +319,7 @@ class PinocchioModelTpl {
    *
    * @param index: index of the link (in the order you passed to the constructor or the
    *    default order)
-   * @param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+   * @param pose: desired pose of the link
    * @param q_init: initial joint configuration
    * @param mask: if the value at a given index is ``true``, the joint is *not* used in
    *    the IK
@@ -329,7 +330,7 @@ class PinocchioModelTpl {
    * @return: joint configuration
    */
   std::tuple<VectorX<S>, bool, Vector6<S>> computeIKCLIK(
-      size_t index, const Vector7<S> &pose, const VectorX<S> &q_init,
+      size_t index, const Pose<S> &pose, const VectorX<S> &q_init,
       const std::vector<bool> &mask, double eps = 1e-5, int max_iter = 1000,
       double dt = 1e-1, double damp = 1e-12);
 
@@ -339,7 +340,7 @@ class PinocchioModelTpl {
    *
    * @param index: index of the link (in the order you passed to the constructor or the
    *    default order)
-   * @param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+   * @param pose: desired pose of the link
    * @param q_init: initial joint configuration
    * @param q_min: minimum joint configuration
    * @param q_max: maximum joint configuration
@@ -350,7 +351,7 @@ class PinocchioModelTpl {
    * @return: joint configuration
    */
   std::tuple<VectorX<S>, bool, Vector6<S>> computeIKCLIKJL(
-      size_t index, const Vector7<S> &pose, const VectorX<S> &q_init,
+      size_t index, const Pose<S> &pose, const VectorX<S> &q_init,
       const VectorX<S> &qmin, const VectorX<S> &qmax, double eps = 1e-5,
       int max_iter = 1000, double dt = 1e-1, double damp = 1e-12);
 

include/mplib/planning_world.h

@@ -10,6 +10,7 @@
 #include "mplib/core/attached_body.h"
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
+#include "mplib/utils/pose.h"
 
 namespace mplib {
 
@@ -265,8 +266,7 @@ class PlanningWorldTpl {
    *  or if planned articulation with given name does not exist
    */
   void attachObject(const std::string &name, const std::string &art_name, int link_id,
-                    const Vector7<S> &pose,
-                    const std::vector<std::string> &touch_links);
+                    const Pose<S> &pose, const std::vector<std::string> &touch_links);
 
   /**
    * Attaches existing normal object to specified link of articulation at given pose.
@@ -284,7 +284,7 @@ class PlanningWorldTpl {
    *  or if planned articulation with given name does not exist
    */
   void attachObject(const std::string &name, const std::string &art_name, int link_id,
-                    const Vector7<S> &pose);
+                    const Pose<S> &pose);
 
   /**
    * Attaches given object (w/ p_geom) to specified link of articulation at given pose.
@@ -299,7 +299,7 @@ class PlanningWorldTpl {
    * @param touch_links: link names that the attached object touches
    */
   void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom,
-                    const std::string &art_name, int link_id, const Vector7<S> &pose,
+                    const std::string &art_name, int link_id, const Pose<S> &pose,
                     const std::vector<std::string> &touch_links);
 
   /**
@@ -316,7 +316,7 @@ class PlanningWorldTpl {
    * @param pose: attached pose (relative pose from attached link to object)
    */
   void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom,
-                    const std::string &art_name, int link_id, const Vector7<S> &pose);
+                    const std::string &art_name, int link_id, const Pose<S> &pose);
 
   /**
    * Attaches given sphere to specified link of articulation (auto touch_links)
@@ -327,7 +327,7 @@ class PlanningWorldTpl {
    * @param pose: attached pose (relative pose from attached link to object)
    */
   void attachSphere(S radius, const std::string &art_name, int link_id,
-                    const Vector7<S> &pose);
+                    const Pose<S> &pose);
 
   /**
    * Attaches given box to specified link of articulation (auto touch_links)
@@ -338,7 +338,7 @@ class PlanningWorldTpl {
    * @param pose: attached pose (relative pose from attached link to object)
    */
   void attachBox(const Vector3<S> &size, const std::string &art_name, int link_id,
-                 const Vector7<S> &pose);
+                 const Pose<S> &pose);
 
   /**
    * Attaches given mesh to specified link of articulation (auto touch_links)
@@ -349,7 +349,7 @@ class PlanningWorldTpl {
    * @param pose: attached pose (relative pose from attached link to object)
    */
   void attachMesh(const std::string &mesh_path, const std::string &art_name,
-                  int link_id, const Vector7<S> &pose);
+                  int link_id, const Pose<S> &pose);
 
   /**
    * Detaches object with given name.

include/mplib/types.h

@@ -14,9 +14,6 @@ using Vector4 = Eigen::Matrix<S, 4, 1>;
 template <typename S>
 using Vector6 = Eigen::Matrix<S, 6, 1>;
 
-template <typename S>
-using Vector7 = Eigen::Matrix<S, 7, 1>;
-
 template <typename S>
 using VectorX = Eigen::Matrix<S, Eigen::Dynamic, 1>;
 

include/mplib/utils/conversion.h

@@ -9,14 +9,6 @@
 
 namespace mplib {
 
-/// Converts an Eigen::Isometry3 matrix to a pose_vec [px, py, pz, qw, qx, qy, qz]
-template <typename S>
-Vector7<S> toPoseVec(const Isometry3<S> &pose);
-
-/// Converts a pose_vec [px, py, pz, qw, qx, qy, qz] to an Eigen::Isometry3 matrix
-template <typename S>
-Isometry3<S> toIsometry(const Vector7<S> &pose_vec);
-
 template <typename S>
 Isometry3<S> toIsometry(const pinocchio::SE3Tpl<S> &T);
 
@@ -37,8 +29,6 @@ pinocchio::InertiaTpl<S> convertInertial(const urdf::InertialSharedPtr &Y);
 
 // Explicit Template Instantiation Declaration =========================================
 #define DECLARE_TEMPLATE_CONVERSION(S)                                       \
-  extern template Vector7<S> toPoseVec<S>(const Isometry3<S> &pose);         \
-  extern template Isometry3<S> toIsometry<S>(const Vector7<S> &pose_vec);    \
   extern template Isometry3<S> toIsometry<S>(const pinocchio::SE3Tpl<S> &T); \
   extern template Isometry3<S> toIsometry<S>(const urdf::Pose &M);           \
   extern template pinocchio::SE3Tpl<S> toSE3<S>(const Isometry3<S> &T);      \