Commit d23a19f1 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++] Corrected naming conventions for Model:: nframes, njoints, nbodies

parent ffe8faa7
......@@ -94,16 +94,15 @@ namespace se3
void visit(PyClass& cl) const
{
cl
.def("getBodyId",&ModelPythonVisitor::getBodyId)
.def("getJointId",&ModelPythonVisitor::getJointId)
.def("createData",&ModelPythonVisitor::createData)
// Class Members
.def("__str__",&ModelPythonVisitor::toString)
.add_property("nq", &ModelPythonVisitor::nq)
.add_property("nv", &ModelPythonVisitor::nv)
.add_property("njoint", &ModelPythonVisitor::njoint)
.add_property("nbody", &ModelPythonVisitor::nbody)
.add_property("njoints", &ModelPythonVisitor::njoints)
.add_property("nbodies", &ModelPythonVisitor::nbodies)
.add_property("nframes", &ModelPythonVisitor::nframes)
.add_property("inertias",
bp::make_function(&ModelPythonVisitor::inertias,
bp::return_internal_reference<>()) )
......@@ -116,30 +115,40 @@ namespace se3
.add_property("parents",
bp::make_function(&ModelPythonVisitor::parents,
bp::return_internal_reference<>()) )
.add_property("subtrees",
bp::make_function(&ModelPythonVisitor::subtrees,
bp::return_internal_reference<>()), "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.")
.add_property("names",
bp::make_function(&ModelPythonVisitor::names,
bp::return_internal_reference<>()) )
.def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
.def("appendBodyToJoint",&ModelPythonVisitor::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
.add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
.add_property("neutralConfiguration", bp::make_function(&ModelPythonVisitor::neutralConfiguration), "Joint's neutral configurations")
.add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
.add_property("velocityLimit", bp::make_function(&ModelPythonVisitor::velocityLimit), "Joint max velocity")
.add_property("lowerPositionLimit", bp::make_function(&ModelPythonVisitor::lowerPositionLimit), "Limit for joint lower position")
.add_property("upperPositionLimit", bp::make_function(&ModelPythonVisitor::upperPositionLimit), "Limit for joint upper position")
.add_property("frames", bp::make_function(&ModelPythonVisitor::frames, bp::return_internal_reference<>()),"Vector of frames contained in the model.")
.def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, const FrameIndex, const SE3_fx &,const FrameType &)) &ModelPythonVisitor::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
.def("addFrame",(bool (*)(ModelHandler&,const Frame &)) &ModelPythonVisitor::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
.add_property("frames", bp::make_function(&ModelPythonVisitor::frames, bp::return_internal_reference<>()),"Vector of frames contained in the model.")
.def("existFrame",&ModelPythonVisitor::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
.def("getFrameId",&ModelPythonVisitor::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
.add_property("subtrees",
bp::make_function(&ModelPythonVisitor::subtrees,
bp::return_internal_reference<>()), "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.")
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
// Class Methods
.def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
// ADD addJoint with limits ? See boost::python overloading/default parameters
.def("appendBodyToJoint",&ModelPythonVisitor::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
.def("addBodyFrame", &ModelPythonVisitor::addBodyFrame, bp::args("body_name", "parentJoint", "body_plaement", "previous_frame(parent frame)"), "add a body to the frame tree")
.def("getBodyId",&ModelPythonVisitor::getBodyId, bp::args("name"), "Return the index of a frame of type BODY given by its name")
.def("existBodyName", &ModelPythonVisitor::existBodyName, bp::args("name"), "Check if a frame of type BODY exists, given its name")
.def("getJointId",&ModelPythonVisitor::getJointId, bp::args("name"), "Return the index of a joint given by its name")
.def("existJointName", &ModelPythonVisitor::existJointName, bp::args("name"), "Check if a joint given by its name exists")
.def("getFrameId",&ModelPythonVisitor::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
.def("existFrame",&ModelPythonVisitor::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
.def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, const FrameIndex, const SE3_fx &,const FrameType &)) &ModelPythonVisitor::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
.def("addFrame",(bool (*)(ModelHandler&,const Frame &)) &ModelPythonVisitor::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
.def("createData",&ModelPythonVisitor::createData)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
.staticmethod("BuildEmptyModel")
.def("BuildHumanoidSimple",&ModelPythonVisitor::maker_humanoidSimple)
......@@ -147,24 +156,28 @@ namespace se3
;
}
static Model::Index getBodyId( const ModelHandler & modelPtr, const std::string & name )
{ return modelPtr->getBodyId(name); }
static Model::Index getJointId( const ModelHandler & modelPtr, const std::string & name )
{ return modelPtr->getJointId(name); }
static boost::shared_ptr<Data> createData(const ModelHandler& m )
{ return boost::shared_ptr<Data>( new Data(*m) ); }
static int nq( ModelHandler & m ) { return m->nq; }
static int nv( ModelHandler & m ) { return m->nv; }
static int njoint( ModelHandler & m ) { return m->njoint; }
static int nbody( ModelHandler & m ) { return m->nbody; }
static int njoints( ModelHandler & m ) { return m->njoints; }
static int nbodies( ModelHandler & m ) { return m->nbodies; }
static int nframes( ModelHandler & m ) { return m->nframes; }
static std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
static JointModelVector & joints( ModelHandler & m ) { return m->joints; }
static std::vector<Model::JointIndex> & parents( ModelHandler & m ) { return m->parents; }
static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
static Eigen::VectorXd neutralConfiguration(ModelHandler & m) {return m->neutralConfiguration;}
static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;}
static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}
static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;}
static Eigen::VectorXd upperPositionLimit(ModelHandler & m) {return m->upperPositionLimit;}
static std::vector<Frame> & frames ( ModelHandler & m ) {return m->frames; }
static std::vector<Model::IndexVector> & subtrees(ModelHandler & m) { return m->subtrees; }
static Motion gravity( ModelHandler & m ) { return m->gravity; }
static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
static JointIndex addJoint(ModelHandler & model,
JointIndex parent_id,
bp::object jmodel,
......@@ -178,33 +191,43 @@ namespace se3
static void appendBodyToJoint(ModelHandler & model,
const JointIndex joint_parent_id,
const Inertia_fx & inertia,
const SE3_fx & body_placement,
const std::string & body_name)
const SE3_fx & body_placement)
{
model->appendBodyToJoint(joint_parent_id,inertia,body_placement);
}
static Eigen::VectorXd neutralConfiguration(ModelHandler & m) {return m->neutralConfiguration;}
static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;}
static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}
static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;}
static Eigen::VectorXd upperPositionLimit(ModelHandler & m) {return m->upperPositionLimit;}
static bool addFrame(ModelHandler & m, const Frame & frame) { return m->addFrame(frame); }
static bool addFrame( ModelHandler & m, const std::string & frameName, const JointIndex parentJoint, const FrameIndex parentFrame, const SE3_fx & placementWrtParent, const FrameType & type)
static bool addBodyFrame( ModelHandler & m, const std::string & bodyName, const JointIndex parentJoint, const SE3_fx & bodyPlacement, int previousFrame)
{
return m->addFrame(Frame(frameName,parentJoint,parentFrame,placementWrtParent,type));
return m->addBodyFrame(bodyName,parentJoint,bodyPlacement,previousFrame);
}
static Model::Index getBodyId( const ModelHandler & m, const std::string & name )
{ return m->getBodyId(name); }
static bool existBodyName( const ModelHandler & m, const std::string & name )
{ return m->existBodyName(name); }
static Model::Index getJointId( const ModelHandler & m, const std::string & name )
{ return m->getJointId(name); }
static bool existJointName( const ModelHandler & m, const std::string & name )
{ return m->existJointName(name); }
static Model::FrameIndex getFrameId(const ModelHandler & m, const std::string & frame_name)
{ return m->getFrameId(frame_name); }
static bool existFrame(const ModelHandler & m, const std::string & frame_name)
{ return m->existFrame(frame_name); }
static bool addFrame(ModelHandler & m, const Frame & frame) { return m->addFrame(frame); }
static bool addFrame( ModelHandler & m, const std::string & frameName, const JointIndex parentJoint, const FrameIndex parentFrame, const SE3_fx & placementWrtParent, const FrameType & type)
{
return m->addFrame(Frame(frameName,parentJoint,parentFrame,placementWrtParent,type));
}
static std::vector<Frame> & frames (ModelHandler & m ) { return m->frames;}
static Motion gravity( ModelHandler & m ) { return m->gravity; }
static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
static boost::shared_ptr<Data> createData(const ModelHandler& m )
{ return boost::shared_ptr<Data>( new Data(*m) ); }
static ModelHandler maker_empty()
{
......
......@@ -10,14 +10,14 @@ class TestModel(TestCase):
def test_empty_model_sizes(self):
model = se3.Model.BuildEmptyModel()
self.assertEqual(model.nbody, 1)
self.assertEqual(model.nbodies, 1)
self.assertEqual(model.nq, 0)
self.assertEqual(model.nv, 0)
def test_model(self):
model = self.model
nb = 28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
self.assertEqual(model.nbody, nb)
self.assertEqual(model.nbodies, nb)
self.assertEqual(model.nq, nb - 1 + 6)
self.assertEqual(model.nv, nb - 1 + 5)
......@@ -46,11 +46,11 @@ class TestModel(TestCase):
q = zero(model.nq)
qdot = zero(model.nv)
qddot = zero(model.nv)
for i in range(model.nbody):
for i in range(model.nbodies):
data.a[i] = se3.Motion.Zero()
se3.rnea(model, data, q, qdot, qddot)
for i in range(model.nbody):
for i in range(model.nbodies):
self.assertApprox(data.v[i].np, zero(6))
self.assertApprox(data.a_gf[0].np, -model.gravity.np)
self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
......@@ -197,19 +197,19 @@ namespace se3
data.a[0] = -model.gravity;
data.u = tau;
for(Model::Index i=1;i<(Model::Index)model.njoint;++i)
for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
{
AbaForwardStep1::run(model.joints[i],data.joints[i],
AbaForwardStep1::ArgsType(model,data,q,v));
}
for( Model::Index i=(Model::Index)model.njoint-1;i>0;--i )
for( Model::Index i=(Model::Index)model.njoints-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.njoint;++i)
for(Model::Index i=1;i<(Model::Index)model.njoints;++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.njoint);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++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.njoint-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoints-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.njoint);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++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.njoint-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoints-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.njoint);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++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.njoint-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
......@@ -276,7 +276,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
......@@ -286,7 +286,7 @@ namespace se3
}
// Backward step
for( Model::JointIndex i= (Model::JointIndex) (model.njoint-1);i>0;--i )
for( Model::JointIndex i= (Model::JointIndex) (model.njoints-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.njoint;++i)
for(Model::JointIndex i=1;i<(Model::JointIndex) model.njoints;++i)
{
CATForwardStep::run(model.joints[i],data.joints[i],
CATForwardStep::ArgsType(model,data,q,v));
}
for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1);i>0;--i)
for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1);i>0;--i)
{
CATBackwardStep::run(model.joints[i],data.joints[i],
CATBackwardStep::ArgsType(model,data));
......
......@@ -47,7 +47,7 @@ namespace se3
inline void
copy (const Model& model, const Data & origin, Data & dest )
{
for( se3::JointIndex jid=1; int(jid)<model.njoint; ++ jid )
for( se3::JointIndex jid=1; int(jid)<model.njoints; ++ jid )
{
assert(LEVEL>=0);
......
......@@ -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.njoint);++i )
for( Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
CrbaForwardStep::ArgsType(model,data,q));
}
for( Model::JointIndex i=(Model::JointIndex)(model.njoint-1);i>0;--i )
for( Model::JointIndex i=(Model::JointIndex)(model.njoints-1);i>0;--i )
{
CrbaBackwardStep::run(model.joints[i],data.joints[i],
CrbaBackwardStep::ArgsType(model,data));
......@@ -207,11 +207,11 @@ namespace se3
forwardKinematics(model, data, q);
data.Ycrb[0].setZero();
for(Model::Index i=1;i<(Model::Index)(model.njoint);++i )
for(Model::Index i=1;i<(Model::Index)(model.njoints);++i )
data.Ycrb[i] = model.inertias[i];
for(Model::Index i=(Model::Index)(model.njoint-1);i>0;--i)
for(Model::Index i=(Model::Index)(model.njoints-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.njoint);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++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.njoint);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++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);
......
......@@ -84,7 +84,7 @@ namespace se3
Data & data
)
{
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nFrames; ++i)
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nframes; ++i)
{
const Frame & frame = model.frames[i];
const Model::JointIndex & parent = frame.parent;
......
......@@ -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.njoint;++i )
for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++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.njoint-1 = number of joints)
* @return The corresponding distances for each joint (size model.njoints-1 = number of joints)
*/
inline Eigen::VectorXd distance(const Model & model,
const Eigen::VectorXd & q0,
......@@ -157,7 +157,7 @@ namespace se3
const Eigen::VectorXd & v)
{
Eigen::VectorXd integ(model.nq);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
IntegrateStep::run(model.joints[i],
IntegrateStep::ArgsType (q, v, integ)
......@@ -196,7 +196,7 @@ namespace se3
const double u)
{
Eigen::VectorXd interp(model.nq);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
InterpolateStep::run(model.joints[i],
InterpolateStep::ArgsType (q0, q1, u, interp)
......@@ -231,7 +231,7 @@ namespace se3
const Eigen::VectorXd & q1)
{
Eigen::VectorXd diff(model.nv);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
DifferentiateStep::run(model.joints[i],
DifferentiateStep::ArgsType (q0, q1, diff)
......@@ -267,8 +267,8 @@ namespace se3
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
Eigen::VectorXd distances(model.njoint-1);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
Eigen::VectorXd distances(model.njoints-1);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
DistanceStep::run(model.joints[i],
DistanceStep::ArgsType (i-1, q0, q1, distances)
......@@ -304,7 +304,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.njoint; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
RandomConfiguration::run(model.joints[i],
RandomConfiguration::ArgsType ( q, lowerLimits, upperLimits)
......@@ -337,7 +337,7 @@ namespace se3
normalize(const Model & model,
Eigen::VectorXd & q)
{
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
NormalizeStep::run(model.joints[i],
NormalizeStep::ArgsType (q));
......@@ -369,7 +369,7 @@ namespace se3
const Eigen::VectorXd & q2)
{
bool result = false;
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
IsSameConfigurationStep::run(model.joints[i], IsSameConfigurationStep::ArgsType (result,q1,q2));
if( !result )
......
......@@ -44,7 +44,7 @@ namespace se3
inline void emptyForwardPass(const Model & model,
Data & data)
{
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoint; ++i)
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++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.njoint; ++i)
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++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.njoint; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++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.njoint; ++i )
for( Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++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.njoint;++i )
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
{
RneaForwardStep::run(model.joints[i],data.joints[i],
RneaForwardStep::ArgsType(model,data,q,v,a));
}
for( Model::JointIndex i=(Model::JointIndex)model.njoint-1;i>0;--i )
for( Model::JointIndex i=(Model::JointIndex)model.njoints-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.njoint;++i )
for( size_t i=1;i<(size_t) model.njoints;++i )
{
NLEForwardStep::run(model.joints[i],data.joints[i],
NLEForwardStep::ArgsType(model,data,q,v));
}
for( size_t i=(size_t) (model.njoint-1);i>0;--i )
for( size_t i=(size_t) (model.njoints-1);i>0;--i )
{
NLEBackwardStep::run(model.joints[i],data.joints[i],
NLEBackwardStep::ArgsType(model,data));
......
......@@ -97,9 +97,16 @@ namespace se3
/// \brief Type of the frame
FrameType type;
}; // struct Frame
inline std::ostream & operator << (std::ostream& os, const Frame & f)
{
os << "Frame name:" << f.name << "paired to (parent joint/ previous frame)" << "(" <<f.parent << "/" << f.previousFrame << ")"<< std::endl;
os << "with relative placement wrt parent joint:\n" << f.placement << std::endl;
return os;
}
} // namespace se3
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Frame)
......
......@@ -99,7 +99,7 @@ namespace se3
*
* @return Name of the GeometryObject
*/
const std::string & getGeometryName(const GeomIndex index) const;
PINOCCHIO_DEPRECATED const std::string & getGeometryName(const GeomIndex index) const;
#ifdef WITH_HPP_FCL
///
......
......@@ -57,13 +57,13 @@ namespace se3
int nv;
/// \brief Number of joints.
int njoint;
int njoints;