Commit 7d57d540 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Use njoint instead of nbody

parent c83fa97a
......@@ -197,19 +197,19 @@ namespace se3
data.a[0] = -model.gravity;
data.u = tau;
for(Model::Index i=1;i<(Model::Index)model.nbody;++i)
for(Model::Index i=1;i<(Model::Index)model.njoint;++i)
{
AbaForwardStep1::run(model.joints[i],data.joints[i],
AbaForwardStep1::ArgsType(model,data,q,v));
}
for( Model::Index i=(Model::Index)model.nbody-1;i>0;--i )
for( Model::Index i=(Model::Index)model.njoint-1;i>0;--i )
{
AbaBackwardStep::run(model.joints[i],data.joints[i],
AbaBackwardStep::ArgsType(model,data));
}
for(Model::Index i=1;i<(Model::Index)model.nbody;++i)
for(Model::Index i=1;i<(Model::Index)model.njoint;++i)
{
AbaForwardStep2::run(model.joints[i],data.joints[i],
AbaForwardStep2::ArgsType(model,data));
......
......@@ -36,7 +36,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
......@@ -46,7 +46,7 @@ namespace se3
}
// Backward Step
for(Model::JointIndex i=(Model::JointIndex)(model.nbody-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
......@@ -84,7 +84,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q, v);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
......@@ -98,7 +98,7 @@ namespace se3
}
// Backward Step
for(Model::JointIndex i=(Model::JointIndex)(model.nbody-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
......@@ -142,7 +142,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q, v, a);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
......@@ -158,7 +158,7 @@ namespace se3
}
// Backward Step
for(Model::JointIndex i=(Model::JointIndex)(model.nbody-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
......@@ -253,7 +253,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
......@@ -263,7 +263,7 @@ namespace se3
}
// Backward step
for( Model::JointIndex i= (Model::JointIndex) (model.nbody-1);i>0;--i )
for( Model::JointIndex i= (Model::JointIndex) (model.njoint-1);i>0;--i )
{
JacobianCenterOfMassBackwardStep
::run(model.joints[i],data.joints[i],
......
......@@ -211,13 +211,13 @@ namespace se3
data.com[0].setZero ();
data.vcom[0].setZero ();
for(Model::JointIndex i=1;i<(Model::JointIndex) model.nbody;++i)
for(Model::JointIndex i=1;i<(Model::JointIndex) model.njoint;++i)
{
CATForwardStep::run(model.joints[i],data.joints[i],
CATForwardStep::ArgsType(model,data,q,v));
}
for(Model::JointIndex i=(Model::JointIndex)(model.nbody-1);i>0;--i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1);i>0;--i)
{
CATBackwardStep::run(model.joints[i],data.joints[i],
CATBackwardStep::ArgsType(model,data));
......
......@@ -102,13 +102,13 @@ namespace se3
crba(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
for( Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i )
for( Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
CrbaForwardStep::ArgsType(model,data,q));
}
for( Model::JointIndex i=(Model::JointIndex)(model.nbody-1);i>0;--i )
for( Model::JointIndex i=(Model::JointIndex)(model.njoint-1);i>0;--i )
{
CrbaBackwardStep::run(model.joints[i],data.joints[i],
CrbaBackwardStep::ArgsType(model,data));
......@@ -185,11 +185,11 @@ namespace se3
forwardKinematics(model, data, q);
data.Ycrb[0].setZero();
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i )
for(Model::Index i=1;i<(Model::Index)(model.njoint);++i )
data.Ycrb[i] = model.inertias[i];
for(Model::Index i=(Model::Index)(model.nbody-1);i>0;--i)
for(Model::Index i=(Model::Index)(model.njoint-1);i>0;--i)
{
CcrbaBackwardStep::run(model.joints[i],data.joints[i],
CcrbaBackwardStep::ArgsType(model,data));
......
......@@ -76,7 +76,7 @@ namespace se3
if (update_kinematics)
forwardKinematics(model,data,q,v);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
data.kinetic_energy *= .5;
......@@ -96,7 +96,7 @@ namespace se3
if (update_kinematics)
forwardKinematics(model,data,q);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
{
com_global = data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever();
data.potential_energy += model.inertias[i].mass() * com_global.dot(g);
......
......@@ -59,7 +59,7 @@ namespace se3
computeJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q)
{
for( Model::JointIndex i=1; i< (Model::JointIndex) model.nbody;++i )
for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoint;++i )
{
JacobiansForwardStep::run(model.joints[i],data.joints[i],
JacobiansForwardStep::ArgsType(model,data,q));
......
......@@ -71,7 +71,7 @@ namespace se3
* @param[in] model Model we want to compute the distance
* @param[in] q0 Configuration 0 (size model.nq)
* @param[in] q1 Configuration 1 (size model.nq)
* @return The corresponding distances for each joint (size model.nbody-1 = number of joints)
* @return The corresponding distances for each joint (size model.njoint-1 = number of joints)
*/
inline Eigen::VectorXd distance(const Model & model,
const Eigen::VectorXd & q0,
......@@ -144,7 +144,7 @@ namespace se3
const Eigen::VectorXd & v)
{
Eigen::VectorXd integ(model.nq);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
{
IntegrateStep::run(model.joints[i],
IntegrateStep::ArgsType (q, v, integ)
......@@ -183,7 +183,7 @@ namespace se3
const double u)
{
Eigen::VectorXd interp(model.nq);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
{
InterpolateStep::run(model.joints[i],
InterpolateStep::ArgsType (q0, q1, u, interp)
......@@ -218,7 +218,7 @@ namespace se3
const Eigen::VectorXd & q1)
{
Eigen::VectorXd diff(model.nv);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
{
DifferentiateStep::run(model.joints[i],
DifferentiateStep::ArgsType (q0, q1, diff)
......@@ -254,8 +254,8 @@ namespace se3
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
Eigen::VectorXd distances(model.nbody-1);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
Eigen::VectorXd distances(model.njoint-1);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
{
DistanceStep::run(model.joints[i],
DistanceStep::ArgsType (i-1, q0, q1, distances)
......@@ -291,7 +291,7 @@ namespace se3
randomConfiguration(const Model & model, const Eigen::VectorXd & lowerLimits, const Eigen::VectorXd & upperLimits)
{
Eigen::VectorXd q(model.nq);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
{
RandomConfiguration::run(model.joints[i],
RandomConfiguration::ArgsType ( q, lowerLimits, upperLimits)
......
......@@ -44,7 +44,7 @@ namespace se3
inline void emptyForwardPass(const Model & model,
Data & data)
{
for (Model::JointIndex i=1; i < (Model::JointIndex) model.nbody; ++i)
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoint; ++i)
{
emptyForwardStep::run(model.joints[i],
data.joints[i],
......@@ -91,7 +91,7 @@ namespace se3
{
assert(q.size() == model.nq && "The configuration vector is not of right size");
for (Model::JointIndex i=1; i < (Model::JointIndex) model.nbody; ++i)
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoint; ++i)
{
ForwardKinematicZeroStep::run(model.joints[i], data.joints[i],
ForwardKinematicZeroStep::ArgsType (model,data,q)
......@@ -146,7 +146,7 @@ namespace se3
data.v[0].setZero();
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
{
ForwardKinematicFirstStep::run(model.joints[i],data.joints[i],
ForwardKinematicFirstStep::ArgsType(model,data,q,v));
......@@ -207,7 +207,7 @@ namespace se3
data.v[0].setZero();
data.a[0].setZero();
for( Model::JointIndex i=1; i < (Model::JointIndex) model.nbody; ++i )
for( Model::JointIndex i=1; i < (Model::JointIndex) model.njoint; ++i )
{
ForwardKinematicSecondStep::run(model.joints[i],data.joints[i],
ForwardKinematicSecondStep::ArgsType(model,data,q,v,a));
......
......@@ -93,13 +93,13 @@ namespace se3
data.v[0].setZero();
data.a_gf[0] = -model.gravity;
for( Model::JointIndex i=1;i<(Model::JointIndex)model.nbody;++i )
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoint;++i )
{
RneaForwardStep::run(model.joints[i],data.joints[i],
RneaForwardStep::ArgsType(model,data,q,v,a));
}
for( Model::JointIndex i=(Model::JointIndex)model.nbody-1;i>0;--i )
for( Model::JointIndex i=(Model::JointIndex)model.njoint-1;i>0;--i )
{
RneaBackwardStep::run(model.joints[i],data.joints[i],
RneaBackwardStep::ArgsType(model,data));
......@@ -174,13 +174,13 @@ namespace se3
data.v[0].setZero ();
data.a_gf[0] = -model.gravity;
for( size_t i=1;i<(size_t) model.nbody;++i )
for( size_t i=1;i<(size_t) model.njoint;++i )
{
NLEForwardStep::run(model.joints[i],data.joints[i],
NLEForwardStep::ArgsType(model,data,q,v));
}
for( size_t i=(size_t) (model.nbody-1);i>0;--i )
for( size_t i=(size_t) (model.njoint-1);i>0;--i )
{
NLEBackwardStep::run(model.joints[i],data.joints[i],
NLEBackwardStep::ArgsType(model,data));
......
......@@ -30,8 +30,8 @@ namespace se3
{
inline std::ostream& operator<< (std::ostream & os, const Model & model)
{
os << "Nb bodies = " << model.nbody << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
os << "Nb joints = " << model.njoint << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
for(Model::Index i=0;i<(Model::Index)(model.njoint);++i)
{
os << " Joint "<< model.names[i] << ": parent=" << model.parents[i] << std::endl;
}
......@@ -240,35 +240,35 @@ namespace se3
inline Data::Data (const Model & ref)
:model(ref)
,joints(0)
,a((std::size_t)ref.nbody)
,a_gf((std::size_t)ref.nbody)
,v((std::size_t)ref.nbody)
,f((std::size_t)ref.nbody)
,oMi((std::size_t)ref.nbody)
,liMi((std::size_t)ref.nbody)
,a((std::size_t)ref.njoint)
,a_gf((std::size_t)ref.njoint)
,v((std::size_t)ref.njoint)
,f((std::size_t)ref.njoint)
,oMi((std::size_t)ref.njoint)
,liMi((std::size_t)ref.njoint)
,tau(ref.nv)
,nle(ref.nv)
,oMf((std::size_t)ref.nFrames)
,Ycrb((std::size_t)ref.nbody)
,Ycrb((std::size_t)ref.njoint)
,M(ref.nv,ref.nv)
,ddq(ref.nv)
,Yaba((std::size_t)ref.nbody)
,Yaba((std::size_t)ref.njoint)
,u(ref.nv)
,Ag(6,ref.nv)
,Fcrb((std::size_t)ref.nbody)
,lastChild((std::size_t)ref.nbody)
,nvSubtree((std::size_t)ref.nbody)
,Fcrb((std::size_t)ref.njoint)
,lastChild((std::size_t)ref.njoint)
,nvSubtree((std::size_t)ref.njoint)
,U(ref.nv,ref.nv)
,D(ref.nv)
,tmp(ref.nv)
,parents_fromRow((std::size_t)ref.nv)
,nvSubtree_fromRow((std::size_t)ref.nv)
,J(6,ref.nv)
,iMf((std::size_t)ref.nbody)
,com((std::size_t)ref.nbody)
,vcom((std::size_t)ref.nbody)
,acom((std::size_t)ref.nbody)
,mass((std::size_t)ref.nbody)
,iMf((std::size_t)ref.njoint)
,com((std::size_t)ref.njoint)
,vcom((std::size_t)ref.njoint)
,acom((std::size_t)ref.njoint)
,mass((std::size_t)ref.njoint)
,Jcom(3,ref.nv)
,JMinvJt()
,llt_JMinvJt()
......@@ -279,12 +279,12 @@ namespace se3
,impulse_c()
{
/* Create data strcture associated to the joints */
for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i)
for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i)
joints.push_back(CreateJointData::run(model.joints[i]));
/* Init for CRBA */
M.fill(0);
for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); }
for(Model::Index i=0;i<(Model::Index)(ref.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
computeLastChild(ref);
/* Init for Cholesky */
......@@ -309,7 +309,7 @@ namespace se3
{
typedef Model::Index Index;
std::fill(lastChild.begin(),lastChild.end(),-1);
for( int i=model.nbody-1;i>=0;--i )
for( int i=model.njoint-1;i>=0;--i )
{
if(lastChild[(Index)i] == -1) lastChild[(Index)i] = i;
const Index & parent = model.parents[(Index)i];
......@@ -323,7 +323,7 @@ namespace se3
inline void Data::computeParents_fromRow (const Model & model)
{
for( Model::Index joint=1;joint<(Model::Index)(model.nbody);joint++)
for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
{
const Model::Index & parent = model.parents[joint];
const int nvj = nv (model.joints[joint]);
......
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