diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
index 13ecee8fd1b9bc9649b465b585eb4dc4f6902cff..0bd17c31ddbb9c01dd70c390757425860500fc24 100644
--- a/src/algorithm/crba.hpp
+++ b/src/algorithm/crba.hpp
@@ -93,32 +93,6 @@ namespace se3
} // namespace internal
- template<typename D>
- struct CrbaInternalBackwardStep : public fusion::JointVisitor<CrbaInternalBackwardStep<D> >
- {
- typedef boost::fusion::vector<const Model&,
- Data&,
- const Eigen::MatrixBase<D>&,
- const int &,
- const int &> ArgsType;
-
- CrbaInternalBackwardStep( JointDataVariant & jdata,ArgsType args ) : jdata(jdata),args(args) {}
- using fusion::JointVisitor< CrbaInternalBackwardStep<D> >::run;
- JointDataVariant & jdata;
- ArgsType args;
-
- template<typename JointModel>
- static void algo(const JointModelBase<JointModel> & jmodel,
- JointDataBase<typename JointModel::JointData> & jdata,
- const Model&,
- Data& data,
- const Eigen::MatrixBase<D> & F,
- const int & idx_v, const int & nv)
- {
- data.M.block(jmodel.idx_v(),idx_v,JointModel::nv,nv) = jdata.S().transpose()*F;
- }
- };
-
struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
{
typedef boost::fusion::vector<const Model&,
@@ -142,9 +116,9 @@ namespace se3
* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
*/
- 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();
- data.M.block(jmodel.idx_v(),jmodel.idx_v(),JointModel::nv,data.nvSubtree[i])
+ data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
// std::cout << "*** joint " << i << std::endl;
diff --git a/src/multibody/constraint.hpp b/src/multibody/constraint.hpp
index 18eddb33c0cf282f81667e9c8fa8dd7cf467ab74..abb13d1e1405695a12043b5352088ae83d7a6e8f 100644
--- a/src/multibody/constraint.hpp
+++ b/src/multibody/constraint.hpp
@@ -15,21 +15,24 @@ namespace se3
class ConstraintTpl
{
public:
- enum { nv = _Dim, Options = _Options };
+ enum { NV = _Dim, Options = _Options };
typedef _Scalar Scalar;
- typedef Eigen::Matrix<Scalar,nv,1,Options> JointMotion;
- typedef Eigen::Matrix<Scalar,nv,1,Options> JointForce;
+ typedef Eigen::Matrix<Scalar,NV,1,Options> JointMotion;
+ typedef Eigen::Matrix<Scalar,NV,1,Options> JointForce;
typedef MotionTpl<Scalar,Options> Motion;
typedef ForceTpl<Scalar,Options> Force;
- typedef Eigen::Matrix<Scalar,6,nv> DenseBase;
+ typedef Eigen::Matrix<Scalar,6,NV> DenseBase;
public:
template<typename D>
ConstraintTpl( const Eigen::MatrixBase<D> & _S ) : S(_S) {}
- //{ EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(S,_S); }
-
- ConstraintTpl() : S() { S.fill( NAN ); }
+ ConstraintTpl() : S()
+ {
+ EIGEN_STATIC_ASSERT_FIXED_SIZE(DenseBase);
+ S.fill( NAN );
+ }
+ ConstraintTpl(const int dim) : S(dim,6) { S.fill( NAN ); }
Motion operator* (const JointMotion& vj) const
{ return Motion(S*vj); }
@@ -47,6 +50,8 @@ namespace se3
DenseBase & matrix() { return S; }
const DenseBase & matrix() const { return S; }
+ int nv() const { return NV; }
+
private:
DenseBase S;
};
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index 6a9cfa9d1c3f01bcd9541c11a0e1805dcc0aaacd..ba9a1f1b67f24f7138251f0a9ec8bc2cf37d695e 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -49,8 +49,8 @@ namespace se3
typedef typename traits<Joint>::Bias_t Bias_t; \
typedef typename traits<Joint>::F_t F_t; \
enum { \
- nq = traits<Joint>::nq, \
- nv = traits<Joint>::nv \
+ NQ = traits<Joint>::NQ, \
+ NV = traits<Joint>::NV \
}
#define SE3_JOINT_USE_INDEXES \
@@ -93,23 +93,30 @@ namespace se3
{ return static_cast<const JointModel*>(this)->calc(data,qs,vs); }
private:
- int i_q,i_v;
+ int i_id; // ID of the joint in the multibody list.
+ int i_q; // Index of the joint configuration in the joint configuration vector.
+ int i_v; // Index of the joint velocity in the joint velocity vector.
+
public:
+ int nv() const { return NV; }
+ int nq() const { return NQ; }
const int & idx_q() const { return i_q; }
const int & idx_v() const { return i_v; }
- void setIndexes(int q,int v) { i_q = q; i_v = v; }
+ const int & id() const { return i_id; }
+
+ void setIndexes(int id,int q,int v) { i_id = id, i_q = q; i_v = v; }
template<typename D>
- typename D::template ConstFixedSegmentReturnType<nv>::Type jointMotion(const Eigen::MatrixBase<D>& a) const { return a.template segment<nv>(i_v); }
+ typename D::template ConstFixedSegmentReturnType<NV>::Type jointMotion(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
template<typename D>
- typename D::template FixedSegmentReturnType<nv>::Type jointMotion(Eigen::MatrixBase<D>& a) const
- { return a.template segment<nv>(i_v); }
+ typename D::template FixedSegmentReturnType<NV>::Type jointMotion(Eigen::MatrixBase<D>& a) const
+ { return a.template segment<NV>(i_v); }
template<typename D>
- typename D::template ConstFixedSegmentReturnType<nv>::Type jointForce(const Eigen::MatrixBase<D>& tau) const
- { return tau.template segment<nv>(i_v); }
+ typename D::template ConstFixedSegmentReturnType<NV>::Type jointForce(const Eigen::MatrixBase<D>& tau) const
+ { return tau.template segment<NV>(i_v); }
template<typename D>
- typename D::template FixedSegmentReturnType<nv>::Type jointForce(Eigen::MatrixBase<D>& tau) const
- { return tau.template segment<nv>(i_v); }
+ typename D::template FixedSegmentReturnType<NV>::Type jointForce(Eigen::MatrixBase<D>& tau) const
+ { return tau.template segment<NV>(i_v); }
};
} // namespace se3
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index defa115ba9698f3e81d11c395e7b57e68c39e022..977cdd93f1b877b59252c88a71462f8e617700e1 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -66,8 +66,8 @@ namespace se3
typedef JointFreeFlyer::BiasZero Bias_t;
typedef Eigen::Matrix<double,6,6> F_t;
enum {
- nq = 7,
- nv = 6
+ NQ = 7,
+ NV = 6
};
};
template<> struct traits<JointDataFreeFlyer> { typedef JointFreeFlyer Joint; };
@@ -102,7 +102,7 @@ namespace se3
void calc( JointData& data,
const Eigen::VectorXd & qs) const
{
- Eigen::VectorXd::ConstFixedSegmentReturnType<nq>::Type q = qs.segment<nq>(idx_q());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type q = qs.segment<NQ>(idx_q());
JointData::Quaternion quat(Eigen::Matrix<double,4,1>(q.tail(4))); // TODO
data.M = SE3(quat.matrix(),q.head<3>());
}
@@ -110,8 +110,8 @@ namespace se3
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
- Eigen::VectorXd::ConstFixedSegmentReturnType<nq>::Type q = qs.segment<nq>(idx_q());
- data.v = vs.segment<nv>(idx_v());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type q = qs.segment<NQ>(idx_q());
+ data.v = vs.segment<NV>(idx_v());
JointData::Quaternion quat(Eigen::Matrix<double,4,1>(q.tail(4))); // TODO
data.M = SE3(quat.matrix(),q.head<3>());
diff --git a/src/multibody/joint/joint-generic.hpp b/src/multibody/joint/joint-generic.hpp
index 533b87c56e15b5d0531c2b6e38af7ac7111cd681..1bc9a198bd808e03821eae32d586cbb9586cb3cf 100644
--- a/src/multibody/joint/joint-generic.hpp
+++ b/src/multibody/joint/joint-generic.hpp
@@ -22,8 +22,8 @@ namespace se3
typedef Motion Motion_t;
typedef Motion Bias_t;
enum {
- nq = -1,
- nv = -1
+ NQ = -1,
+ NV = -1
};
};
template<> struct traits<JointDataGeneric> { typedef JointGeneric Joint; };
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 804fc7afd6eaaa4d0c1762d8999411eae28a1bf8..2e898d6995bda29b034fc4849ffd06a5ae55b2a3 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -242,8 +242,8 @@ namespace se3
typedef typename JointRevolute<axis>::BiasZero Bias_t;
typedef Eigen::Matrix<double,6,1> F_t;
enum {
- nq = 1,
- nv = 1
+ NQ = 1,
+ NV = 1
};
};
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index 3e7ae192f8588ec213c1f141e12c07458be0743d..311719750e16ac55ec4c837cdbe337b3de395568 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -28,7 +28,7 @@ namespace se3
public:
template<typename D>
int operator()(const JointModelBase<D> & ) const
- { return D::nv; }
+ { return D::NV; }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_nv(), jmodel ); }
@@ -39,7 +39,7 @@ namespace se3
public:
template<typename D>
int operator()(const JointModelBase<D> & ) const
- { return D::nq; }
+ { return D::NQ; }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_nq(), jmodel ); }
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 8adb4ecbdb537ac33b95d6c763dcacb9a34f969b..db7493e64f2273a44f7181dac3be1bbea2bd6657 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -131,20 +131,20 @@ namespace se3
{
assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
&&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
- assert( (j.nq>=0)&&(j.nv>=0) );
+ assert( (j.nq()>=0)&&(j.nv()>=0) );
Index idx = nbody ++;
joints .push_back(j.derived());
- boost::get<D&>(joints.back()).setIndexes(nq,nv);
+ boost::get<D&>(joints.back()).setIndexes(idx,nq,nv);
inertias .push_back(Y);
parents .push_back(parent);
jointPlacements.push_back(placement);
names .push_back( (name!="")?name:random(8) );
- nq += j.nq;
- nv += j.nv;
+ nq += j.nq();
+ nv += j.nv();
return idx;
}
Model::Index Model::getBodyId( const std::string & name ) const
diff --git a/unittest/crba.cpp b/unittest/crba.cpp
index 69a818aa4e6d93aa2201d446f30e8f698e24a7e1..e8e6c7893273e4b8e96e939cf2a504665538dadf 100644
--- a/unittest/crba.cpp
+++ b/unittest/crba.cpp
@@ -36,31 +36,7 @@ int main(int argc, const char ** argv)
if(argc>1) filename = argv[1];
model = se3::buildModel(filename,argc>1);
-
- // SIMPLE no FF
- // model.addBody(model.getBodyId("universe"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg1");
- // model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg2");
- // model.addBody(model.getBodyId("rleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg3");
-
- // model.addBody(model.getBodyId("universe"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg1");
- // model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg2");
- // model.addBody(model.getBodyId("lleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg3");
-
-
- //SIMPLE with FF
- // model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"root");
-
- // model.addBody(model.getBodyId("root"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg1");
- // model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg2");
- // model.addBody(model.getBodyId("rleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"rleg3");
-
- // model.addBody(model.getBodyId("root"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg1");
- // model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg2");
- // model.addBody(model.getBodyId("lleg2"),JointModelRX(),SE3(I3,SE3::Vector3::Random()),Inertia::Random(),"lleg3");
-
-
se3::Data data(model);
-
VectorXd q = VectorXd::Zero(model.nq);
StackTicToc timer(StackTicToc::US); timer.tic();