Merge pull request #1253 from loco-3d/pin3

Pinocchio v3 compatibility

.github/workflows/conda/conda-env.yml

@@ -11,7 +11,8 @@ dependencies:
   - eigenpy
   - hpp-fcl
   - urdfdom
-  - cppad
+  # last cppadcodegen is not compatible with cppad 20240000
+  - cppad<=20230000
   - cppadcodegen
   - pinocchio
   - ipopt

CHANGELOG.md

@@ -7,6 +7,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 ## [Unreleased]
 
 * Exported version for Python in https://github.com/loco-3d/crocoddyl/pull/1254
+* Added pinocchio 3 preliminary support in https://github.com/loco-3d/crocoddyl/pull/1253
 * Updated CMake packaging in https://github.com/loco-3d/crocoddyl/pull/1249
 * Fixed ruff reported error in https://github.com/loco-3d/crocoddyl/pull/1248
 * Fixed yapf reported errors in https://github.com/loco-3d/crocoddyl/pull/1238

CMakeLists.txt

@@ -233,6 +233,8 @@ if(UNIX)
   target_link_libraries(${PROJECT_NAME} pinocchio::pinocchio)
   target_link_libraries(${PROJECT_NAME} Boost::filesystem Boost::system
                         Boost::serialization)
+  target_compile_definitions(
+    ${PROJECT_NAME} PUBLIC PINOCCHIO_ENABLE_COMPATIBILITY_WITH_VERSION_2)
 
   if(BUILD_WITH_MULTITHREADS)
     target_link_libraries(${PROJECT_NAME} OpenMP::OpenMP_CXX)

unittest/bindings/factory.py

@@ -747,9 +747,16 @@ def calc(self, data, x, u=None):
         data.cost = data.activation.a_value
 
     def calcDiff(self, data, x, u=None):
-        data.residual.Rx[:] = self.state.Jdiff(
-            self.xref, x, crocoddyl.Jcomponent.second
-        )[0]
+        # The old code was looking like this.
+        # But, the std::vector<Eigen::MatrixXd> returned by Jdiff is destroyed
+        # before the assignment.
+        # To avoid this issue, we store the std::vector in a variable.
+        # data.residual.Rx[:] = self.state.Jdiff(
+        #     self.xref, x, crocoddyl.Jcomponent.second
+        # )[0]
+
+        diff = self.state.Jdiff(self.xref, x, crocoddyl.Jcomponent.second)
+        data.residual.Rx[:] = diff[0]
         self.activation.calcDiff(data.activation, data.residual.r)
         data.Lx[:] = np.dot(data.residual.Rx.T, data.activation.Ar)
         data.Lxx[:, :] = np.dot(

unittest/factory/residual.cpp

@@ -92,14 +92,14 @@ ResidualModelFactory::create(ResidualModelTypes::Type residual_type,
       geometry->addGeometryObject(pinocchio::GeometryObject(
           "frame", frame_index,
           state->get_pinocchio()->frames[frame_index].parent,
-          CollisionGeometryPtr(new hpp::fcl::Sphere(0)), frame_SE3));
+          std::make_shared<hpp::fcl::Sphere>(0), frame_SE3));
   pinocchio::GeomIndex ig_obs =
       geometry->addGeometryObject(pinocchio::GeometryObject(
           "obs", state->get_pinocchio()->getFrameId("universe"),
           state->get_pinocchio()
               ->frames[state->get_pinocchio()->getFrameId("universe")]
               .parent,
-          CollisionGeometryPtr(new hpp::fcl::Sphere(0)), frame_SE3_obstacle));
+          std::make_shared<hpp::fcl::Sphere>(0), frame_SE3_obstacle));
   geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
 #endif  // PINOCCHIO_WITH_HPP_FCL
   if (nu == std::numeric_limits<std::size_t>::max()) {