diff --git a/src/multibody/joint.hpp b/src/multibody/joint.hpp
index 445b96410220ef778495407981ec60ee872db710..43b8f02d5799daf347ec294a99b2a3c16aebdf3a 100644
--- a/src/multibody/joint.hpp
+++ b/src/multibody/joint.hpp
@@ -30,9 +30,8 @@ namespace se3
typedef typename traits<Joint>::JointModel JointModel; \
typedef typename traits<Joint>::Constraint_t Constraint_t; \
typedef typename traits<Joint>::Transformation_t Transformation_t; \
- typedef typename traits<Joint>::Velocity_t Velocity_t; \
+ typedef typename traits<Joint>::Motion_t Motion_t; \
typedef typename traits<Joint>::Bias_t Bias_t; \
- typedef typename traits<Joint>::JointMotion_t JointMotion_t; \
enum { \
nq = traits<Joint>::nq, \
nv = traits<Joint>::nv \
@@ -49,9 +48,8 @@ namespace se3
const Constraint_t & S() { return static_cast<JointData*>(this)->S; }
const Transformation_t & M() { return static_cast<JointData*>(this)->M; }
- const Velocity_t & v() { return static_cast<JointData*>(this)->v; }
+ const Motion_t & v() { return static_cast<JointData*>(this)->v; }
const Bias_t & c() { return static_cast<JointData*>(this)->c; }
- const JointMotion_t & qdd() { return static_cast<JointData*>(this)->qdd; }
};
@@ -68,9 +66,8 @@ namespace se3
JointData createData() const { return static_cast<const JointModel*>(this)->createData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs,
- const Eigen::VectorXd & vs,
- const Eigen::VectorXd & as ) const
- { return static_cast<const JointModel*>(this)->calc(data,qs,vs,as); }
+ const Eigen::VectorXd & vs ) const
+ { return static_cast<const JointModel*>(this)->calc(data,qs,vs); }
private:
int i_q,i_v;
@@ -78,12 +75,30 @@ namespace se3
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; }
- };
+ // typename Eigen::VectorXd::ConstFixedSegmentReturnType<nv>::Type jointMotion(const Eigen::VectorXd & a) const
+ // {
+ // return a.template segment<nv>(i_v);
+ // }
+ // typename Eigen::VectorXd::FixedSegmentReturnType<nv>::Type jointForce(Eigen::VectorXd & tau) const
+ // {
+ // return tau.template segment<nv>(i_v);
+ // }
+ template<typename D>
+ 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); }
+ template<typename D>
+ 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); }
+ };
- struct JointDataFreeFlyer;
- struct JointModelFreeFlyer;
@@ -110,6 +125,7 @@ namespace se3
return Motion(Motion::Vector3::Zero(),Motion::Vector3(wx,0,0));
}
}; // struct MotionRX
+
friend const MotionRX& operator+ (const MotionRX& m, const BiasZero&) { return m; }
friend Motion operator+( const MotionRX& m1, const Motion& m2)
{
@@ -130,7 +146,7 @@ namespace se3
}
struct ConstraintRX
- {
+ {
template<typename D>
MotionRX operator*( const Eigen::MatrixBase<D> & v ) const { return MotionRX(v[0]); }
@@ -148,14 +164,10 @@ namespace se3
{
typedef JointDataRX JointData;
typedef JointModelRX JointModel;
- //typedef ConstraintTpl<1,double,0> Constraint_t;
typedef typename JointRX::ConstraintRX Constraint_t;
typedef SE3 Transformation_t;
- //typedef Motion Velocity_t;
- typedef JointRX::MotionRX Velocity_t;
+ typedef JointRX::MotionRX Motion_t;
typedef JointRX::BiasZero Bias_t;
- //typedef Constraint_t::JointMotion JointMotion_t;
- typedef Eigen::Matrix<double,1,1> JointMotion_t;
enum {
nq = 1,
nv = 1
@@ -172,15 +184,12 @@ namespace se3
Constraint_t S;
Transformation_t M;
- Velocity_t v;
+ Motion_t v;
Bias_t c;
- JointMotion_t qdd;
- JointDataRX() : S(),M(1)
+ JointDataRX() : M(1)
{
- //S.matrix() << 0,0,0,1,0,0;
M.translation(SE3::Vector3::Zero());
- //v.linear(Motion::Vector3::Zero());
}
};
@@ -196,15 +205,12 @@ namespace se3
JointData createData() const { return JointData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs,
- const Eigen::VectorXd & vs,
- const Eigen::VectorXd & as ) const
+ const Eigen::VectorXd & vs ) const
{
const double & q = qs[idx_q()];
const double & v = vs[idx_v()];
- data.qdd[0] = as[idx_v()];
data.M.rotation(rotationX(q));
- //data.v.angular(Eigen::Vector3d(v,0,0));
data.v.wx = v;
}
@@ -227,6 +233,10 @@ namespace se3
/* --- REVOLUTE FF -------------------------------------------------------- */
/* --- REVOLUTE FF -------------------------------------------------------- */
+
+ struct JointDataFreeFlyer;
+ struct JointModelFreeFlyer;
+
struct JointFreeFlyer
{
struct BiasZero {};
@@ -258,9 +268,8 @@ namespace se3
typedef JointModelFreeFlyer JointModel;
typedef JointFreeFlyer::ConstraintIdentity Constraint_t;
typedef SE3 Transformation_t;
- typedef Motion Velocity_t;
+ typedef Motion Motion_t;
typedef JointFreeFlyer::BiasZero Bias_t;
- typedef Eigen::Matrix<double,6,1> JointMotion_t;
enum {
nq = 7,
nv = 6
@@ -280,9 +289,8 @@ namespace se3
Constraint_t S;
Transformation_t M;
- Velocity_t v;
+ Motion_t v;
Bias_t c;
- JointMotion_t qdd;
JointDataFreeFlyer() : M(1)
{
@@ -297,12 +305,10 @@ namespace se3
JointData createData() const { return JointData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs,
- const Eigen::VectorXd & vs,
- const Eigen::VectorXd & as ) const
+ const Eigen::VectorXd & vs ) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<nq>::Type q = qs.segment<nq>(idx_q());
data.v = vs.segment<nv>(idx_v());
- data.qdd = as.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/model.hpp b/src/multibody/model.hpp
index aa3b8feb10e6d53a63bf6390245f7c25b3bfa187..2f4fdec9b49b5cc91ae2c4ae1f00844c15a4ffc5 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -24,19 +24,18 @@ namespace se3
public:
typedef int Index;
- int nq;
- int nv;
- int nbody;
+ int nq; // Dimension of the configuration representation
+ int nv; // Dimension of the velocity vector space
+ int nbody; // Number of bodies (= number of joints + 1)
- std::vector<Inertia> inertias;
- std::vector<SE3> jointPlacements;
- JointModelVector joints;
- //std::vector<JointModelRX> joints;
- std::vector<Index> parents;
- std::vector<std::string> names;
+ std::vector<Inertia> inertias; // Spatial inertias of the body <i> in the supporting joint frame <i>
+ std::vector<SE3> jointPlacements; // Placement (SE3) of the input of joint <i> in parent joint output <li>
+ JointModelVector joints; // Model of joint <i>
+ std::vector<Index> parents; // Joint parent of joint <i>, denoted <li> (li==parents[i])
+ std::vector<std::string> names; // name of the body attached to the output of joint <i>
- Motion gravity;
- static const Eigen::Vector3d gravity981;
+ Motion gravity; // Spatial gravity
+ static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,9.81))
Model()
: nq(0)
@@ -65,7 +64,6 @@ namespace se3
const Model& model;
JointDataVector joints;
- //std::vector<JointDataRX> joints;
std::vector<Motion> a; // Body acceleration
std::vector<Motion> v; // Body velocity
std::vector<Force> f; // Body force
@@ -127,7 +125,7 @@ namespace se3
Model::Index Model::getBodyId( const std::string & name ) const
{
Index res = std::find(names.begin(),names.end(),name) - names.begin();
- assert( (res>=0)&&(res<nbody) );
+ assert( (res>=0)&&(res<nbody)&&"The body name you asked do not exist" );
return res;
}
@@ -148,7 +146,6 @@ namespace se3
,tau(ref.nv)
{
for(int i=0;i<model.nbody;++i)
- //joints.push_back(model.joints[i].createData());
joints.push_back(CreateJointData::run(model.joints[i]));
}
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 76d27b60382290d9d6f182ab3a848258130090d6..a86857dc36c917806ecb6ef936fb061c1e1cf307 100644
--- a/unittest/rnea.cpp
+++ b/unittest/rnea.cpp
@@ -20,7 +20,7 @@ void rneaForwardStep(const se3::Model& model,
using namespace Eigen;
using namespace se3;
- jmodel.calc(jdata.derived(),q,v,a);
+ jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i]*jdata.M();
@@ -31,7 +31,8 @@ void rneaForwardStep(const se3::Model& model,
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
- data.a[i] = jdata.S()*jdata.qdd() + jdata.c() + (data.v[i] ^ jdata.v()) ;
+ jmodel.jointMotion(a);
+ //data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
if(parent>0) data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
@@ -48,7 +49,7 @@ void rneaBackwardStep(const se3::Model& model,
using namespace se3;
const Model::Index & parent = model.parents[i];
- data.tau.segment(jmodel.idx_v(),jmodel.nv) = jdata.S().transpose()*data.f[i];
+ jmodel.jointForce(data.tau) = jdata.S().transpose()*data.f[i];
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
}
@@ -131,7 +132,6 @@ int main()
se3::Model model;
model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Random(),Inertia::Random(),"root");
-
model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg1");
model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg2");
model.addBody(model.getBodyId("rleg2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg3");
@@ -148,12 +148,12 @@ int main()
model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso1");
model.addBody(model.getBodyId("torso1"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso2");
- model.addBody(model.getBodyId("torso2"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso3");
- model.addBody(model.getBodyId("torso3"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck1");
+ //model.addBody(model.getBodyId("torso2"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso3");
+ model.addBody(model.getBodyId("torso2"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck1");
model.addBody(model.getBodyId("neck1"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck2");
- model.addBody(model.getBodyId("neck2"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck3");
+ //model.addBody(model.getBodyId("neck2"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck3");
- model.addBody(model.getBodyId("torso3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm1");
+ model.addBody(model.getBodyId("torso2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm1");
model.addBody(model.getBodyId("rarm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm2");
model.addBody(model.getBodyId("rarm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm3");
model.addBody(model.getBodyId("rarm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm4");
@@ -161,7 +161,7 @@ int main()
model.addBody(model.getBodyId("rarm5"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm6");
model.addBody(model.getBodyId("rarm6"),JointModelRX(),SE3::Random(),Inertia::Random(),"rgrip");
- model.addBody(model.getBodyId("torso3"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm1");
+ model.addBody(model.getBodyId("torso2"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm1");
model.addBody(model.getBodyId("larm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm2");
model.addBody(model.getBodyId("larm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm3");
model.addBody(model.getBodyId("larm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm4");