From c7c198ff324bb164415d6f70e136a4646601bdf3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 3 Oct 2024 18:13:42 +0200 Subject: [PATCH] Make code compile with CppAd --- include/pinocchio/algorithm/check.hxx | 2 +- include/pinocchio/algorithm/crba.hxx | 52 +++++++++++++------ include/pinocchio/algorithm/jacobian.hxx | 4 +- include/pinocchio/multibody/data.hxx | 14 +++-- .../joint/joint-common-operations.hpp | 12 +++-- .../pinocchio/multibody/joint/joint-mimic.hpp | 3 +- unittest/cppad/joints.cpp | 25 ++++----- 7 files changed, 71 insertions(+), 41 deletions(-) diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 94783bf975..f297b94591 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -160,7 +160,7 @@ namespace pinocchio for (JointIndex j = 1; int(j) < model.njoints; ++j) { - if (boost::get(&model.joints[j])) + if (boost::get>(&model.joints[j])) continue; JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 8e88536edf..979007eaf5 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -60,9 +60,16 @@ namespace pinocchio }; /// \brief Patch to the crba algorithm for joint mimic (in local convention) - template + template< + typename JointModel, + typename Scalar, + int Options, + template + class JointCollectionTpl> static inline void mimic_patch_CrbaWorldConventionBackwardStep( - const JointModelBase &, const Model &, Data &) + const JointModelBase &, + const ModelTpl &, + DataTpl &) { } @@ -73,20 +80,19 @@ namespace pinocchio template class JointCollectionTpl> static inline void mimic_patch_CrbaWorldConventionBackwardStep( const JointModelBase> & jmodel, - const Model & model, - Data & data) + const ModelTpl & model, + DataTpl & data) { - typedef JointModelMimicTpl JointModel; const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); - JointIndex ancestor_prim, ancestor_sec; + size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor - for (int k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) + for (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) { const JointIndex i = model.supports[secondary_id].at(k); @@ -102,7 +108,7 @@ namespace pinocchio * model.joints.at(i).jointJacCols(data.J); } // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint - for (int k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) + for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { const JointIndex i = model.supports[primary_id].at(k); jmodel.jointVelCols(data.M) @@ -179,10 +185,17 @@ namespace pinocchio } }; - /// \brief Patch to the crba algorithm for joint mimic (in local convention) - template + // /// \brief Patch to the crba algorithm for joint mimic (in local convention) + template< + typename JointModel, + typename Scalar, + int Options, + template + class JointCollectionTpl> static inline void mimic_patch_CrbaLocalConventionBackwardStep( - const JointModelBase &, const Model &, Data &) + const JointModelBase &, + const ModelTpl &, + DataTpl &) { } @@ -209,8 +222,8 @@ namespace pinocchio template class JointCollectionTpl> static inline void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase> & jmodel, - const Model & model, - Data & data) + const ModelTpl & model, + DataTpl & data) { typedef JointModelMimicTpl JointModel; typedef typename Eigen::Matrix< @@ -226,7 +239,7 @@ namespace pinocchio assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); - JointIndex ancestor_prim, ancestor_sec; + size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); SpatialForcesBlock jF = jmodel.jointVelCols(data.Fcrb[secondary_id]); // Mimic joint forces @@ -234,7 +247,7 @@ namespace pinocchio SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor - for (int k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) + for (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) { const JointIndex i = model.supports[secondary_id].at(k); const JointIndex ui = @@ -253,7 +266,7 @@ namespace pinocchio .noalias() += GetSNoMalloc::run(data.joints[i]).transpose() * iF; } // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint - for (int k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) + for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { const JointIndex i = model.supports[primary_id].at(k); iMj = data.liMi[i].actInv(iMj); @@ -324,6 +337,13 @@ namespace pinocchio mimic_patch_CrbaLocalConventionBackwardStep(jmodel, model, data); } + + /// \brief Patch to the crba algorithm for joint mimic (in local convention) + template + static inline void mimic_patch_CrbaLocalConventionBackwardStep( + const JointModelBase &, const Model &, Data &) + { + } }; template< diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 526d9f62a5..653dda07af 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -193,7 +193,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); - Jout_.fill(0); + Jout_.setZero(); typedef typename ModelTpl::JointIndex JointIndex; typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; @@ -373,7 +373,7 @@ namespace pinocchio data.iMf[jointId].setIdentity(); Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); - J_.fill(0); + J_.setZero(); typedef JointJacobianForwardStep< Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 80f2eb28dd..644f596306 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -202,10 +202,14 @@ namespace pinocchio const Index & parent = model.parents[(Index)i]; lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); - Scalar nv_; - - if (boost::get(&model.joints[(Index)lastChild[(Index)i]])) - nv_ = boost::get(model.joints[(Index)lastChild[(Index)i]]).jmodel().nv(); + int nv_; + + if (boost::get>( + &model.joints[(Index)lastChild[(Index)i]])) + nv_ = boost::get>( + model.joints[(Index)lastChild[(Index)i]]) + .jmodel() + .nv(); else nv_ = nv(model.joints[(Index)lastChild[(Index)i]]); @@ -222,7 +226,7 @@ namespace pinocchio for (Index joint = 1; joint < (Index)(model.njoints); joint++) { - if (boost::get(&model.joints[joint])) + if (boost::get>(&model.joints[joint])) continue; // Mimic joints should not override mimicked joint fromRow values const Index & parent = model.parents[joint]; const int nvj = model.joints[joint].nv(); diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index d10215d171..ac6922b877 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -7,6 +7,7 @@ #include "pinocchio/macros.hpp" #include "pinocchio/math/matrix.hpp" +#include "pinocchio/math/fwd.hpp" #include #include @@ -95,10 +96,13 @@ namespace pinocchio const Scalar & offset, const Eigen::MatrixBase & qOut) { - assert( - fabs(scaling - 1.0) < Eigen::NumTraits::dummy_precision() - && fabs(offset) < Eigen::NumTraits::dummy_precision() - && "No ConfigVectorAffineTransform specialized for this joint type"); + if ( + math::fabs(static_cast(scaling - Scalar(1))) + < Eigen::NumTraits::dummy_precision() + && math::fabs(offset) < Eigen::NumTraits::dummy_precision()) + throw std::invalid_argument( + "No ConfigVectorAffineTransform specialized for this joint type"); + PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = qIn; } }; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 98039cc604..53aeaa25c8 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -806,7 +806,8 @@ namespace pinocchio typedef typename CastType::type ReturnType; ReturnType res( - m_jmodel_ref.template cast(), (NewScalar)m_scaling, (NewScalar)m_offset); + m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), + ScalarCast::cast(m_offset)); res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/unittest/cppad/joints.cpp b/unittest/cppad/joints.cpp index 023067f249..3ab7e3ec33 100644 --- a/unittest/cppad/joints.cpp +++ b/unittest/cppad/joints.cpp @@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) typedef JointCollection::JointDataRX JointDataRX; JointModelRX jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); JointDataRX jdata(jmodel.createData()); JointModelRXAD jmodel_ad = jmodel.cast(); @@ -108,7 +108,7 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -118,7 +118,7 @@ struct TestADOnJoints { typedef pinocchio::JointModelHelicalTpl JointModel; JointModel jmodel(Scalar(0.4)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -129,7 +129,7 @@ struct TestADOnJoints typedef pinocchio::JointModelUniversalTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::UnitX(), Vector3::UnitY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -140,7 +140,7 @@ struct TestADOnJoints typedef pinocchio::JointModelHelicalUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -151,7 +151,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -162,7 +162,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -173,7 +173,7 @@ struct TestADOnJoints typedef pinocchio::JointModelPrismaticUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -184,14 +184,15 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteTpl JointModelRX; typedef pinocchio::JointModelTpl JointModel; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } // TODO: implement it - template - void operator()(const pinocchio::JointModelMimic & /*jmodel*/) const + template class JointCollection> + void operator()( + const pinocchio::JointModelMimicTpl & /*jmodel*/) const { /* do nothing */ } @@ -204,7 +205,7 @@ struct TestADOnJoints typedef pinocchio::JointModelCompositeTpl JointModel; JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); }