Skip to content
GitLab
Commit 907fbb5a authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Moved joint::nv as a function.

parent fe8407cd
Hide whitespace changes
Inline Side-by-side
src/algorithm/crba.hpp
View file @ 907fbb5a
......@@ -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;
......
src/multibody/constraint.hpp
View file @ 907fbb5a
......@@ -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;
};
......
src/multibody/joint/joint-base.hpp
View file @ 907fbb5a
......@@ -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
......
src/multibody/joint/joint-free-flyer.hpp
View file @ 907fbb5a
......@@ -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>());
......
src/multibody/joint/joint-generic.hpp
View file @ 907fbb5a
......@@ -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; };
......
src/multibody/joint/joint-revolute.hpp
View file @ 907fbb5a
......@@ -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
};
};
......
src/multibody/joint/joint-variant.hpp
View file @ 907fbb5a
......@@ -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 ); }
......
src/multibody/model.hpp
View file @ 907fbb5a
......@@ -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
......
unittest/crba.cpp
View file @ 907fbb5a
......@@ -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();
......
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