Commit 9a134cee authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #329 from fvalenza/topic/bindingsUpdate

update python's API + minor modifs on name conventions
parents ffe8faa7 32476206
......@@ -118,6 +118,7 @@ SET(${PROJECT_NAME}_MATH_HEADERS
math/fwd.hpp
math/sincos.hpp
math/quaternion.hpp
math/matrix.hpp
)
SET(${PROJECT_NAME}_TOOLS_HEADERS
......
......@@ -57,6 +57,21 @@ namespace se3
{
return randomConfiguration(*model, lowerPosLimit, upperPosLimit);
}
static void normalize_proxy(const ModelHandler & model,
VectorXd_fx & config)
{
Eigen::VectorXd q(config);
normalize(*model, q);
config = q;
}
static bool isSameConfiguration_proxy(const ModelHandler & model,
const VectorXd_fx & q1,
const VectorXd_fx & q2)
{
return isSameConfiguration(*model, q1, q2);
}
void exposeJointsAlgo()
{
......@@ -88,6 +103,15 @@ namespace se3
"Joint lower limits (size Model::nq)",
"Joint upper limits (size Model::nq)"),
"Generate a random configuration ensuring provied joint limits are respected ");
bp::def("normalize",normalize_proxy,
bp::args("Model",
"Configuration q (size Model::nq)"),
"return the configuration normalized ");
bp::def("isSameConfiguration",isSameConfiguration_proxy,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"),
"Return true if two configurations are equivalent");
}
} // namespace python
......
......@@ -85,6 +85,7 @@ namespace se3
.def("__add__",&ForcePythonVisitor::add)
.def("__sub__",&ForcePythonVisitor::subst)
.def("__neg__",&ForcePythonVisitor::neg)
.def(bp::self_ns::str(bp::self_ns::self))
.def(bp::self_ns::repr(bp::self_ns::self))
......@@ -108,6 +109,7 @@ namespace se3
static Force_fx add(const Force_fx & f1, const Force_fx & f2) { return f1+f2; }
static Force_fx subst(const Force_fx & f1, const Force_fx & f2) { return f1-f2; }
static Force_fx neg(const Force_fx & f1) { return -f1; }
static void expose()
{
......
......@@ -63,6 +63,9 @@ namespace se3
&FramePythonVisitor::setPlacementWrtParentJoint,
"placement in the parent joint local frame")
.def_readwrite("type", &Frame::type, "type of the frame")
.def(bp::self_ns::str(bp::self_ns::self))
.def(bp::self_ns::repr(bp::self_ns::self))
;
}
......
......@@ -110,18 +110,33 @@ namespace se3
bp::make_function(&GeometryDataPythonVisitor::oMg,
bp::return_internal_reference<>()),
"Vector of collision objects placement relative to the world.")
#ifdef WITH_HPP_FCL
.add_property("distance_results",
bp::make_function(&GeometryDataPythonVisitor::distance_results,
#ifdef WITH_HPP_FCL
.add_property("activeCollisionPairs",
bp::make_function(&GeometryDataPythonVisitor::activeCollisionPairs,
bp::return_internal_reference<>()))
.add_property("distanceRequest",
bp::make_function(&GeometryDataPythonVisitor::distanceRequest,
bp::return_internal_reference<>()),
"Defines what information should be computed by fcl for distances")
.add_property("distanceResults",
bp::make_function(&GeometryDataPythonVisitor::distanceResults,
bp::return_internal_reference<>()),
"Vector of distance results computed in ")
.add_property("CollisionRequest",
bp::make_function(&GeometryDataPythonVisitor::CollisionRequest,
bp::return_internal_reference<>()),
"Defines what information should be computed by fcl for collision tests")
.add_property("collision_results",
bp::make_function(&GeometryDataPythonVisitor::collision_results,
bp::return_internal_reference<>()) )
.add_property("activeCollisionPairs",
bp::make_function(&GeometryDataPythonVisitor::activeCollisionPairs,
bp::return_internal_reference<>()))
.add_property("radius",
bp::make_function(&GeometryDataPythonVisitor::radius,
bp::return_internal_reference<>()),
"Vector of radius of bodies, ie distance of the further point of the geometry object from the joint center ")
.def("fillInnerOuterObjectMaps", &GeometryDataPythonVisitor::fillInnerOuterObjectMaps,
bp::args("GeometryModel"),
"Fill inner and outer objects maps")
.def("activateCollisionPair",&GeometryDataPythonVisitor::activateCollisionPair,
bp::args("pairIndex (int)"),
"Activate pair ID <pairIndex> in geomModel.collisionPairs."
......@@ -142,9 +157,15 @@ namespace se3
static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
#ifdef WITH_HPP_FCL
static std::vector<fcl::DistanceResult> & distance_results( GeometryDataHandler & m ) { return m->distanceResults; }
static std::vector<fcl::CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collisionResults; }
static std::vector<bool> & activeCollisionPairs(GeometryDataHandler & m) { return m->activeCollisionPairs; }
static fcl::DistanceRequest & distanceRequest( GeometryDataHandler & m ) { return m->distanceRequest; }
static std::vector<fcl::DistanceResult> & distanceResults( GeometryDataHandler & m ) { return m->distanceResults; }
static fcl::CollisionRequest & CollisionRequest( GeometryDataHandler & m ) { return m->collisionRequest; }
static std::vector<fcl::CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collisionResults; }
static std::vector<double> & radius( GeometryDataHandler & m ) { return m->radius; }
static void fillInnerOuterObjectMaps(GeometryDataHandler & m, const GeometryModelHandler & model)
{m->fillInnerOuterObjectMaps(*model);}
static void activateCollisionPair(GeometryDataHandler & m,
Index pairID) { m->activateCollisionPair(pairID); }
......
......@@ -28,6 +28,8 @@
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/bindings/python/model.hpp"
namespace se3
{
namespace python
......@@ -62,17 +64,19 @@ namespace se3
{
cl
.add_property("ngeoms", &GeometryModelPythonVisitor::ngeoms)
.def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
.def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
.def("getGeometryName",&GeometryModelPythonVisitor::getGeometryName)
.add_property("geometryObjects",
bp::make_function(&GeometryModelPythonVisitor::geometryObjects,
bp::return_internal_reference<>()) )
.def("addGeometryObject", &GeometryModelPythonVisitor::addGeometryObject,
bp::args("GeometryObject", "Model", "bool"),
"Add a GeometryObject to a GeometryModel")
.def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
.def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
.def("__str__",&GeometryModelPythonVisitor::toString)
#ifdef WITH_HPP_FCL
.add_property("collision_pairs",
bp::make_function(&GeometryModelPythonVisitor::collision_pairs,
.add_property("collisionPairs",
bp::make_function(&GeometryModelPythonVisitor::collisionPairs,
bp::return_internal_reference<>()),
"Vector of collision pairs.")
.def("addCollisionPair",&GeometryModelPythonVisitor::addCollisionPair,
......@@ -106,12 +110,14 @@ namespace se3
{ return gmodelPtr->getGeometryId(name); }
static bool existGeometryName(const GeometryModelHandler & gmodelPtr, const std::string & name)
{ return gmodelPtr->existGeometryName(name);}
static std::string getGeometryName(const GeometryModelHandler & gmodelPtr, const GeomIndex index)
{ return gmodelPtr->getGeometryName(index);}
static std::vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
static GeomIndex addGeometryObject( GeometryModelHandler & m, GeometryObject gobject, const ModelHandler & model, const bool autofillJointParent)
{ return m-> addGeometryObject(gobject, *model, autofillJointParent); }
#ifdef WITH_HPP_FCL
static std::vector<CollisionPair> & collision_pairs( GeometryModelHandler & m )
static std::vector<CollisionPair> & collisionPairs( GeometryModelHandler & m )
{ return m->collisionPairs; }
static void addCollisionPair (GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
......
......@@ -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,41 @@ 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("addJointFrame", &ModelPythonVisitor::addJointFrame, bp::args("jointIndex", "frameIndex"), "add the joint at index jointIndex as a frame to the frame tree")
.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 +157,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,
......@@ -175,36 +189,51 @@ namespace se3
return boost::apply_visitor(addJointVisitor(model,parent_id,joint_placement,joint_name), jmodel_variant);
}
static int addJointFrame( ModelHandler & m, const JointIndex jointIndex, int frameIndex)
{
return m->addJointFrame(jointIndex, frameIndex);
}
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()
{
......
......@@ -91,6 +91,7 @@ namespace se3
.def("__add__",&MotionPythonVisitor::add)
.def("__sub__",&MotionPythonVisitor::subst)
.def("__neg__",&MotionPythonVisitor::neg)
.def(bp::self_ns::str(bp::self_ns::self))
.def("Random",&Motion_fx::Random)
......@@ -113,6 +114,7 @@ namespace se3
static Motion_fx add( const Motion_fx& m1,const Motion_fx& m2 ) { return m1+m2; }
static Motion_fx subst( const Motion_fx& m1,const Motion_fx& m2 ) { return m1-m2; }
static Motion_fx neg(const Motion_fx & m1) { return -m1; }
static Motion_fx cross_motion( const Motion_fx& m1,const Motion_fx& m2 ) { return m1.cross(m2); }
static Force_fx cross_force( const Motion_fx& m,const Force_fx& f ) { return m.cross(f); }
......
......@@ -86,7 +86,9 @@ class ModelWrapper(object):
self.display.viewer.gui.addSphere(body_name, dimensions, color(body_color))
body_inertia.lever = lever.translation
joint_id = self.model.addJoint(parent, joint_model, joint_placement, joint_name)
self.model.appendBodyToJoint(joint_id, body_inertia, se3.SE3.Identity(), body_name)
self.model.appendBodyToJoint(joint_id, body_inertia, se3.SE3.Identity())
self.model.addJointFrame(joint_id, -1)
self.model.addBodyFrame(body_name, joint_id, se3.SE3.Identity(),-1)
self.visuals.append((body_name, joint_placement, lever))
self.data = self.model.createData()
if self.display:
......
......@@ -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[