diff --git a/src/algorithm/aba.hxx b/src/algorithm/aba.hxx
index 9ecb9c4c9c4a2130cae4cbaa855adf709300d1dd..e0d175532dcee8a80913342ad64d386aaaa4d961 100644
--- a/src/algorithm/aba.hxx
+++ b/src/algorithm/aba.hxx
@@ -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));
diff --git a/src/algorithm/center-of-mass.hxx b/src/algorithm/center-of-mass.hxx
index 10571588a4db128cc4a19ca073f170d74be17479..680fd31ece03f922a94b91f48594551aa5929ad4 100644
--- a/src/algorithm/center-of-mass.hxx
+++ b/src/algorithm/center-of-mass.hxx
@@ -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],
diff --git a/src/algorithm/compute-all-terms.hpp b/src/algorithm/compute-all-terms.hpp
index ec83b54340c1135cecf1d319831ae5c363e01381..eceab35543f10bf0a8ee28d374d7300b865ae7f5 100644
--- a/src/algorithm/compute-all-terms.hpp
+++ b/src/algorithm/compute-all-terms.hpp
@@ -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));
diff --git a/src/algorithm/crba.hxx b/src/algorithm/crba.hxx
index e92988db59f2dc83abe2f9b0f3e84470cbeae5ab..eb9511711fb208c3067ad9cdbe9c41949960cf6f 100644
--- a/src/algorithm/crba.hxx
+++ b/src/algorithm/crba.hxx
@@ -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));
diff --git a/src/algorithm/energy.hpp b/src/algorithm/energy.hpp
index da060d1064881098ede7db57e5f5309c725d3dc8..6c6207e57f0645f5d3f410fe6194ec1dc0e7d980 100644
--- a/src/algorithm/energy.hpp
+++ b/src/algorithm/energy.hpp
@@ -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);
diff --git a/src/algorithm/jacobian.hxx b/src/algorithm/jacobian.hxx
index f5fcbf3670bee0e68155323d744877722fe7e158..396c54ec036ad4ea97acd7708726dd981b77282b 100644
--- a/src/algorithm/jacobian.hxx
+++ b/src/algorithm/jacobian.hxx
@@ -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));
diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index 9bb6324b6e657cd2e1eb0b13789063d188b4e3b9..ef29e29f9743251a137387a4441aec2af5988d08 100644
--- a/src/algorithm/joint-configuration.hpp
+++ b/src/algorithm/joint-configuration.hpp
@@ -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)
diff --git a/src/algorithm/kinematics.hxx b/src/algorithm/kinematics.hxx
index eb2446dd07e32b36534a3f4451799904da32c5b5..51361b8124fbbd32d042499e16475779fe039e72 100644
--- a/src/algorithm/kinematics.hxx
+++ b/src/algorithm/kinematics.hxx
@@ -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));
diff --git a/src/algorithm/rnea.hxx b/src/algorithm/rnea.hxx
index fe31feac12f14f1a7cb1e965b10f99840c6d8faa..b08303b380827dd8b707fb39161c484ff831ced0 100644
--- a/src/algorithm/rnea.hxx
+++ b/src/algorithm/rnea.hxx
@@ -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));
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index d5846f5f5acdb3e33a8ca93dfb91857d19d3551f..8f95be2297f547849a32fbab69f5f7593e6332ef 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -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]);