Skip to content

Commit

Permalink
Make code compile with CppAd
Browse files Browse the repository at this point in the history
  • Loading branch information
Megane Millan committed Oct 3, 2024
1 parent ec18dd3 commit c7c198f
Show file tree
Hide file tree
Showing 7 changed files with 71 additions and 41 deletions.
2 changes: 1 addition & 1 deletion include/pinocchio/algorithm/check.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,7 @@ namespace pinocchio

for (JointIndex j = 1; int(j) < model.njoints; ++j)
{
if (boost::get<JointModelMimic>(&model.joints[j]))
if (boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(&model.joints[j]))
continue;
JointIndex c = (JointIndex)data.lastChild[j];
CHECK_DATA((int)c < model.njoints);
Expand Down
52 changes: 36 additions & 16 deletions include/pinocchio/algorithm/crba.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,9 +60,16 @@ namespace pinocchio
};

/// \brief Patch to the crba algorithm for joint mimic (in local convention)
template<typename JointModel>
template<
typename JointModel,
typename Scalar,
int Options,
template<typename, int>
class JointCollectionTpl>
static inline void mimic_patch_CrbaWorldConventionBackwardStep(
const JointModelBase<JointModel> &, const Model &, Data &)
const JointModelBase<JointModel> &,
const ModelTpl<Scalar, Options, JointCollectionTpl> &,
DataTpl<Scalar, Options, JointCollectionTpl> &)
{
}

Expand All @@ -73,20 +80,19 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
static inline void mimic_patch_CrbaWorldConventionBackwardStep(
const JointModelBase<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>> & jmodel,
const Model & model,
Data & data)
const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
DataTpl<Scalar, Options, JointCollectionTpl> & data)
{
typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> 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);

Expand All @@ -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)
Expand Down Expand Up @@ -179,10 +185,17 @@ namespace pinocchio
}
};

/// \brief Patch to the crba algorithm for joint mimic (in local convention)
template<typename JointModel>
// /// \brief Patch to the crba algorithm for joint mimic (in local convention)
template<
typename JointModel,
typename Scalar,
int Options,
template<typename, int>
class JointCollectionTpl>
static inline void mimic_patch_CrbaLocalConventionBackwardStep(
const JointModelBase<JointModel> &, const Model &, Data &)
const JointModelBase<JointModel> &,
const ModelTpl<Scalar, Options, JointCollectionTpl> &,
DataTpl<Scalar, Options, JointCollectionTpl> &)
{
}

Expand All @@ -209,8 +222,8 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
static inline void mimic_patch_CrbaLocalConventionBackwardStep(
const JointModelBase<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>> & jmodel,
const Model & model,
Data & data)
const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
DataTpl<Scalar, Options, JointCollectionTpl> & data)
{
typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModel;
typedef typename Eigen::Matrix<
Expand All @@ -226,15 +239,15 @@ 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
SpatialForcesX iF(6, jmodel.nj()); // Current joint forces
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 =
Expand All @@ -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);
Expand Down Expand Up @@ -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<typename JointModel>
static inline void mimic_patch_CrbaLocalConventionBackwardStep(
const JointModelBase<JointModel> &, const Model &, Data &)
{
}
};

template<
Expand Down
4 changes: 2 additions & 2 deletions include/pinocchio/algorithm/jacobian.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
Expand Down Expand Up @@ -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>
Expand Down
14 changes: 9 additions & 5 deletions include/pinocchio/multibody/data.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,10 +202,14 @@ namespace pinocchio
const Index & parent = model.parents[(Index)i];

lastChild[parent] = std::max<int>(lastChild[(Index)i], lastChild[parent]);
Scalar nv_;

if (boost::get<JointModelMimic>(&model.joints[(Index)lastChild[(Index)i]]))
nv_ = boost::get<JointModelMimic>(model.joints[(Index)lastChild[(Index)i]]).jmodel().nv();
int nv_;

if (boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(
&model.joints[(Index)lastChild[(Index)i]]))
nv_ = boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(
model.joints[(Index)lastChild[(Index)i]])
.jmodel()
.nv();
else
nv_ = nv(model.joints[(Index)lastChild[(Index)i]]);

