pinnochio 3 support

legubiao · Jan 16, 2025 · 3d5ecda · 3d5ecda
1 parent 446dccb
commit 3d5ecda
Show file tree

Hide file tree

Showing 12 changed files with 722 additions and 810 deletions.
diff --git a/.gitmodules b/.gitmodules
@@ -1,9 +1,6 @@
 [submodule "ocs2_robotic_assets"]
 	path = submodules/ocs2_robotic_assets
 	url = https://github.com/zhengxiang94/ocs2_robotic_assets
-[submodule "pinocchio"]
-	path = submodules/pinocchio
-	url = https://github.com/zhengxiang94/pinocchio.git
 [submodule "submodules/plane_segmentation_ros2"]
 	path = submodules/plane_segmentation_ros2
 	url = https://github.com/legubiao/plane_segmentation_ros2
diff --git a/README.md b/README.md
@@ -14,6 +14,7 @@ ROS2 and modern cmake. Below is the current todolist of the project:
 - [x] tinyxml2 problem in Ubuntu 24.04 ROS2 Jazzy
 - [x] raisim demo
 - [x] mpc_net demo
+- [x] **[2025-01-16]** Pinnochio 3 support, removed hpp-fcl dependency
 
 The IDE I used is CLion, you can follow the [guide](https://www.jetbrains.com/help/clion/ros2-tutorial.html) to set up
 the IDE.
@@ -46,6 +47,8 @@ Tested system and ROS2 version:
 * C++ compiler with C++17 support
 * Eigen (v3.4)
 * Boost C++ (v1.74)
+* [Pinocchio](https://stack-of-tasks.github.io/pinocchio/download.html)
+> **Warm Reminder**: Please follow the guide in the link to install Pinocchio before building the project
 
 ### 2.3 Clone Repositories
 

diff --git a/robotics/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp b/robotics/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp
diff --git a/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt b/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt
@@ -6,7 +6,6 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 set(dependencies
         ocs2_robotic_tools
-        hpp-fcl
         pinocchio
         urdf
         urdfdom
@@ -16,8 +15,6 @@ find_package(ament_cmake REQUIRED)
 find_package(ocs2_robotic_tools REQUIRED)
 find_package(urdf REQUIRED)
 find_package(urdfdom REQUIRED)
-
-find_package(hpp-fcl REQUIRED)
 find_package(pinocchio REQUIRED)
 
 # Add pinocchio configurations

diff --git a/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp b/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp
diff --git a/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp b/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp
diff --git a/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp b/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp
@@ -33,70 +33,60 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace ocs2 {
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-template <>
-template <>
-PinocchioInterfaceCppAd PinocchioInterface::toCppAd() const {
-  auto cppAdModel = getModel().cast<ad_scalar_t>();
+    template<>
+    template<>
+    PinocchioInterfaceCppAd PinocchioInterface::toCppAd() const {
+        auto cppAdModel = getModel().cast<ad_scalar_t>();
 
-  // TODO (rgrandia) : remove after bug fix. The cast function forgets to copy this member.
-  cppAdModel.supports = getModel().supports;
+        // TODO (rgrandia) : remove after bug fix. The cast function forgets to copy this member.
+        cppAdModel.supports = getModel().supports;
 
-  return PinocchioInterfaceCppAd(cppAdModel);
-}
-
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-// explicit instantiation
-template class PinocchioInterfaceTpl<scalar_t>;
-
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-std::ostream& operator<<(std::ostream& os, const PinocchioInterface& p) {
-  const auto& model = p.getModel();
-  os << "model.nv = " << model.nv << '\n';
-  os << "model.nq = " << model.nq << '\n';
-  os << "model.njoints = " << model.njoints << '\n';
-  os << "model.nbodies = " << model.nbodies << '\n';
-  os << "model.nframes = " << model.nframes << '\n';
-
-  os << "\nJoints:\n";
-  for (int k = 0; k < model.njoints; ++k) {
-    os << std::setw(20) << model.names[k] << ":  ";
-    os << " ID = " << k;
-    os << '\n';
-  }
-
-  os << "\nFrames:\n";
-  for (int k = 0; k < model.nframes; ++k) {
-    os << std::setw(20) << model.frames[k].name << ":  ";
-    os << " ID = " << k;
-    os << ", parent = " << model.frames[k].parent;
-    os << ", type = ";
-
-    std::string frameType;
-    if ((model.frames[k].type & pinocchio::FrameType::OP_FRAME) != 0) {
-      frameType += "OP_FRAME ";
+        return PinocchioInterfaceCppAd(cppAdModel);
     }
-    if ((model.frames[k].type & pinocchio::FrameType::JOINT) != 0) {
-      frameType += "JOINT ";
-    }
-    if ((model.frames[k].type & pinocchio::FrameType::FIXED_JOINT) != 0) {
-      frameType += "FIXED_JOINT ";
-    }
-    if ((model.frames[k].type & pinocchio::FrameType::BODY) != 0) {
-      frameType += "BODY ";
-    }
-    if ((model.frames[k].type & pinocchio::FrameType::SENSOR) != 0) {
-      frameType += "SENSOR ";
-    }
-    os << "\"" << frameType << "\"\n";
-  }
-  return os;
-}
 
-}  // namespace ocs2
+    // explicit instantiation
+    template class PinocchioInterfaceTpl<scalar_t>;
+
+    std::ostream &operator<<(std::ostream &os, const PinocchioInterface &p) {
+        const auto &model = p.getModel();
+        os << "model.nv = " << model.nv << '\n';
+        os << "model.nq = " << model.nq << '\n';
+        os << "model.njoints = " << model.njoints << '\n';
+        os << "model.nbodies = " << model.nbodies << '\n';
+        os << "model.nframes = " << model.nframes << '\n';
+
+        os << "\nJoints:\n";
+        for (int k = 0; k < model.njoints; ++k) {
+            os << std::setw(20) << model.names[k] << ":  ";
+            os << " ID = " << k;
+            os << '\n';
+        }
+
+        os << "\nFrames:\n";
+        for (int k = 0; k < model.nframes; ++k) {
+            os << std::setw(20) << model.frames[k].name << ":  ";
+            os << " ID = " << k;
+            os << ", parent = " << model.frames[k].parentJoint;
+            os << ", type = ";
+
+            std::string frameType;
+            if ((model.frames[k].type & pinocchio::FrameType::OP_FRAME) != 0) {
+                frameType += "OP_FRAME ";
+            }
+            if ((model.frames[k].type & pinocchio::FrameType::JOINT) != 0) {
+                frameType += "JOINT ";
+            }
+            if ((model.frames[k].type & pinocchio::FrameType::FIXED_JOINT) != 0) {
+                frameType += "FIXED_JOINT ";
+            }
+            if ((model.frames[k].type & pinocchio::FrameType::BODY) != 0) {
+                frameType += "BODY ";
+            }
+            if ((model.frames[k].type & pinocchio::FrameType::SENSOR) != 0) {
+                frameType += "SENSOR ";
+            }
+            os << "\"" << frameType << "\"\n";
+        }
+        return os;
+    }
+} // namespace ocs2
diff --git a/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterfaceCppAd.cpp b/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterfaceCppAd.cpp
@@ -31,8 +31,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_pinocchio_interface/implementation/PinocchioInterface.h>
 
 namespace ocs2 {
-
-// explicit instantiation
-template class PinocchioInterfaceTpl<ad_scalar_t>;
-
-}  // namespace ocs2
+    // explicit instantiation
+    template class PinocchioInterfaceTpl<ad_scalar_t>;
+} // namespace ocs2
diff --git a/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp b/robotics/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp
@@ -36,71 +36,50 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace ocs2 {
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-PinocchioInterface getPinocchioInterfaceFromUrdfFile(const std::string& urdfFile) {
-  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(urdfFile);
-  if (urdfTree != nullptr) {
-    return getPinocchioInterfaceFromUrdfModel(urdfTree);
-  } else {
-    throw std::invalid_argument("The file " + urdfFile + " does not contain a valid URDF model.");
-  }
-}
+    PinocchioInterface getPinocchioInterfaceFromUrdfFile(const std::string &urdfFile) {
+        urdf::ModelInterfaceSharedPtr urdfTree = urdf::parseURDFFile(urdfFile);
+        if (urdfTree != nullptr) {
+            return getPinocchioInterfaceFromUrdfModel(urdfTree);
+        }
+        throw std::invalid_argument("The file " + urdfFile + " does not contain a valid URDF model.");
+    }
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-PinocchioInterface getPinocchioInterfaceFromUrdfFile(const std::string& urdfFile, const PinocchioInterface::JointModel& rootJoint) {
-  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(urdfFile);
-  if (urdfTree != nullptr) {
-    return getPinocchioInterfaceFromUrdfModel(urdfTree, rootJoint);
-  } else {
-    throw std::invalid_argument("The file " + urdfFile + " does not contain a valid URDF model.");
-  }
-}
+    PinocchioInterface getPinocchioInterfaceFromUrdfFile(const std::string &urdfFile,
+                                                         const PinocchioInterface::JointModel &rootJoint) {
+        urdf::ModelInterfaceSharedPtr urdfTree = urdf::parseURDFFile(urdfFile);
+        if (urdfTree != nullptr) {
+            return getPinocchioInterfaceFromUrdfModel(urdfTree, rootJoint);
+        }
+        throw std::invalid_argument("The file " + urdfFile + " does not contain a valid URDF model.");
+    }
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-PinocchioInterface getPinocchioInterfaceFromUrdfString(const std::string& xmlString) {
-  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
-  if (urdfTree != nullptr) {
-    return getPinocchioInterfaceFromUrdfModel(urdfTree);
-  } else {
-    throw std::invalid_argument("The XML stream does not contain a valid URDF model.");
-  }
-}
+    PinocchioInterface getPinocchioInterfaceFromUrdfString(const std::string &xmlString) {
+        urdf::ModelInterfaceSharedPtr urdfTree = urdf::parseURDF(xmlString);
+        if (urdfTree != nullptr) {
+            return getPinocchioInterfaceFromUrdfModel(urdfTree);
+        }
+        throw std::invalid_argument("The XML stream does not contain a valid URDF model.");
+    }
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-PinocchioInterface getPinocchioInterfaceFromUrdfString(const std::string& xmlString, const PinocchioInterface::JointModel& rootJoint) {
-  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
-  if (urdfTree != nullptr) {
-    return getPinocchioInterfaceFromUrdfModel(urdfTree, rootJoint);
-  } else {
-    throw std::invalid_argument("The XML stream does not contain a valid URDF model.");
-  }
-}
+    PinocchioInterface getPinocchioInterfaceFromUrdfString(const std::string &xmlString,
+                                                           const PinocchioInterface::JointModel &rootJoint) {
+        urdf::ModelInterfaceSharedPtr urdfTree = urdf::parseURDF(xmlString);
+        if (urdfTree != nullptr) {
+            return getPinocchioInterfaceFromUrdfModel(urdfTree, rootJoint);
+        }
+        throw std::invalid_argument("The XML stream does not contain a valid URDF model.");
+    }
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-PinocchioInterface getPinocchioInterfaceFromUrdfModel(const std::shared_ptr<::urdf::ModelInterface>& urdfTree) {
-  pinocchio::ModelTpl<scalar_t> model;
-  pinocchio::urdf::buildModel(urdfTree, model);
-  return PinocchioInterface(model, urdfTree);
-}
+    PinocchioInterface getPinocchioInterfaceFromUrdfModel(const std::shared_ptr<urdf::ModelInterface> &urdfTree) {
+        pinocchio::ModelTpl<scalar_t> model;
+        pinocchio::urdf::buildModel(urdfTree, model);
+        return PinocchioInterface(model, urdfTree);
+    }
 
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-PinocchioInterface getPinocchioInterfaceFromUrdfModel(const std::shared_ptr<::urdf::ModelInterface>& urdfTree,
-                                                      const PinocchioInterface::JointModel& rootJoint) {
-  pinocchio::ModelTpl<scalar_t> model;
-  pinocchio::urdf::buildModel(urdfTree, rootJoint, model);
-  return PinocchioInterface(model, urdfTree);
-}
-
-}  // namespace ocs2
+    PinocchioInterface getPinocchioInterfaceFromUrdfModel(const std::shared_ptr<urdf::ModelInterface> &urdfTree,
+                                                          const PinocchioInterface::JointModel &rootJoint) {
+        pinocchio::ModelTpl<scalar_t> model;
+        pinocchio::urdf::buildModel(urdfTree, rootJoint, model);
+        return PinocchioInterface(model, urdfTree);
+    }
+} // namespace ocs2
diff --git a/robotics/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp b/robotics/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp
@@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pinocchio/multibody/geometry.hpp>
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/parsers/urdf.hpp>
+#include <pinocchio/collision/distance.hpp>
 
 #ifdef URDFDOM_VERSION_GT_4
 #include <tinyxml2.h>
@@ -68,7 +69,7 @@ namespace ocs2 {
         const PinocchioInterface &pinocchioInterface) const {
         pinocchio::GeometryData geometryData(*geometryModelPtr_);
 
-        pinocchio::updateGeometryPlacements(pinocchioInterface.getModel(),
+        updateGeometryPlacements(pinocchioInterface.getModel(),
                                             pinocchioInterface.getData(),
                                             *geometryModelPtr_, geometryData);
         pinocchio::computeDistances(*geometryModelPtr_, geometryData);

diff --git a/robotics/ocs2_pinocchio/ocs2_self_collision/src/SelfCollision.cpp b/robotics/ocs2_pinocchio/ocs2_self_collision/src/SelfCollision.cpp
@@ -93,7 +93,7 @@ namespace ocs2 {
             const vector3_t joint2Position = data.oMi[joint2].translation();
             const vector3_t pt2Offset = distanceArray[i].nearest_points[1] - joint2Position;
             matrix_t joint2Jacobian = matrix_t::Zero(6, model.nv);
-            pinocchio::getJointJacobian(model, data, joint2, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
+            getJointJacobian(model, data, joint2, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
                                         joint2Jacobian);
             const matrix_t pt2Jacobian = joint2Jacobian.topRows(3) - skewSymmetricMatrix(pt2Offset) * joint2Jacobian.
                                          bottomRows(3);

diff --git a/submodules/pinocchio b/submodules/pinocchio