Skip to content

GitLab

Unverified Commit 06f2c611 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #840 from jcarpent/devel 

use PINOCCHIO_EIGEN_CONST_CAST as much as possible
parents 6d6372b6 41420187
Hide whitespace changes
Inline Side-by-side
src/algorithm/aba-derivatives.hxx
View file @ 06f2c611
......@@ -323,7 +323,7 @@ namespace pinocchio
const Eigen::MatrixBase<Min> & J,
const Eigen::MatrixBase<Mout> & F)
{
Mout & F_ = const_cast<Mout &>(F.derived());
Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F);
motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose());
}
};
......
src/algorithm/cholesky.hxx
View file @ 06f2c611
......@@ -268,7 +268,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = const_cast<Eigen::MatrixBase<Mat> &>(m).derived();
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
internal::Utiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
return m_.derived();
}
......@@ -331,7 +331,7 @@ namespace pinocchio
const Eigen::MatrixBase<Mat> & min,
const Eigen::MatrixBase<MatRes> & mout)
{
MatRes & mout_ = const_cast<Eigen::MatrixBase<MatRes> &>(mout).derived();
MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout);
internal::Mv<Mat,MatRes,Mat::ColsAtCompileTime>::run(model,data,min.derived(),mout_);
return mout_.derived();
}
......
src/algorithm/rnea.hxx
View file @ 06f2c611
......@@ -472,7 +472,7 @@ namespace pinocchio
const Eigen::MatrixBase<Min> & J,
const Eigen::MatrixBase<Mout> & F)
{
Mout & F_ = const_cast<Mout &>(F.derived());
Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F);
motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose());
}
};
......
src/math/casadi.hpp
View file @ 06f2c611
......@@ -163,7 +163,7 @@ namespace pinocchio
{
typedef typename MatrixDerived::Scalar SX;
MatrixDerived & eig_mat_ = const_cast<MatrixDerived &>(eig_mat.derived());
MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived,eig_mat);
for (Eigen::Index i = 0; i < eig_mat.rows(); ++i)
for (Eigen::Index j = 0; j < eig_mat.cols(); ++j)
eig_mat_(i, j) = SX::sym(name + "_" + std::to_string(i) + "_" + std::to_string(j));
......@@ -191,7 +191,7 @@ namespace pinocchio
SX cs_mat_inv = SX::inv(cs_mat);
MatrixOut & dest_ = const_cast<MatrixOut &>(dest.derived());
MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
casadi::copy(cs_mat_inv,dest_);
}
......
src/math/matrix.hpp
View file @ 06f2c611
......@@ -102,7 +102,7 @@ namespace pinocchio
static void run(const Eigen::MatrixBase<MatrixIn> & m_in,
const Eigen::MatrixBase<MatrixOut> & dest)
{
MatrixOut & dest_ = const_cast<MatrixOut &>(dest.derived());
MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
dest_.noalias() = m_in.inverse();
}
};
......@@ -113,7 +113,7 @@ namespace pinocchio
inline void inverse(const Eigen::MatrixBase<MatrixIn> & m_in,
const Eigen::MatrixBase<MatrixOut> & dest)
{
MatrixOut & dest_ = const_cast<MatrixOut &>(dest.derived());
MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
}
......
src/math/quaternion.hpp
View file @ 06f2c611
......@@ -89,7 +89,7 @@ namespace pinocchio
assert(static_leq::op(math::fabs(N2-1.), epsilon));
#endif
const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
#ifndef NDEBUG
const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
assert(static_leq::op(math::fabs(q.norm() - Scalar(1)),
......
src/multibody/joint/joint-basic-visitors.hxx
View file @ 06f2c611
......@@ -121,7 +121,7 @@ namespace pinocchio
const bool & update_I
)
{
Matrix6Type & I_ = const_cast<Matrix6Type &>(I.derived());
Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I);
jmodel.calc_aba(jdata.derived(),I_,update_I);
}
......@@ -135,7 +135,7 @@ namespace pinocchio
{
typedef JointCalcAbaVisitor<Matrix6Type> Algo;
Matrix6Type & I_ = const_cast<Matrix6Type &>(I.derived());
Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I);
Algo::run(jmodel, jdata, typename Algo::ArgsType(I_, update_I) );
}
......
src/multibody/liegroup/cartesian-product-variant.hpp
View file @ 06f2c611
......@@ -98,7 +98,7 @@ namespace pinocchio
assert(v.size() == m_nv);
assert(qout.size() == m_nq);
ConfigOut_t & qout_ = const_cast< ConfigOut_t& >(qout.derived());
ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
Index id_q = 0, id_v = 0;
for(size_t k = 0; k < liegroups.size(); ++k)
{
......
src/multibody/liegroup/cartesian-product.hpp
View file @ 06f2c611
......@@ -200,15 +200,15 @@ namespace pinocchio
template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type V1 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1_.nv())); }
template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type V2 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2_.nv())); }
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type Qo1 (const Eigen::MatrixBase<Config >& q) const { return const_cast<Config &>(q.derived()).template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1_.nq())); }
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type Qo2 (const Eigen::MatrixBase<Config >& q) const { return const_cast<Config &>(q.derived()).template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2_.nq())); }
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type Vo1 (const Eigen::MatrixBase<Tangent>& v) const { return const_cast<Tangent&>(v.derived()).template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1_.nv())); }
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type Vo2 (const Eigen::MatrixBase<Tangent>& v) const { return const_cast<Tangent&>(v.derived()).template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2_.nv())); }
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1_.nv(),lg1_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1_.nv(),lg2_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2_.nv(),lg1_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2_.nv(),lg2_.nv()); }
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type Qo1 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1_.nq())); }
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type Qo2 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2_.nq())); }
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type Vo1 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1_.nv())); }
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type Vo2 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2_.nv())); }
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1_.nv(),lg1_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1_.nv(),lg2_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2_.nv(),lg1_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2_.nv(),lg2_.nv()); }
#undef REMOVE_IF_EIGEN_TOO_LOW
}; // struct CartesianProductOperation
......
src/multibody/liegroup/liegroup-base.hxx
View file @ 06f2c611
......@@ -282,12 +282,12 @@ namespace pinocchio {
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const
{
if (u == 0) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q0;
else if(u == 1) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q1;
if (u == 0) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q0;
else if(u == 1) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q1;
else
{
TangentVector_t vdiff(u * difference(q0, q1));
integrate(q0, vdiff, qout);
integrate(q0, vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout));
}
}
......
src/multibody/liegroup/liegroup-variant-visitors.hxx
View file @ 06f2c611
......@@ -134,7 +134,7 @@ namespace pinocchio
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
ConfigOut_t & qout_ = const_cast< ConfigOut_t& >(qout.derived());
ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
lg.integrate(Eigen::Ref<const typename LieGroupDerived::ConfigVector_t>(q),
Eigen::Ref<const typename LieGroupDerived::TangentVector_t>(v),
Eigen::Ref<typename LieGroupDerived::ConfigVector_t>(qout_));
......@@ -156,8 +156,8 @@ namespace pinocchio
assert(v.size() == nv(lg));
assert(qout.size() == nq(lg));
ConfigOut_t & qout_ = const_cast< ConfigOut_t& >(qout.derived());
Operation::run(lg,typename Operation::ArgsType(q.derived(),v.derived(),qout_.derived()));
ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
Operation::run(lg,typename Operation::ArgsType(q.derived(),v.derived(),qout_));
}
}
......
src/multibody/liegroup/special-euclidean.hpp
View file @ 06f2c611
......@@ -573,7 +573,7 @@ namespace pinocchio
template <class Config_t>
static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
{
Config_t& qout_ = (const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived();
Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
qout_.template tail<4>().normalize();
}
......
src/multibody/liegroup/special-orthogonal.hpp
View file @ 06f2c611
......@@ -227,9 +227,9 @@ namespace pinocchio
// const Eigen::MatrixBase<ConfigR_t> & q1)
template <class Config_t>
static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
static void normalize_impl (const Eigen::MatrixBase<Config_t> & qout)
{
Config_t& qout_ = (const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived();
Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).derived();
qout_.normalize();
}
......
src/spatial/act-on-set.hxx
View file @ 06f2c611
......@@ -40,7 +40,7 @@ namespace pinocchio
typedef ForceRef<MatRet> ForceRefOnMatRet;
ForceRefOnMat fin(iF.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jF).derived());
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF));
switch(Op)
{
......@@ -72,7 +72,7 @@ namespace pinocchio
for(int col=0;col<jF.cols();++col)
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col);
forceSet::se3Action<Op>(m,iF.col(col),jFc);
}
}
......@@ -124,7 +124,7 @@ namespace pinocchio
typedef ForceRef<MatRet> ForceRefOnMatRet;
ForceRefOnMat fin(iF.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jF).derived());
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF));
run(v,fin,fout);
}
......@@ -139,7 +139,7 @@ namespace pinocchio
for(int col=0;col<jF.cols();++col)
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col);
forceSet::motionAction<Op>(v,iF.col(col),jFc);
}
}
......@@ -173,7 +173,7 @@ namespace pinocchio
typedef ForceRef<MatRet> ForceRefOnMatRet;
ForceRefOnMat fin(iF.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jF).derived());
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF));
switch(Op)
{
......@@ -206,7 +206,7 @@ namespace pinocchio
for(int col=0;col<jF.cols();++col)
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col);
forceSet::se3ActionInverse<Op>(m,iF.col(col),jFc);
}
}
......@@ -295,7 +295,7 @@ namespace pinocchio
typedef MotionRef<MatRet> MotionRefOnMatRet;
MotionRefOnMat min(iV.derived());
MotionRefOnMatRet mout(const_cast<Eigen::MatrixBase<MatRet> &>(jV).derived());
MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV));
switch(Op)
{
......@@ -327,7 +327,7 @@ namespace pinocchio
for(int col=0;col<jV.cols();++col)
{
typename MatRet::ColXpr jVc
= const_cast<Eigen::MatrixBase<MatRet> &>(jV).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col);
motionSet::se3Action<Op>(m,iV.col(col),jVc);
}
}
......@@ -352,7 +352,7 @@ namespace pinocchio
for(int col=0;col<jV.cols();++col)
{
typename MatRet::ColXpr jVc
= const_cast<Eigen::MatrixBase<MatRet> &>(jV).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col);
motionSet::motionAction<Op>(v,iV.col(col),jVc);
}
}
......@@ -371,7 +371,7 @@ namespace pinocchio
typedef MotionRef<MatRet> MotionRefOnMatRet;
MotionRefOnMat min(iV.derived());
MotionRefOnMatRet mout(const_cast<Eigen::MatrixBase<MatRet> &>(jV).derived());
MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV));
switch(Op)
{
......@@ -420,7 +420,7 @@ namespace pinocchio
typedef MotionRef<MatRet> MotionRefOnMatRet;
MotionRefOnMat min(iV.derived());
MotionRefOnMatRet mout(const_cast<Eigen::MatrixBase<MatRet> &>(jV).derived());
MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV));
switch(Op)
{
......@@ -452,7 +452,7 @@ namespace pinocchio
for(int col=0;col<jV.cols();++col)
{
typename MatRet::ColXpr jVc
= const_cast<Eigen::MatrixBase<MatRet> &>(jV).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col);
motionSet::se3ActionInverse<Op>(m,iV.col(col),jVc);
}
}
......@@ -477,7 +477,7 @@ namespace pinocchio
for(int col=0;col<jV.cols();++col)
{
typename MatRet::ColXpr jVc
= const_cast<Eigen::MatrixBase<MatRet> &>(jV).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col);
motionSet::inertiaAction<Op>(I,iV.col(col),jVc);
}
}
......@@ -495,7 +495,7 @@ namespace pinocchio
typedef MotionRef<const Mat> MotionRefOnMat;
typedef ForceRef<MatRet> ForceRefOnMatRet;
MotionRefOnMat min(iV.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jV).derived());
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV));
switch(Op)
{
......@@ -525,7 +525,7 @@ namespace pinocchio
for(int col=0;col<jF.cols();++col)
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col);
motionSet::act<Op>(iV.col(col),f,jFc);
}
}
......@@ -548,7 +548,7 @@ namespace pinocchio
typedef ForceRef<MatRet> ForceRefOnMatRet;
MotionRefOnMat vin(iV.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jF).derived());
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF));
ForceSetMotionAction<Op,MotionRefOnMat,Mat,MatRet,1>::run(vin,f,fout);
}
......
src/spatial/cartesian-axis.hpp
View file @ 06f2c611
......@@ -78,7 +78,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
vout_[0] = 0.; vout_[1] = -vin[2]; vout_[2] = vin[1];
}
......@@ -89,7 +89,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
vout_[0] = vin[2]; vout_[1] = 0.; vout_[2] = -vin[0];
}
......@@ -100,7 +100,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
vout_[0] = -vin[1]; vout_[1] = vin[0]; vout_[2] = 0.;
}
......@@ -112,7 +112,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
vout_[0] = 0.; vout_[1] = -s*vin[2]; vout_[2] = s*vin[1];
}
......@@ -124,7 +124,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
vout_[0] = s*vin[2]; vout_[1] = 0.; vout_[2] = -s*vin[0];
}
......@@ -136,7 +136,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
V3_out & vout_ = const_cast<Eigen::MatrixBase<V3_out> &>(vout).derived();
V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
vout_[0] = -s*vin[1]; vout_[1] = s*vin[0]; vout_[2] = 0.;
}
......
src/spatial/explog.hpp
View file @ 06f2c611
......@@ -153,7 +153,7 @@ namespace pinocchio
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1);
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);
Matrix3Like & Jout = const_cast<Matrix3Like &>(Jexp.derived());
Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
typedef typename Matrix3Like::Scalar Scalar;
Scalar n2 = r.squaredNorm(),a,b,c;
......@@ -193,7 +193,7 @@ namespace pinocchio
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, log, 3, 1);
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jlog, 3, 3);
Matrix3Like & Jout = const_cast<Matrix3Like &>(Jlog.derived());
Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);
if (theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
{
......@@ -382,7 +382,7 @@ namespace pinocchio
typedef typename MotionDerived::Scalar Scalar;
typedef typename MotionDerived::Vector3 Vector3;
typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
Matrix6Like & Jout = const_cast<Matrix6Like &> (Jexp.derived());
Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);
const typename MotionDerived::ConstLinearType & v = nu.linear();
const typename MotionDerived::ConstAngularType & w = nu.angular();
......@@ -454,7 +454,7 @@ namespace pinocchio
typedef SE3Tpl<Scalar,Options> SE3;
typedef typename SE3::Vector3 Vector3;
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6);
Matrix6Like & value = const_cast<Matrix6Like &> (Jlog.derived());
Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);
typename SE3::ConstAngularRef R = M.rotation();
typename SE3::ConstLinearRef p = M.translation();
......
src/spatial/inertia.hpp
View file @ 06f2c611
......@@ -405,10 +405,12 @@ namespace pinocchio
}
template<typename M6>
static void vxi_impl(const Motion & v, const InertiaTpl & I, const Eigen::MatrixBase<M6> & Iout)
static void vxi_impl(const Motion & v,
const InertiaTpl & I,
const Eigen::MatrixBase<M6> & Iout)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
M6 & Iout_ = const_cast<Eigen::MatrixBase<M6> &>(Iout).derived();
M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
// Block 1,1
alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
......@@ -438,10 +440,12 @@ namespace pinocchio
}
template<typename M6>
static void ivx_impl(const Motion & v, const InertiaTpl & I, const Eigen::MatrixBase<M6> & Iout)
static void ivx_impl(const Motion & v,
const InertiaTpl & I,
const Eigen::MatrixBase<M6> & Iout)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
M6 & Iout_ = const_cast<Eigen::MatrixBase<M6> &>(Iout).derived();
M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout);
// Block 1,1
alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR));
......
src/spatial/skew.hpp
View file @ 06f2c611
......@@ -24,7 +24,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
typedef typename Matrix3::RealScalar Scalar;
M_(0,0) = Scalar(0); M_(0,1) = -v[2]; M_(0,2) = v[1];
......@@ -86,7 +86,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
assert((M + M.transpose()).isMuchSmallerThan(M));
Vector3 & v_ = const_cast<Eigen::MatrixBase<Vector3> &>(v).derived();
Vector3 & v_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,v);
typedef typename Vector3::RealScalar Scalar;
v_[0] = Scalar(0.5) * (M(2,1) - M(1,2));
......@@ -128,7 +128,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
typedef typename Matrix3::RealScalar RealScalar;
M_(0,0) = RealScalar(0); M_(0,1) = -v[2] * alpha; M_(0,2) = v[1] * alpha;
......@@ -171,7 +171,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V2,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
Matrix3 & C_ = const_cast<Eigen::MatrixBase<Matrix3> &>(C).derived();
Matrix3 & C_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,C);
typedef typename Matrix3::RealScalar Scalar;
C_.noalias() = v*u.transpose();
......@@ -216,7 +216,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT(Matrix3xIn::RowsAtCompileTime==3,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Matrix3xOut::RowsAtCompileTime==3,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
Matrix3xOut & Mout_ = const_cast<Eigen::MatrixBase<Matrix3xOut> &>(Mout).derived();
Matrix3xOut & Mout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut,Mout);
Mout_.row(0) = v[1]*Min.row(2) - v[2]*Min.row(1);
Mout_.row(1) = v[2]*Min.row(0) - v[0]*Min.row(2);
......
src/spatial/spatial-axis.hpp
View file @ 06f2c611
//
// Copyright (c) 2017-2018 CNRS
// Copyright (c) 2017-2019 CNRS INRIA
//
#ifndef __pinocchio_spatial_axis_hpp__
......@@ -104,7 +104,8 @@ namespace pinocchio
inline void SpatialAxis<axis>::cross(const MotionDense<Derived1> & min,
const MotionDense<Derived2> & mout)
{
Derived2 & mout_ = const_cast<MotionDense<Derived2> &>(mout).derived();
Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,mout);
if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
{
mout_.angular().setZero();
......@@ -122,7 +123,8 @@ namespace pinocchio
inline void SpatialAxis<axis>::cross(const ForceDense<Derived1> & fin,
const ForceDense<Derived2> & fout)
{
Derived2 & fout_ = const_cast<ForceDense<Derived2> &>(fout).derived();
Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,fout);
if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
{
fout_.linear().setZero();
......
src/spatial/symmetric3.hpp
View file @ 06f2c611
......@@ -237,7 +237,7 @@ namespace pinocchio
const typename Vector3::RealScalar & v1 = v[1];
const typename Vector3::RealScalar & v2 = v[2];
Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
M_(0,0) = d * v1 - b * v2;
M_(1,0) = a * v2 - d * v0;
M_(2,0) = b * v0 - a * v1;
......@@ -297,7 +297,7 @@ namespace pinocchio
const typename Vector3::RealScalar & v1 = v[1];
const typename Vector3::RealScalar & v2 = v[2];
Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
M_(0,0) = b * v2 - d * v1;
M_(1,0) = c * v2 - e * v1;
M_(2,0) = e * v2 - f * v1;
......@@ -345,7 +345,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in,Vector3);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out,Vector3);