Verified Commit 4f25bc98 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

spatial/motion: add const MotionRef prototype

parent 0fd4bbad
......@@ -139,6 +139,7 @@ namespace se3
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::Matrix6x::ConstColXpr M6xColXpr;
Matrix6Like & J_ = EIGEN_CONST_CAST(Matrix6Like,J);
......@@ -147,7 +148,11 @@ namespace se3
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{
if(rf == WORLD) J_.col(j) = data.J.col(j);
else J_.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector(); // TODO: use MotionRef
else
{
const MotionRef<M6xColXpr> mref(data.J.col(j).derived());
J_.col(j) = oMjoint.actInv(mref).toVector();
}
}
}
......
......@@ -118,13 +118,14 @@ namespace se3
}; // struct ConstraintIdentityTpl
template<typename Scalar, int Options, typename Vector6Like>
MotionRef<Vector6Like>
operator*(const ConstraintIdentityTpl<Scalar,Options> &, const Eigen::MatrixBase<Vector6Like>& v)
MotionRef<const Vector6Like>
operator*(const ConstraintIdentityTpl<Scalar,Options> &,
const Eigen::MatrixBase<Vector6Like> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
// typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion;
typedef MotionRef<Vector6Like> Motion;
return Motion(v);
typedef MotionRef<const Vector6Like> Motion;
return Motion(v.derived());
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
......
......@@ -486,7 +486,7 @@ namespace se3
QuaternionMap_t res_quat (out.template tail<4>().data());
SE3 M0 (quat.matrix(), q.derived().template head<3>());
MotionRef<Velocity_t> mref_v(v);
MotionRef<const Velocity_t> mref_v(v.derived());
SE3 M1 (M0 * exp6(mref_v));
out.template head<3>() = M1.translation();
......@@ -539,7 +539,7 @@ namespace se3
const Eigen::MatrixBase<JacobianOut_t>& J)
{
JacobianOut_t & Jout = EIGEN_CONST_CAST(JacobianOut_t,J);
Jout = exp6(MotionRef<Tangent_t>(v)).toDualActionMatrix().transpose();
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......@@ -547,7 +547,7 @@ namespace se3
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J)
{
Jexp6(MotionRef<Tangent_t>(v), J.derived());
Jexp6(MotionRef<const Tangent_t>(v.derived()), J.derived());
}
// interpolate_impl use default implementation.
......
......@@ -304,7 +304,7 @@ namespace se3
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
typedef MotionRef<Mat> MotionRefOnMat;
typedef MotionRef<const Mat> MotionRefOnMat;
typedef MotionRef<MatRet> MotionRefOnMatRet;
MotionRefOnMat min(iV.derived());
......@@ -380,7 +380,7 @@ namespace se3
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
typedef MotionRef<Mat> MotionRefOnMat;
typedef MotionRef<const Mat> MotionRefOnMat;
typedef MotionRef<MatRet> MotionRefOnMatRet;
MotionRefOnMat min(iV.derived());
......@@ -505,7 +505,7 @@ namespace se3
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
typedef MotionRef<Mat> MotionRefOnMat;
typedef MotionRef<const Mat> MotionRefOnMat;
typedef ForceRef<MatRet> ForceRefOnMatRet;
MotionRefOnMat min(iV.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jV).derived());
......@@ -557,7 +557,7 @@ namespace se3
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
typedef MotionRef<Mat> MotionRefOnMat;
typedef MotionRef<const Mat> MotionRefOnMat;
typedef ForceRef<MatRet> ForceRefOnMatRet;
MotionRefOnMat vin(iV.derived());
......
......@@ -324,7 +324,7 @@ namespace se3
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);
MotionRef<Vector6Like> nu(v);
MotionRef<const Vector6Like> nu(v.derived());
return exp6(nu);
}
......
//
// Copyright (c) 2017-2018 CNRS
// Copyright (c) 2017-2018 CNRS INRIA
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -82,8 +81,8 @@ namespace se3
using Base::__mequ__;
using Base::__mult__;
MotionRef(const Eigen::MatrixBase<Vector6ArgType> & v_like)
: m_ref(const_cast<Vector6ArgType &>(v_like.derived()))
MotionRef(typename EIGEN_REF_TYPE(Vector6ArgType) v_like)
: m_ref(v_like)
{
EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
......@@ -155,7 +154,68 @@ namespace se3
protected:
DataRefType m_ref;
}; // class MotionTpl
}; // class MotionRef<Vector6Like>
template<typename Vector6ArgType>
class MotionRef<const Vector6ArgType>
: public MotionDense< MotionRef<const Vector6ArgType> >
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef MotionDense<MotionRef> Base;
typedef typename traits<MotionRef>::DataRefType DataRefType;
MOTION_TYPEDEF_TPL(MotionRef);
using Base::operator=;
using Base::linear;
using Base::angular;
using Base::__plus__;
using Base::__opposite__;
using Base::__minus__;
using Base::__mult__;
MotionRef(typename EIGEN_REF_CONSTTYPE(Vector6ArgType) v_like)
: m_ref(v_like)
{
EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
assert(v_like.size() == 6);
}
ToVectorConstReturnType toVector_impl() const { return m_ref; }
// Getters
ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
// Specific operators for MotionTpl and MotionRef
template<typename S1, int O1>
MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
{ return MotionPlain(m_ref+v.toVector()); }
template<typename Vector6Like>
MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
{ return MotionPlain(m_ref+v.toVector()); }
template<typename S1, int O1>
MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
{ return MotionPlain(m_ref-v.toVector()); }
template<typename Vector6Like>
MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
{ return MotionPlain(m_ref-v.toVector()); }
template<typename OtherScalar>
MotionPlain __mult__(const OtherScalar & alpha) const
{ return MotionPlain(alpha*m_ref); }
const MotionRef & ref() const { return *this; }
protected:
DataRefType m_ref;
}; // class MotionRef<const Vector6Like>
} // namespace se3
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment