From ecdbdb249dfb458553cc4e07cbbf0e8c38b5fe5a Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Sun, 19 May 2024 10:21:25 +0200 Subject: [PATCH 1/8] CMake: set compatibility with pinocchio 2 --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 82c15d79f..a764a02a4 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) From 43eba1e2812b891e465b0f07e4b0284fe4adb4f3 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 24 May 2024 16:02:25 +0200 Subject: [PATCH 2/8] unittest: Use the not deprecated GeometryObject constructor --- unittest/factory/cost.cpp | 8 ++++---- unittest/factory/residual.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/unittest/factory/cost.cpp b/unittest/factory/cost.cpp index 3bd3c758b..01496854e 100644 --- a/unittest/factory/cost.cpp +++ b/unittest/factory/cost.cpp @@ -229,16 +229,16 @@ boost::shared_ptr CostModelFactory::create( pinocchio::GeomIndex ig_frame = geometry->addGeometryObject(pinocchio::GeometryObject( "frame", frame_index, - state->get_pinocchio()->frames[frame_index].parent, - CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)), frame_SE3)); + state->get_pinocchio()->frames[frame_index].parent, frame_SE3, + CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)))); 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::Capsule(0, beta)), - frame_SE3_obstacle)); + frame_SE3_obstacle, + CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)))); geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs)); switch (cost_type) { diff --git a/unittest/factory/residual.cpp b/unittest/factory/residual.cpp index 1ea2109a1..f6f45cc48 100644 --- a/unittest/factory/residual.cpp +++ b/unittest/factory/residual.cpp @@ -91,15 +91,15 @@ ResidualModelFactory::create(ResidualModelTypes::Type residual_type, pinocchio::GeomIndex ig_frame = geometry->addGeometryObject(pinocchio::GeometryObject( "frame", frame_index, - state->get_pinocchio()->frames[frame_index].parent, - CollisionGeometryPtr(new hpp::fcl::Sphere(0)), frame_SE3)); + state->get_pinocchio()->frames[frame_index].parent, frame_SE3, + CollisionGeometryPtr(new hpp::fcl::Sphere(0)))); 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)); + .parent, frame_SE3_obstacle, + CollisionGeometryPtr(new hpp::fcl::Sphere(0)))); geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs)); #endif // PINOCCHIO_WITH_HPP_FCL if (nu == std::numeric_limits::max()) { From fa1b641c4b7db747029d87bbaae4ef9213e69c38 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 24 May 2024 16:37:12 +0200 Subject: [PATCH 3/8] Revert "unittest: Use the not deprecated GeometryObject constructor" This reverts commit 43eba1e2812b891e465b0f07e4b0284fe4adb4f3. --- unittest/factory/cost.cpp | 8 ++++---- unittest/factory/residual.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/unittest/factory/cost.cpp b/unittest/factory/cost.cpp index 01496854e..3bd3c758b 100644 --- a/unittest/factory/cost.cpp +++ b/unittest/factory/cost.cpp @@ -229,16 +229,16 @@ boost::shared_ptr CostModelFactory::create( pinocchio::GeomIndex ig_frame = geometry->addGeometryObject(pinocchio::GeometryObject( "frame", frame_index, - state->get_pinocchio()->frames[frame_index].parent, frame_SE3, - CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)))); + state->get_pinocchio()->frames[frame_index].parent, + CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)), 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, - frame_SE3_obstacle, - CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)))); + CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)), + frame_SE3_obstacle)); geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs)); switch (cost_type) { diff --git a/unittest/factory/residual.cpp b/unittest/factory/residual.cpp index f6f45cc48..1ea2109a1 100644 --- a/unittest/factory/residual.cpp +++ b/unittest/factory/residual.cpp @@ -91,15 +91,15 @@ ResidualModelFactory::create(ResidualModelTypes::Type residual_type, pinocchio::GeomIndex ig_frame = geometry->addGeometryObject(pinocchio::GeometryObject( "frame", frame_index, - state->get_pinocchio()->frames[frame_index].parent, frame_SE3, - CollisionGeometryPtr(new hpp::fcl::Sphere(0)))); + state->get_pinocchio()->frames[frame_index].parent, + CollisionGeometryPtr(new 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, frame_SE3_obstacle, - CollisionGeometryPtr(new hpp::fcl::Sphere(0)))); + .parent, + CollisionGeometryPtr(new 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::max()) { From 22b2fc2c92db6e2abe5dfa88d9dedb4e6f95cd06 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Sat, 25 May 2024 22:55:27 +0200 Subject: [PATCH 4/8] unittest: Avoid undefined behavior in test using ResidualModelFactory ResidualModelFactory was allocating the content of a `std::shared_ptr` manually (with a new). On GNU/Linux with G++, the standard library use free to deallocate this memory. This is an undefined behavior that cause random error on at least test_residual unit test. --- unittest/factory/residual.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittest/factory/residual.cpp b/unittest/factory/residual.cpp index 1ea2109a1..2f4143d81 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()) { From 8b8f2b0391e25b7c6dc600db11f7e1abe3772ccf Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Mon, 27 May 2024 09:55:31 +0200 Subject: [PATCH 5/8] ci: Bound cppad version to 20230000 because of incompatibility issues with cppadcodegen --- .github/workflows/conda/conda-env.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/conda/conda-env.yml b/.github/workflows/conda/conda-env.yml index 620a868f6..d7f407a19 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 From 3ad0bbe70dc09fdd65268aa5e54a1e8dd563ae24 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Mon, 27 May 2024 15:43:11 +0200 Subject: [PATCH 6/8] unittest: Remove an undefined behavior in test_costs.py This fix probably hide a bigger issue in the Python binding. --- unittest/bindings/factory.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/unittest/bindings/factory.py b/unittest/bindings/factory.py index e0406394a..0937c6cb6 100644 --- a/unittest/bindings/factory.py +++ b/unittest/bindings/factory.py @@ -747,9 +747,18 @@ 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( + # 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 - )[0] + ) + 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( From 1d2836995a7d6992c1ca4571cc1923128c5032a9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 28 May 2024 08:13:45 +0000 Subject: [PATCH 7/8] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- unittest/bindings/factory.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/unittest/bindings/factory.py b/unittest/bindings/factory.py index 0937c6cb6..9cd0104cb 100644 --- a/unittest/bindings/factory.py +++ b/unittest/bindings/factory.py @@ -755,9 +755,7 @@ def calcDiff(self, data, x, u=None): # self.xref, x, crocoddyl.Jcomponent.second # )[0] - diff = self.state.Jdiff( - self.xref, x, crocoddyl.Jcomponent.second - ) + 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) From 38c95511dcde9ededa0c96eef9fd46d88d785102 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 30 May 2024 15:53:48 +0200 Subject: [PATCH 8/8] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0743b7cde..e07ba64da 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +* 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