Expand All @@ -222,7 +226,7 @@ namespace pinocchio

for (Index joint = 1; joint < (Index)(model.njoints); joint++)
{
if (boost::get<JointModelMimic>(&model.joints[joint]))
if (boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(&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();
Expand Down
12 changes: 8 additions & 4 deletions include/pinocchio/multibody/joint/joint-common-operations.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

#include "pinocchio/macros.hpp"
#include "pinocchio/math/matrix.hpp"
#include "pinocchio/math/fwd.hpp"

#include <boost/type_traits.hpp>
#include <boost/variant.hpp>
Expand Down Expand Up @@ -95,10 +96,13 @@ namespace pinocchio
const Scalar & offset,
const Eigen::MatrixBase<ConfigVectorOut> & qOut)
{
assert(
fabs(scaling - 1.0) < Eigen::NumTraits<Scalar>::dummy_precision()
&& fabs(offset) < Eigen::NumTraits<Scalar>::dummy_precision()
&& "No ConfigVectorAffineTransform specialized for this joint type");
if (
math::fabs(static_cast<Scalar>(scaling - Scalar(1)))
< Eigen::NumTraits<Scalar>::dummy_precision()
&& math::fabs(offset) < Eigen::NumTraits<Scalar>::dummy_precision())
throw std::invalid_argument(
"No ConfigVectorAffineTransform specialized for this joint type");

PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = qIn;
}
};
Expand Down
3 changes: 2 additions & 1 deletion include/pinocchio/multibody/joint/joint-mimic.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -806,7 +806,8 @@ namespace pinocchio
typedef typename CastType<NewScalar, JointModelMimicTpl>::type ReturnType;

ReturnType res(
m_jmodel_ref.template cast<NewScalar>(), (NewScalar)m_scaling, (NewScalar)m_offset);
m_jmodel_ref.template cast<NewScalar>(), ScalarCast<NewScalar, Scalar>::cast(m_scaling),
ScalarCast<NewScalar, Scalar>::cast(m_offset));
res.setIndexes(id(), idx_q(), idx_v(), idx_j());
return res;
}
Expand Down
25 changes: 13 additions & 12 deletions unittest/cppad/joints.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<AD_double>();
Expand Down Expand Up @@ -108,7 +108,7 @@ struct TestADOnJoints
void operator()(const pinocchio::JointModelBase<JointModel> &) const
{
JointModel jmodel;
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}
Expand All @@ -118,7 +118,7 @@ struct TestADOnJoints
{
typedef pinocchio::JointModelHelicalTpl<Scalar, Options, axis> JointModel;
JointModel jmodel(Scalar(0.4));
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}
Expand All @@ -129,7 +129,7 @@ struct TestADOnJoints
typedef pinocchio::JointModelUniversalTpl<Scalar, Options> 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);
}
Expand All @@ -140,7 +140,7 @@ struct TestADOnJoints
typedef pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> JointModel;
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}
Expand All @@ -151,7 +151,7 @@ struct TestADOnJoints
typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> JointModel;
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}
Expand All @@ -162,7 +162,7 @@ struct TestADOnJoints
typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModel;
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}
Expand All @@ -173,7 +173,7 @@ struct TestADOnJoints
typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> JointModel;
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}
Expand All @@ -184,14 +184,15 @@ struct TestADOnJoints
typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
typedef pinocchio::JointModelTpl<Scalar, Options, JointCollection> JointModel;
JointModel jmodel((JointModelRX()));
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}

// TODO: implement it
template<typename JointModel_>
void operator()(const pinocchio::JointModelMimic<JointModel_> & /*jmodel*/) const
template<typename Scalar, int Options, template<typename, int> class JointCollection>
void operator()(
const pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection> & /*jmodel*/) const
{
/* do nothing */
}
Expand All @@ -204,7 +205,7 @@ struct TestADOnJoints
typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> JointModel;
JointModel jmodel((JointModelRX()));
jmodel.addJoint(JointModelRY());
jmodel.setIndexes(0, 0, 0);
jmodel.setIndexes(0, 0, 0, 0);

test(jmodel);
}
Expand Down

0 comments on commit c7c198f

Please sign in to comment.