diff --git a/.github/workflows/conda/conda-env.yml b/.github/workflows/conda/conda-env.yml index 620a868f68..d7f407a19f 100644 --- a/.github/workflows/conda/conda-env.yml +++ b/.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 diff --git a/CHANGELOG.md b/CHANGELOG.md index 18462ca4d1..1bd2385229 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 82c15d79f3..a764a02a4b 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/unittest/bindings/factory.py b/unittest/bindings/factory.py index e0406394ae..9cd0104cbe 100644 --- a/unittest/bindings/factory.py +++ b/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 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( diff --git a/unittest/factory/residual.cpp b/unittest/factory/residual.cpp index 1ea2109a1c..2f4143d814 100644 --- a/unittest/factory/residual.cpp +++ b/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(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(0), frame_SE3_obstacle)); geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs)); #endif // PINOCCHIO_WITH_HPP_FCL if (nu == std::numeric_limits::max()) {