From c4d2e61f37eb097d188ba7161a6af60ce3c90685 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 28 Oct 2024 18:57:29 +0100 Subject: [PATCH 1/7] algo/jacobians: use const_cast_derived() --- include/pinocchio/algorithm/jacobian.hxx | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 32cd99a91..60dc8ddc9 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2015-2024 CNRS INRIA // #ifndef __pinocchio_algorithm_jacobian_hxx__ @@ -56,7 +56,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); + Matrix6xLike & J_ = J.const_cast_derived(); jmodel.jointCols(J_) = data.oMi[i].act(jdata.S()); } }; @@ -88,7 +88,7 @@ namespace pinocchio { Pass::run( model.joints[i], data.joints[i], - ArgsType(model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6x, data.J))); + ArgsType(model, data, q.derived(), data.J.const_cast_derived())); } return data.J; @@ -148,7 +148,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols()); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); - Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); + Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; @@ -189,7 +189,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); - Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); + Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; @@ -277,8 +277,7 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); ::pinocchio::details::translateJointJacobian( - model, data, joint_id, reference_frame, data.J, - PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)); + model, data, joint_id, reference_frame, data.J, J.const_cast_derived()); } template< @@ -319,7 +318,7 @@ namespace pinocchio data.liMi[i] = model.jointPlacements[i] * jdata.M(); data.iMf[parent] = data.liMi[i] * data.iMf[i]; - Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); + Matrix6xLike & J_ = J.const_cast_derived(); jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); } }; @@ -352,8 +351,7 @@ namespace pinocchio { Pass::run( model.joints[i], data.joints[i], - typename Pass::ArgsType( - model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J))); + typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); } } From d60c5e3cf7af542d59c2859da09eda8a147d3426 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 28 Oct 2024 18:57:58 +0100 Subject: [PATCH 2/7] algo/jacobians: fix bug with getJointJacobianTimeVariation --- include/pinocchio/algorithm/jacobian.hxx | 62 ++++++++++++++++++++++-- 1 file changed, 59 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 60dc8ddc9..8f91d2a57 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -469,10 +469,66 @@ namespace pinocchio const DataTpl & data, const JointIndex jointId, const ReferenceFrame rf, - const Eigen::MatrixBase & dJ) + const Eigen::MatrixBase & dJ_) { - ::pinocchio::details::translateJointJacobian( - model, data, jointId, rf, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ)); + typedef DataTpl Data; + typedef typename Data::SE3 SE3; + typedef typename SE3::Vector3 Vector3; + typedef typename Data::Motion Motion; + + PINOCCHIO_CHECK_INPUT_ARGUMENT( + jointId < JointIndex(model.njoints) + && "jointId is larger than the number of joints contained in the model"); + + Matrix6xLike & dJ = dJ_.const_cast_derived(); + ::pinocchio::details::translateJointJacobian(model, data, jointId, rf, data.dJ, dJ); + + // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED + switch (rf) + { + case LOCAL: { + const SE3 & oMjoint = data.oMi[jointId]; + const Motion & v_joint = data.v[jointId]; + const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1; + for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + { + typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; + typedef const MotionRef MotionIn; + + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(j)); + MotionOut v_out(dJ.col(j)); + + v_out -= v_joint.cross(oMjoint.actInv(v_in)); + } + break; + } + case LOCAL_WORLD_ALIGNED: { + const Motion & ov_joint = data.ov[jointId]; + const SE3 & oMjoint = data.oMi[jointId]; + const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1; + for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + { + typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; + typedef const MotionRef MotionIn; + + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(j)); + MotionOut v_out(dJ.col(j)); + + v_out.linear() -= + Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation())) + .cross(v_in.angular()); + } + break; + } + + case WORLD: + default: + break; + } } } // namespace impl From 54d001e41ef7a5ab43478f3b433a3c4ae56d2eb9 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 28 Oct 2024 18:58:26 +0100 Subject: [PATCH 3/7] test/jacobians: enforce correct testing of getJointJacobianTimeVariation --- unittest/joint-jacobian.cpp | 39 ++++++++++++++++++++++++++++++++----- 1 file changed, 34 insertions(+), 5 deletions(-) diff --git a/unittest/joint-jacobian.cpp b/unittest/joint-jacobian.cpp index 8eb7a0a7c..4dc54ffff 100644 --- a/unittest/joint-jacobian.cpp +++ b/unittest/joint-jacobian.cpp @@ -129,12 +129,15 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) Data::SE3 worldMlocal = data.oMi[idx]; worldMlocal.translation().setZero(); + Data::Motion world_v_local = data.ov[idx]; + world_v_local.linear().setZero(); v_idx = (Motion::Vector6)(J * v); BOOST_CHECK(v_idx.isApprox(worldMlocal.act(data_ref.v[idx]))); a_idx = (Motion::Vector6)(J * a + dJ * v); - BOOST_CHECK(a_idx.isApprox(worldMlocal.act(data_ref.a[idx]))); + BOOST_CHECK(a_idx.isApprox( + world_v_local.cross(worldMlocal.act(data_ref.v[idx])) + worldMlocal.act(data_ref.a[idx]))); // compare to finite differencies : WORLD { @@ -184,11 +187,8 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) computeJointJacobians(model, data_ref_plus, q_plus); getJointJacobian(model, data_ref_plus, idx, LOCAL, J_ref_plus); - const Data::SE3 M_plus = data_ref.oMi[idx].inverse() * data_ref_plus.oMi[idx]; - Data::Matrix6x dJ_ref(6, model.nv); - dJ_ref.fill(0.); - dJ_ref = (M_plus.toActionMatrix() * J_ref_plus - J_ref) / alpha; + dJ_ref = (J_ref_plus - J_ref) / alpha; computeJointJacobiansTimeVariation(model, data, q, v); Data::Matrix6x dJ(6, model.nv); @@ -197,6 +197,35 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(alpha))); } + + // compare to finite differencies : LOCAL_WORLD_ALIGNED + { + Data data_ref(model), data_ref_plus(model); + + const double alpha = 1e-8; + Eigen::VectorXd q_plus(model.nq); + q_plus = integrate(model, q, alpha * v); + + Data::Matrix6x J_ref(6, model.nv); + J_ref.fill(0.); + computeJointJacobians(model, data_ref, q); + getJointJacobian(model, data_ref, idx, LOCAL_WORLD_ALIGNED, J_ref); + + Data::Matrix6x J_ref_plus(6, model.nv); + J_ref_plus.fill(0.); + computeJointJacobians(model, data_ref_plus, q_plus); + getJointJacobian(model, data_ref_plus, idx, LOCAL_WORLD_ALIGNED, J_ref_plus); + + Data::Matrix6x dJ_ref(6, model.nv); + dJ_ref = (J_ref_plus - J_ref) / alpha; + + computeJointJacobiansTimeVariation(model, data, q, v); + Data::Matrix6x dJ(6, model.nv); + dJ.fill(0.); + getJointJacobianTimeVariation(model, data, idx, LOCAL_WORLD_ALIGNED, dJ); + + BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(alpha))); + } } BOOST_AUTO_TEST_CASE(test_timings) From eb01753d9ee8938207c930476a2361cc37a4032a Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 28 Oct 2024 19:04:54 +0100 Subject: [PATCH 4/7] algo/frames: remove call to PINOCCHIO_EIGEN_CONST_CAST --- include/pinocchio/algorithm/frames.hxx | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/include/pinocchio/algorithm/frames.hxx b/include/pinocchio/algorithm/frames.hxx index 9faf3badc..79487f0b1 100644 --- a/include/pinocchio/algorithm/frames.hxx +++ b/include/pinocchio/algorithm/frames.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2021 CNRS INRIA +// Copyright (c) 2015-2024 CNRS INRIA // #ifndef __pinocchio_algorithm_frames_hxx__ @@ -171,8 +171,7 @@ namespace pinocchio const typename Data::SE3 oMframe = data.oMi[joint_id] * placement; details::translateJointJacobian( - model, data, joint_id, reference_frame, oMframe, data.J, - PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)); + model, data, joint_id, reference_frame, oMframe, data.J, J.const_cast_derived()); } template< @@ -220,8 +219,7 @@ namespace pinocchio JointIndex parent = joint_support[k]; Pass::run( model.joints[parent], data.joints[parent], - typename Pass::ArgsType( - model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J))); + typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); } if (reference_frame == LOCAL_WORLD_ALIGNED) @@ -229,7 +227,7 @@ namespace pinocchio typename Data::SE3 & oMframe = data.oMf[frameId]; oMframe = data.oMi[joint_id] * frame.placement; - Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); + Matrix6xLike & J_ = J.const_cast_derived(); const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1; for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) @@ -252,8 +250,7 @@ namespace pinocchio { Pass::run( model.joints[i], data.joints[i], - typename Pass::ArgsType( - model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J))); + typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); } break; } @@ -292,7 +289,7 @@ namespace pinocchio oMframe = data.oMi[joint_id] * frame.placement; details::translateJointJacobian( - model, data, joint_id, rf, oMframe, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ)); + model, data, joint_id, rf, oMframe, data.dJ, dJ.const_cast_derived()); } template class JointCollectionTpl> From b2c996bda2c3ca2124b2e0ac047588162131860b Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 28 Oct 2024 19:39:16 +0100 Subject: [PATCH 5/7] algo/frames: fix bug with getFrameJacobianTimeVariation --- include/pinocchio/algorithm/frames.hxx | 54 +++++++++++++++++++++++++- 1 file changed, 53 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/frames.hxx b/include/pinocchio/algorithm/frames.hxx index 79487f0b1..79a54ab2b 100644 --- a/include/pinocchio/algorithm/frames.hxx +++ b/include/pinocchio/algorithm/frames.hxx @@ -270,9 +270,11 @@ namespace pinocchio DataTpl & data, const FrameIndex frame_id, const ReferenceFrame rf, - const Eigen::MatrixBase & dJ) + const Eigen::MatrixBase & dJ_) { assert(model.check(data) && "data is not consistent with model."); + + Matrix6xLike & dJ = dJ_.const_cast_derived(); PINOCCHIO_CHECK_ARGUMENT_SIZE( dJ.cols(), model.nv, "The numbers of columns in the Jacobian matrix does not math the " @@ -281,6 +283,9 @@ namespace pinocchio typedef ModelTpl Model; typedef DataTpl Data; typedef typename Model::Frame Frame; + typedef typename Data::SE3 SE3; + typedef typename SE3::Vector3 Vector3; + typedef typename Data::Motion Motion; const Frame & frame = model.frames[frame_id]; const JointIndex & joint_id = frame.parentJoint; @@ -290,6 +295,53 @@ namespace pinocchio details::translateJointJacobian( model, data, joint_id, rf, oMframe, data.dJ, dJ.const_cast_derived()); + + // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED + switch (rf) + { + case LOCAL: { + const Motion & v_joint = data.v[joint_id]; + const Motion v_frame = frame.placement.actInv(v_joint); + + const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1; + for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + { + typedef typename Data::Matrix6x::ColXpr ColXprIn; + typedef const MotionRef MotionIn; + + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(j)); + MotionOut v_out(dJ.col(j)); + + v_out -= v_frame.cross(oMframe.actInv(v_in)); + } + break; + } + case LOCAL_WORLD_ALIGNED: { + const Motion & ov_joint = data.ov[joint_id]; + const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1; + for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + { + typedef typename Data::Matrix6x::ColXpr ColXprIn; + typedef const MotionRef MotionIn; + + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(j)); + MotionOut v_out(dJ.col(j)); + + v_out.linear() -= + Vector3(ov_joint.linear() + ov_joint.angular().cross(oMframe.translation())) + .cross(v_in.angular()); + } + break; + } + + case WORLD: + default: + break; + } } template class JointCollectionTpl> From 6cb88b2887f86d5a7bc32039bf1b85f8bc118679 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 28 Oct 2024 19:39:48 +0100 Subject: [PATCH 6/7] test/frames: fix testing of getFrameJacobianTimeVariation --- unittest/frames.cpp | 28 ++++++++++------------------ 1 file changed, 10 insertions(+), 18 deletions(-) diff --git a/unittest/frames.cpp b/unittest/frames.cpp index 1883d2e0f..0e76b4762 100644 --- a/unittest/frames.cpp +++ b/unittest/frames.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2016-2020 CNRS INRIA +// Copyright (c) 2016-2024 CNRS INRIA // #include "pinocchio/multibody/model.hpp" @@ -569,14 +569,14 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation) const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]); const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local); - const SE3 & wMf = SE3(data_ref.oMf[idx].rotation(), SE3::Vector3::Zero()); - const Motion & v_ref_local_world_aligned = wMf.act(v_ref_local); + const SE3 wMf = SE3(data_ref.oMf[idx].rotation(), SE3::Vector3::Zero()); + const Motion v_ref_local_world_aligned = wMf.act(v_ref_local); BOOST_CHECK(v_idx.isApprox(v_ref)); Motion a_idx(J * a + dJ * v); - const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]); - const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local); - const Motion & a_ref_local_world_aligned = wMf.act(a_ref_local); + const Motion a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]); + const Motion a_ref = data_ref.oMf[idx].act(a_ref_local); + const Motion a_ref_local_world_aligned = wMf.act(a_ref_local); BOOST_CHECK(a_idx.isApprox(a_ref)); J.fill(0.); @@ -594,12 +594,15 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation) // Regarding to the LOCAL_WORLD_ALIGNED frame getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, J); getFrameJacobianTimeVariation(model, data, idx, LOCAL_WORLD_ALIGNED, dJ); + Data::Motion world_v_frame = data.ov[parent_idx]; + world_v_frame.linear().setZero(); v_idx = (Motion::Vector6)(J * v); BOOST_CHECK(v_idx.isApprox(v_ref_local_world_aligned)); a_idx = (Motion::Vector6)(J * a + dJ * v); - BOOST_CHECK(a_idx.isApprox(a_ref_local_world_aligned)); + BOOST_CHECK( + a_idx.isApprox(world_v_frame.cross(wMf.act(v_ref_local)) + a_ref_local_world_aligned)); // compare to finite differencies { @@ -617,7 +620,6 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation) J_ref_local_world_aligned.fill(0.); computeJointJacobians(model, data_ref, q); updateFramePlacements(model, data_ref); - const SE3 & oMf_q = data_ref.oMf[idx]; getFrameJacobian(model, data_ref, idx, WORLD, J_ref_world); getFrameJacobian(model, data_ref, idx, LOCAL, J_ref_local); getFrameJacobian(model, data_ref, idx, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned); @@ -630,21 +632,11 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation) J_ref_plus_local_world_aligned.fill(0.); computeJointJacobians(model, data_ref_plus, q_plus); updateFramePlacements(model, data_ref_plus); - const SE3 & oMf_q_plus = data_ref_plus.oMf[idx]; getFrameJacobian(model, data_ref_plus, idx, WORLD, J_ref_plus_world); getFrameJacobian(model, data_ref_plus, idx, LOCAL, J_ref_plus_local); getFrameJacobian( model, data_ref_plus, idx, LOCAL_WORLD_ALIGNED, J_ref_plus_local_world_aligned); - // Move J_ref_plus_local to reference frame - J_ref_plus_local = (oMf_q.inverse() * oMf_q_plus).toActionMatrix() * (J_ref_plus_local); - - // Move J_ref_plus_local_world_aligned to reference frame - SE3 oMf_translation = SE3::Identity(); - oMf_translation.translation() = oMf_q_plus.translation() - oMf_q.translation(); - J_ref_plus_local_world_aligned = - oMf_translation.toActionMatrix() * (J_ref_plus_local_world_aligned); - Data::Matrix6x dJ_ref_world(6, model.nv), dJ_ref_local(6, model.nv), dJ_ref_local_world_aligned(6, model.nv); dJ_ref_world.fill(0.); From 831df1f35de518b83350f303600cb1fd40d7e4ff Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 28 Oct 2024 20:15:35 +0100 Subject: [PATCH 7/7] changelog: sync --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 69c90dae5..a8c155ec4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,6 +30,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Fix `pinocchio-test-cpp-mjcf` unittest with Boost 1.86 ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459)) - Fix `pinocchio-test-cpp-constraint-variants` uninitialized values ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459)) - Fix mixing library symbols between Pinocchio scalar bindings ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459)) +- Fix bug for get{Joint,Frame}JacobianTimeVariation ([#2466](https://github.com/stack-of-tasks/pinocchio/pull/2466)) ### Changed