Skip to content
Snippets Groups Projects
Commit 89621835 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

With JointModel::jointMotion and JointModel::jointForce.

parent dc6f742b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/joint.hpp
+ 38
− 32
View file @ 89621835
......@@ -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>());
......
src/multibody/model.hpp
+ 11
− 14
View file @ 89621835
......@@ -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]));
}
......
unittest/rnea.cpp
+ 9
− 9
View file @ 89621835
......@@ -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");
......
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