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/geometry.hpp b/src/multibody/geometry.hpp
index fc65a07732e84bb8d2ca4044c28040826a3d0b81..a7f818740c0941d25fab46376d8886ad68ff5561 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -444,15 +444,13 @@ struct GeometryObject
GeometryData(const GeometryModel & modelGeom)
: model_geom(modelGeom)
, oMg(model_geom.ngeoms)
- , activeCollisionPairs()
- , distance_results()
- , collision_results()
+ , activeCollisionPairs(modelGeom.collisionPairs.size(), true)
+ , distance_results(modelGeom.collisionPairs.size())
+ , collision_results(modelGeom.collisionPairs.size())
, radius()
{
- activeCollisionPairs.resize(modelGeom.collisionPairs.size());
- distance_results.resize(modelGeom.collisionPairs.size());
- collision_results.resize(modelGeom.collisionPairs.size());
+ collisionObjects.reserve(modelGeom.geometryObjects.size());
BOOST_FOREACH( const GeometryObject & geom, modelGeom.geometryObjects)
{ collisionObjects.push_back
(fcl::CollisionObject(geom.collision_geometry)); }
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index a2377d27ff16d71aa043630d51fe02efc336f3d4..8157d6ee7b9b7900eab4d8a8ad74d8748bdcc293 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -362,9 +362,6 @@ namespace se3
typedef SE3::Vector3 Vector3;
public:
- /// \brief A const reference to the reference model.
- const Model & model;
-
/// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, encapsulated in JointDataAccessor.
JointDataVector joints;
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index d5846f5f5acdb3e33a8ca93dfb91857d19d3551f..cee1244d40aa2ff1b7125739c832d8783357cff7 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;
}
@@ -237,59 +237,58 @@ 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)
- ,tau(ref.nv)
- ,nle(ref.nv)
- ,oMf((std::size_t)ref.nFrames)
- ,Ycrb((std::size_t)ref.nbody)
- ,M(ref.nv,ref.nv)
- ,ddq(ref.nv)
- ,Yaba((std::size_t)ref.nbody)
- ,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)
- ,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)
- ,Jcom(3,ref.nv)
+ inline Data::Data (const Model & model)
+ :joints(0)
+ ,a((std::size_t)model.njoint)
+ ,a_gf((std::size_t)model.njoint)
+ ,v((std::size_t)model.njoint)
+ ,f((std::size_t)model.njoint)
+ ,oMi((std::size_t)model.njoint)
+ ,liMi((std::size_t)model.njoint)
+ ,tau(model.nv)
+ ,nle(model.nv)
+ ,oMf((std::size_t)model.nFrames)
+ ,Ycrb((std::size_t)model.njoint)
+ ,M(model.nv,model.nv)
+ ,ddq(model.nv)
+ ,Yaba((std::size_t)model.njoint)
+ ,u(model.nv)
+ ,Ag(6,model.nv)
+ ,Fcrb((std::size_t)model.njoint)
+ ,lastChild((std::size_t)model.njoint)
+ ,nvSubtree((std::size_t)model.njoint)
+ ,U(model.nv,model.nv)
+ ,D(model.nv)
+ ,tmp(model.nv)
+ ,parents_fromRow((std::size_t)model.nv)
+ ,nvSubtree_fromRow((std::size_t)model.nv)
+ ,J(6,model.nv)
+ ,iMf((std::size_t)model.njoint)
+ ,com((std::size_t)model.njoint)
+ ,vcom((std::size_t)model.njoint)
+ ,acom((std::size_t)model.njoint)
+ ,mass((std::size_t)model.njoint)
+ ,Jcom(3,model.nv)
,JMinvJt()
,llt_JMinvJt()
,lambda_c()
- ,sDUiJt(ref.nv,ref.nv)
- ,torque_residual(ref.nv)
+ ,sDUiJt(model.nv,model.nv)
+ ,torque_residual(model.nv)
,dq_after(model.nv)
,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); }
- computeLastChild(ref);
+ for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
+ computeLastChild(model);
/* Init for Cholesky */
U.setIdentity();
- computeParents_fromRow(ref);
+ computeParents_fromRow(model);
/* Init Jacobian */
J.setZero();
@@ -309,7 +308,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 +322,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]);
diff --git a/unittest/compute-all-terms.cpp b/unittest/compute-all-terms.cpp
index ab96ecb5486436b4fb702c4443e08234a7e63dda..368db7155c4f9ca2a3edc2c3f93aa564192642dd 100644
--- a/unittest/compute-all-terms.cpp
+++ b/unittest/compute-all-terms.cpp
@@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.nbody; ++k)
+ for (int k=0; k<model.njoint; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
@@ -103,7 +103,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.nbody; ++k)
+ for (int k=0; k<model.njoint; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
@@ -134,7 +134,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.nbody; ++k)
+ for (int k=0; k<model.njoint; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
@@ -165,7 +165,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.nbody; ++k)
+ for (int k=0; k<model.njoint; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp
index cf87062eb15b6364963b14498aba2b616bba0c2d..c361be5681fea9390acf3597bca49c0002ac737f 100644
--- a/unittest/finite-differences.cpp
+++ b/unittest/finite-differences.cpp
@@ -160,7 +160,7 @@ BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
q.segment<4>(3).normalize();
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoint-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh);
diff --git a/unittest/frames.cpp b/unittest/frames.cpp
index ff32af394e9c001c8d11d81b257e7fdc7ee84e46..1b305cf0d40c9e4100427f9c5fa8b16a92461d6d 100644
--- a/unittest/frames.cpp
+++ b/unittest/frames.cpp
@@ -39,7 +39,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
se3::Model model;
se3::buildModels::humanoidSimple(model);
- Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.nbody-1);
+ Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoint-1);
const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(frame_name, parent_idx, framePlacement);
@@ -61,7 +61,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
Model model;
buildModels::humanoidSimple(model);
- Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.nbody-1);
+ Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoint-1);
const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(frame_name, parent_idx, framePlacement);
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 71e827b1dac6a0d8cbf388a62d192e48770db9f3..dc9b53ed5e5b2ef79f3e3a0bb253d4befc0ebcd1 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -47,7 +47,7 @@ typedef std::map <std::string, se3::SE3> PositionsMap_t;
typedef std::map <std::string, se3::SE3> JointPositionsMap_t;
typedef std::map <std::string, se3::SE3> GeometryPositionsMap_t;
typedef std::map <std::pair < std::string , std::string >, fcl::DistanceResult > PairDistanceMap_t;
-JointPositionsMap_t fillPinocchioJointPositions(const se3::Data & data);
+JointPositionsMap_t fillPinocchioJointPositions(const se3::Model& model, const se3::Data & data);
GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryData & data_geom);
#ifdef WITH_HPP_MODEL_URDF
JointPositionsMap_t fillHppJointPositions(const hpp::model::HumanoidRobotPtr_t robot);
@@ -346,7 +346,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
/// ********** COMPARISON ********* ///
/// ********************************* ///
// retrieve all joint and geometry objects positions
- JointPositionsMap_t joints_pin = fillPinocchioJointPositions(data);
+ JointPositionsMap_t joints_pin = fillPinocchioJointPositions(model, data);
JointPositionsMap_t joints_hpp = fillHppJointPositions(humanoidRobot);
GeometryPositionsMap_t geom_pin = fillPinocchioGeometryPositions(data_geom);
GeometryPositionsMap_t geom_hpp = fillHppGeometryPositions(humanoidRobot);
@@ -489,12 +489,12 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
#endif
BOOST_AUTO_TEST_SUITE_END ()
-JointPositionsMap_t fillPinocchioJointPositions(const se3::Data & data)
+JointPositionsMap_t fillPinocchioJointPositions(const se3::Model& model, const se3::Data & data)
{
JointPositionsMap_t result;
- for (se3::Model::Index i = 0; i < (se3::Model::Index)data.model.nbody; ++i)
+ for (se3::Model::Index i = 0; i < (se3::Model::Index)model.njoint; ++i)
{
- result[data.model.getJointName(i)] = data.oMi[i];
+ result[model.getJointName(i)] = data.oMi[i];
}
return result;
}
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index ff3c9dfd27a2e0ca0d3b5b2ef630362fea624e3b..9c6e4b855610a773d2fc08bfd04613e8c79e98c0 100644
--- a/unittest/jacobian.cpp
+++ b/unittest/jacobian.cpp
@@ -43,7 +43,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Zero(model.nq);
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoint-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh);
@@ -110,7 +110,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 1 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoint-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -125,7 +125,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 2 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoint-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -140,7 +140,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 3 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoint-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index c33831f8c5a5d572e53112aabe7e063f99af5345..e4308c52dbd06d4cd9448802ed17b3540752106d 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -369,7 +369,7 @@ BOOST_AUTO_TEST_CASE ( distance_computation_test )
Eigen::Quaterniond quat_ff_2(q2[6],q2[3],q2[4],q2[5]);
Eigen::Quaterniond quat_spherical_2(q2[10],q2[7],q2[8],q2[9]);
- Eigen::VectorXd expected(model.nbody-1);
+ Eigen::VectorXd expected(model.njoint-1);
// Quaternion freeflyer
// Compute rotation vector between q2 and q1.
@@ -408,14 +408,14 @@ BOOST_AUTO_TEST_CASE ( distance_computation_test )
// Test Case 1 : Distance between two zero configs
//
Eigen::VectorXd q_zero(Eigen::VectorXd::Zero(model.nq));
- expected = Eigen::VectorXd::Zero(model.nbody-1);
+ expected = Eigen::VectorXd::Zero(model.njoint-1);
result = distance(model,q_zero,q_zero);
BOOST_CHECK_MESSAGE(result.isApprox(expected, 1e-12), "Distance between two zero configs of full model - wrong results");
//
// Test Case 2 : Distance between two same configs
//
- expected = Eigen::VectorXd::Zero(model.nbody-1);
+ expected = Eigen::VectorXd::Zero(model.njoint-1);
result = distance(model,q1,q1);
BOOST_CHECK_MESSAGE(result.isApprox(expected, 1e-12), "Distance between two same configs of full model - wrong results");