Commit 34363b16 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

[Major] JointDense with dynamic size.

parent 805abfe4
......@@ -263,7 +263,8 @@ namespace se3
= data.mass[i]*oSk.template topLeftCorner<3,1>()
- data.com[i].cross(oSk.template bottomLeftCorner<3,1>()) ;
else
data.Jcom.template block<3,JointModel::NV>(0,jmodel.idx_v())
jmodel.jointCols(data.Jcom)
//data.Jcom.template block<3,JointModel::NV>(0,jmodel.idx_v())
= data.mass[i]*oSk.template topRows<3>()
- skew(data.com[i]) * oSk.template bottomRows<3>() ;
......
......@@ -86,7 +86,8 @@ namespace se3
const Model::Index & i = (Model::Index) jmodel.id();
/* F[1:6,i] = Y*S */
data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
//data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
/* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
......
......@@ -62,7 +62,7 @@ namespace se3
if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
else data.oMi[i] = data.liMi[i];
data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
}
};
......@@ -128,7 +128,7 @@ namespace se3
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.iMf[parent] = data.liMi[i]*data.iMf[i];
data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.iMf[i].inverse().act(jdata.S());
jmodel.jointCols(data.J) = data.iMf[i].inverse().act(jdata.S());
}
};
......
......@@ -81,8 +81,15 @@ namespace se3
}; // traits ConstraintTpl
namespace internal
{
template<int Dim, typename Scalar, int Options>
struct ActionReturn<ConstraintTpl<Dim,Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,Dim> Type; };
}
template<int _Dim, typename _Scalar, int _Options>
class ConstraintTpl : ConstraintBase<ConstraintTpl < _Dim, _Scalar, _Options > >
class ConstraintTpl : public ConstraintBase<ConstraintTpl < _Dim, _Scalar, _Options > >
{
public:
......@@ -101,13 +108,16 @@ namespace se3
ConstraintTpl() : S()
{
EIGEN_STATIC_ASSERT_FIXED_SIZE(DenseBase);
#ifndef NDEBUG
S.fill( NAN );
#endif
}
ConstraintTpl(const int dim) : S(6,dim)
{
#ifndef NDEBUG
S.fill( NAN );
#endif
}
Motion __mult__(const JointMotion& vj) const
......@@ -123,6 +133,14 @@ namespace se3
JointForce operator* (const Force& f) const
{ return ref.S.transpose()*f.toVector(); }
template<typename D>
typename Eigen::Matrix<_Scalar,NV,Eigen::Dynamic>
operator*( const Eigen::MatrixBase<D> & F )
{
return ref.S.transpose()*F;
}
};
Transpose transpose() const { return Transpose(*this); }
......@@ -131,10 +149,24 @@ namespace se3
int nv_impl() const { return NV; }
//template<int Dim,typename Scalar,int Options>
friend Eigen::Matrix<_Scalar,6,_Dim>
operator*( const InertiaTpl<_Scalar,_Options> & Y,const ConstraintTpl<_Dim,_Scalar,_Options> & S)
{ return Y.matrix()*S.S; }
Eigen::Matrix<_Scalar,6,NV> se3Action(const SE3 & m) const
{
return m.toActionMatrix()*S;
}
private:
DenseBase S;
}; // class ConstraintTpl
typedef ConstraintTpl<1,double,0> Constraint1d;
typedef ConstraintTpl<3,double,0> Constraint3d;
typedef ConstraintTpl<6,double,0> Constraint6d;
......
......@@ -165,6 +165,39 @@ public:
};
template<int NV>
struct SizeDepType
{
template<class Mat>
struct SegmentReturn
{
typedef typename Mat::template FixedSegmentReturnType<NV>::Type Type;
typedef typename Mat::template ConstFixedSegmentReturnType<NV>::Type ConstType;
};
template<class Mat>
struct ColsReturn
{
typedef typename Mat::template NColsBlockXpr<NV>::Type Type;
typedef typename Mat::template ConstNColsBlockXpr<NV>::Type ConstType;
};
};
template<>
struct SizeDepType<Eigen::Dynamic>
{
template<class Mat>
struct SegmentReturn
{
typedef typename Mat::SegmentReturnType Type;
typedef typename Mat::ConstSegmentReturnType ConstType;
};
template<class Mat>
struct ColsReturn
{
typedef typename Mat::ColsBlockXpr Type;
typedef typename Mat::ConstColsBlockXpr ConstType;
};
};
template<typename _JointModel>
struct JointModelBase
{
......@@ -196,8 +229,12 @@ public:
public:
int nv() const { return NV; }
int nq() const { return NQ; }
int nv() const { return derived().nv_impl(); }
int nq() const { return derived().nq_impl(); }
// Both _impl methods are reimplemented by dynamic-size joints.
int nv_impl() const { return NV; }
int nq_impl() const { return NQ; }
const int & idx_q() const { return i_q; }
const int & idx_v() const { return i_v; }
const Index & id() const { return i_id; }
......@@ -208,68 +245,104 @@ public:
const Eigen::Matrix<double,NQ,1> & maxEffortLimit() const { return effortMax;}
const Eigen::Matrix<double,NV,1> & maxVelocityLimit() const { return velocityMax;}
void setIndexes(Index id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void setLowerPositionLimit(const Eigen::VectorXd & lowerPos)
{
if (lowerPos.rows() == NQ)
position_lower = lowerPos;
else
position_lower.fill(-std::numeric_limits<double>::infinity());
position_lower = lowerPos;
}
void setUpperPositionLimit(const Eigen::VectorXd & upperPos)
{
if (upperPos.rows() == NQ)
position_upper = upperPos;
else
position_upper.fill(std::numeric_limits<double>::infinity());
position_upper = upperPos;
}
void setMaxEffortLimit(const Eigen::VectorXd & effort)
{
if (effort.rows() == NV)
effortMax = effort;
else
effortMax.fill(std::numeric_limits<double>::infinity());
effortMax = effort;
}
void setMaxVelocityLimit(const Eigen::VectorXd & v)
{
if (v.rows() == NV)
velocityMax = v;
else
velocityMax.fill(std::numeric_limits<double>::infinity());
velocityMax = v;
}
/* Acces to dedicated segment in robot config velocity. */
// Const access
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointMotion(const Eigen::MatrixBase<D>& a) const { return derived().jointMotion_impl(a); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointMotion_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
// Non-const access
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointMotion( Eigen::MatrixBase<D>& a) const { return derived().jointMotion_impl(a); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointMotion_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
/* Acces to dedicated segment in robot joint torques. */
// Const access
template<typename D>
typename D::template ConstFixedSegmentReturnType<NV>::Type jointMotion(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointForce(const Eigen::MatrixBase<D>& a) const { return derived().jointForce_impl(a); }
template<typename D>
typename D::template FixedSegmentReturnType<NV>::Type jointMotion(Eigen::MatrixBase<D>& a) const
{ return a.template segment<NV>(i_v); }
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointForce_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
// Non-const access
template<typename D>
typename D::template ConstFixedSegmentReturnType<NV>::Type jointForce(const Eigen::MatrixBase<D>& tau) const
{ return tau.template segment<NV>(i_v); }
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointForce( Eigen::MatrixBase<D>& a) const { return derived().jointForce_impl(a); }
template<typename D>
typename D::template FixedSegmentReturnType<NV>::Type jointForce(Eigen::MatrixBase<D>& tau) const
{ return tau.template segment<NV>(i_v); }
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointForce_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
/* Acces to dedicated segment in robot config limit. */
// Const access
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointLimit (const Eigen::MatrixBase<D>& a) const { return derived().jointLimit_impl(a); }
template<typename D>
typename D::template ConstFixedSegmentReturnType<NQ>::Type jointLimit(const Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NQ>(i_q); }
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointLimit_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
// Non-const access
template<typename D>
typename D::template FixedSegmentReturnType<NQ>::Type jointLimit(Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NQ>(i_q); }
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointLimit( Eigen::MatrixBase<D>& a) const { return derived().jointLimit_impl(a); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointLimit_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
/* Acces to dedicated segment in robot config velocityLimit. */
// Const access
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointVelocityLimit (const Eigen::MatrixBase<D>& a) const { return derived().jointVelocityLimit_impl(a); }
template<typename D>
typename D::template ConstFixedSegmentReturnType<NV>::Type jointTangentLimit(const Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NV>(i_v); }
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointVelocityLimit_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
// Non-const access
template<typename D>
typename D::template FixedSegmentReturnType<NV>::Type jointTangentLimit(Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NV>(i_v); }
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointVelocityLimit( Eigen::MatrixBase<D>& a) const { return derived().jointVelocityLimit_impl(a); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointVelocityLimit_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::ConstType
jointCols(const Eigen::MatrixBase<D>& A) const { return derived().jointCols_impl(A); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::ConstType
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols(Eigen::MatrixBase<D>& A) const { return derived().jointCols_impl(A); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
JointModelDense<NQ, NV> toDense() const { return static_cast<const JointModel*>(this)->toDense_impl(); }
};
......@@ -316,6 +389,8 @@ public:
// M.translation(SE3::Vector3::Zero());
// }
JointDataDense() {};
JointDataDense( Constraint_t S,
Transformation_t M,
Motion_t v,
......@@ -350,10 +425,14 @@ public:
using JointModelBase<JointModelDense<_NQ, _NV > >::setMaxEffortLimit;
using JointModelBase<JointModelDense<_NQ, _NV > >::setMaxVelocityLimit;
using JointModelBase<JointModelDense<_NQ, _NV > >::setIndexes;
using JointModelBase<JointModelDense<_NQ, _NV > >::i_v;
using JointModelBase<JointModelDense<_NQ, _NV > >::i_q;
int nv_dyn,nq_dyn;
JointData createData() const
{
assert(false && "JointModelDense is read-only, should not createData");
//assert(false && "JointModelDense is read-only, should not createData");
return JointData();
}
void calc( JointData& ,
......@@ -393,6 +472,41 @@ public:
setMaxVelocityLimit(maxVel);
}
int nv_impl() const { return nv_dyn; }
int nq_impl() const { return nq_dyn; }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointMotion_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointMotion_impl( Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointForce_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointForce_impl( Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointLimit_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointLimit_impl( Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointVelocityLimit_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointVelocityLimit_impl( Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::ConstType
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
JointModelDense<_NQ, _NV> toDense_impl() const
{
assert(false && "Trying to convert a jointModelDense to JointModelDense : useless"); // disapear with release optimizations
......@@ -401,6 +515,58 @@ public:
}; // struct JointModelDense
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointMotion_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointMotion_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointForce_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointForce_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointLimit_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointLimit_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointVelocityLimit_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointVelocityLimit_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_dyn); }
template<>
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_dyn); }
} // namespace se3
#endif // ifndef __se3_joint_base_hpp__
......@@ -29,8 +29,8 @@
namespace se3
{
typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation> JointModelVariant;
typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation> JointDataVariant;
typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation,JointModelDense<-1,-1> > JointModelVariant;
typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation,JointDataDense<-1,-1> > JointDataVariant;
typedef std::vector<JointModelVariant> JointModelVector;
typedef std::vector<JointDataVariant> JointDataVector;
......
......@@ -79,7 +79,7 @@ namespace se3
data.oMi[i] = data.liMi[i];
}
data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.a[i] += data.liMi[i].actInv(data.a[parent]);
......@@ -113,7 +113,7 @@ namespace se3
const Model::Index & parent = model.parents[i];
/* F[1:6,i] = Y*S */
data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
/* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
......
......@@ -184,8 +184,8 @@ namespace se3
{
Matrix6 M;
const Matrix3 & c_cross = (skew(c));
M.template block<3,3>(LINEAR, LINEAR ).template setZero ();
M.template block<3,3>(LINEAR, LINEAR ).template diagonal ().template fill (m);
M.template block<3,3>(LINEAR, LINEAR ).setZero ();
M.template block<3,3>(LINEAR, LINEAR ).diagonal ().fill (m);
M.template block<3,3>(ANGULAR,LINEAR ) = m * c_cross;
M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3> (ANGULAR, LINEAR);
M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)(I - M.template block<3,3>(ANGULAR, LINEAR) * c_cross);
......
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