diff --git a/CMakeLists.txt b/CMakeLists.txt
index ed2b4e253c8ac8d93e2adb62776e05ab492ea5c1..26bb27ab58b3e45232786d25e9b827b18b2a4117 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
diff --git a/bindings/python/algorithm/expose-joints.cpp b/bindings/python/algorithm/expose-joints.cpp
index 54023c1959ecf7d4d2bb294394a1df8a466fa9fd..0c4179b72b326d2c79db0e2857ce517b7a81fc6e 100644
--- a/bindings/python/algorithm/expose-joints.cpp
+++ b/bindings/python/algorithm/expose-joints.cpp
@@ -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
diff --git a/bindings/python/force.hpp b/bindings/python/force.hpp
index 52852ff0dadc4b0549fe127c1bb168d1331b1603..5e29e6cc7c71657a4e108ed345056f8ff91e30b4 100644
--- a/bindings/python/force.hpp
+++ b/bindings/python/force.hpp
@@ -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()
{
diff --git a/bindings/python/frame.hpp b/bindings/python/frame.hpp
index 0b785165bdc03da70e9816476fe3c5fedf6b7f5f..c255ef3b349218c80d1c4c600f55af270675fec5 100644
--- a/bindings/python/frame.hpp
+++ b/bindings/python/frame.hpp
@@ -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))
;
}
diff --git a/bindings/python/geometry-data.hpp b/bindings/python/geometry-data.hpp
index 00d940ccfeb995be33b2a362632a0c3c9c75e896..b95ec75734b8d569db046cb6dcc1443c77cd5801 100644
--- a/bindings/python/geometry-data.hpp
+++ b/bindings/python/geometry-data.hpp
@@ -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); }
diff --git a/bindings/python/geometry-model.hpp b/bindings/python/geometry-model.hpp
index c2b2bfffe50aa250516f7e816fc841b804ec0e47..7fa8a1ece64c6ce96ca1cf074b6d590ed1a07ce5 100644
--- a/bindings/python/geometry-model.hpp
+++ b/bindings/python/geometry-model.hpp
@@ -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)
diff --git a/bindings/python/model.hpp b/bindings/python/model.hpp
index d8f09c450f7843739be2e58d1d13b8ade3e572c3..a9136c0cdfef877a4edadc75065ef01c4cad7a99 100644
--- a/bindings/python/model.hpp
+++ b/bindings/python/model.hpp
@@ -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()
{
diff --git a/bindings/python/motion.hpp b/bindings/python/motion.hpp
index d0be176ce44f1ffef1edc1073bdf6276cf3d7a0f..b84af9dfeb75b87a2a330eba6d46788576df9bb3 100644
--- a/bindings/python/motion.hpp
+++ b/bindings/python/motion.hpp
@@ -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); }
diff --git a/models/simple_model.py b/models/simple_model.py
index 2151c0819af9bbe67481e0e0368346469965463b..6fbf0661b35411256671d8705226a305ab885e7f 100644
--- a/models/simple_model.py
+++ b/models/simple_model.py
@@ -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:
diff --git a/python/model.py b/python/model.py
index 280a4d2378c7576d150513fd78b1744f907d6b6d..4824c16029629e12e6fea54d42245e475ccfe764 100644
--- a/python/model.py
+++ b/python/model.py
@@ -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])
diff --git a/src/algorithm/aba.hxx b/src/algorithm/aba.hxx
index e0d175532dcee8a80913342ad64d386aaaa4d961..e9c70eba3b595db14ed9a8b807d3346e1502f1c5 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.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));
diff --git a/src/algorithm/center-of-mass.hxx b/src/algorithm/center-of-mass.hxx
index 8ad804a49a49ff5bf19b655732827726b5dcfc18..af008090177ebcd86cbd520af8feb0fb9f3e6945 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.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],
diff --git a/src/algorithm/compute-all-terms.hpp b/src/algorithm/compute-all-terms.hpp
index eceab35543f10bf0a8ee28d374d7300b865ae7f5..66525c539c3b9661b2f1f9d2dd979840ec286626 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.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));
diff --git a/src/algorithm/copy.hpp b/src/algorithm/copy.hpp
index c9bc510f8db12e6b44b398745ac50160ebe45dd3..c0d5b1e913b354317ccc2a43c90e3f8f40c04423 100644
--- a/src/algorithm/copy.hpp
+++ b/src/algorithm/copy.hpp
@@ -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);
diff --git a/src/algorithm/crba.hxx b/src/algorithm/crba.hxx
index 74d0eddaa845a86363dd914252aa535acb62b18d..af917e1e1013005444cb544f154b9689bee35aef 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.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));
diff --git a/src/algorithm/energy.hpp b/src/algorithm/energy.hpp
index 6c6207e57f0645f5d3f410fe6194ec1dc0e7d980..c6287805a839aca3f2c6292de13ce629c2cd7adf 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.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);
diff --git a/src/algorithm/frames.hpp b/src/algorithm/frames.hpp
index 30fbc7c1573126c17019ce3f99d92777360c4d59..825251898c5f8b31e964282e90a24ee741f83b6e 100644
--- a/src/algorithm/frames.hpp
+++ b/src/algorithm/frames.hpp
@@ -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;
diff --git a/src/algorithm/jacobian.hxx b/src/algorithm/jacobian.hxx
index 396c54ec036ad4ea97acd7708726dd981b77282b..adae3233e437e02b84a5da1efb49bc0802cf7ca8 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.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));
diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index 78b617aec143927038bf3c6f5d55caf341a66830..d4a98f35e405fd52e411367811812ae45caa36d7 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.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 )
diff --git a/src/algorithm/kinematics.hxx b/src/algorithm/kinematics.hxx
index 51361b8124fbbd32d042499e16475779fe039e72..efb20a29fdb25e71fc3299af14b45aedab7742d1 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.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));
diff --git a/src/algorithm/rnea.hxx b/src/algorithm/rnea.hxx
index b08303b380827dd8b707fb39161c484ff831ced0..f0eeaca52a6413b932737e31611c08596730a1a1 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.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));
diff --git a/src/math/matrix.hpp b/src/math/matrix.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..08e09bb417ac1806d3e708c5a54913d02b9f77de
--- /dev/null
+++ b/src/math/matrix.hpp
@@ -0,0 +1,34 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __math_matrix_hpp__
+#define __math_matrix_hpp__
+
+#include <Eigen/Dense>
+
+namespace se3
+{
+
+ template<typename Derived>
+ inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
+ {
+ return !((m.derived().array()==m.derived().array()).all());
+ }
+
+
+}
+#endif //#ifndef __math_matrix_hpp__
diff --git a/src/multibody/frame.hpp b/src/multibody/frame.hpp
index 68fd090453f40c15e8a6be7ede6dd6dec3b84e92..c440277efc64bdc7f9c4cd352512347967a04bc6 100644
--- a/src/multibody/frame.hpp
+++ b/src/multibody/frame.hpp
@@ -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)
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index b2249094a6a445bd6a36fb438f0da81c226d05eb..e9b00f5578b4b1246ec3e8ddf2c79d3af9c5f39b 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -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
///
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 6e478a9bcf85b0e1847afd6590435afda705e897..1af6822d90bd979f43f5f1d16d15654ba040af9a 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -57,13 +57,13 @@ namespace se3
int nv;
/// \brief Number of joints.
- int njoint;
+ int njoints;
/// \brief Number of bodies.
- int nbody;
+ int nbodies;
/// \brief Number of operational frames.
- int nFrames;
+ int nframes;
/// \brief Spatial inertias of the body <i> expressed in the supporting joint frame <i>.
std::vector<Inertia> inertias;
@@ -111,9 +111,9 @@ namespace se3
Model()
: nq(0)
, nv(0)
- , njoint(1)
- , nbody(1)
- , nFrames(0)
+ , njoints(1)
+ , nbodies(1)
+ , nframes(0)
, inertias(1)
, jointPlacements(1, SE3::Identity())
, joints(1)
@@ -249,14 +249,6 @@ namespace se3
///
bool existBodyName(const std::string & name) const;
- ///
- /// \brief Get the name of a body given by its index.
- ///
- /// \param[in] index Index of the body.
- ///
- /// \return Name of the body.
- ///
- const std::string & getBodyName(const JointIndex index) const;
///
/// \brief Return the index of a joint given by its name.
@@ -286,7 +278,7 @@ namespace se3
///
/// \return Name of the joint.
///
- const std::string & getJointName(const JointIndex index) const;
+ PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) const;
///
/// \brief Returns the index of a frame given by its name.
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 6f5dee8033c5648014db4e9830d616c0241b865e..8b21b59a1c91f307feda2860778161c1352fa726 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -43,8 +43,8 @@ namespace se3
inline std::ostream& operator<< (std::ostream & os, const Model & model)
{
- 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 << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
+ for(Model::Index i=0;i<(Model::Index)(model.njoints);++i)
{
os << " Joint "<< model.names[i] << ": parent=" << model.parents[i] << std::endl;
}
@@ -64,17 +64,17 @@ namespace se3
)
{
typedef JointModelDerived D;
- assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
- &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
+ assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
+ &&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
assert(max_effort.size() == joint_model.nv()
&& max_velocity.size() == joint_model.nv()
&& min_config.size() == joint_model.nq()
&& max_config.size() == joint_model.nq());
-
- Model::JointIndex idx = (Model::JointIndex) (njoint++);
-
+
+ Model::JointIndex idx = (Model::JointIndex) (njoints++);
+
joints .push_back(JointModel(joint_model.derived()));
boost::get<JointModelDerived>(joints.back()).setIndexes(idx,nq,nv);
@@ -141,7 +141,7 @@ namespace se3
{
const Inertia & iYf = Y.se3Action(body_placement);
inertias[joint_index] += iYf;
- nbody++;
+ nbodies++;
}
inline int Model::addBodyFrame (const std::string & body_name,
@@ -168,11 +168,6 @@ namespace se3
return existFrame(name, BODY);
}
- inline const std::string& Model::getBodyName (const Model::JointIndex index) const
- {
- assert( index < (Model::Index)nbody );
- return frames[index].name;
- }
inline Model::JointIndex Model::getJointId (const std::string & name) const
{
@@ -218,8 +213,8 @@ namespace se3
if( !existFrame(frame.name, frame.type) )
{
frames.push_back(frame);
- nFrames++;
- return nFrames - 1;
+ nframes++;
+ return nframes - 1;
}
else
{
@@ -236,35 +231,35 @@ namespace se3
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)
+ ,a((std::size_t)model.njoints)
+ ,a_gf((std::size_t)model.njoints)
+ ,v((std::size_t)model.njoints)
+ ,f((std::size_t)model.njoints)
+ ,oMi((std::size_t)model.njoints)
+ ,liMi((std::size_t)model.njoints)
,tau(model.nv)
,nle(model.nv)
- ,oMf((std::size_t)model.nFrames)
- ,Ycrb((std::size_t)model.njoint)
+ ,oMf((std::size_t)model.nframes)
+ ,Ycrb((std::size_t)model.njoints)
,M(model.nv,model.nv)
,ddq(model.nv)
- ,Yaba((std::size_t)model.njoint)
+ ,Yaba((std::size_t)model.njoints)
,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)
+ ,Fcrb((std::size_t)model.njoints)
+ ,lastChild((std::size_t)model.njoints)
+ ,nvSubtree((std::size_t)model.njoints)
,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)
+ ,iMf((std::size_t)model.njoints)
+ ,com((std::size_t)model.njoints)
+ ,vcom((std::size_t)model.njoints)
+ ,acom((std::size_t)model.njoints)
+ ,mass((std::size_t)model.njoints)
,Jcom(3,model.nv)
,JMinvJt()
,llt_JMinvJt()
@@ -275,12 +270,12 @@ namespace se3
,impulse_c()
{
/* Create data strcture associated to the joints */
- for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::Index i=0;i<(Model::JointIndex)(model.njoints);++i)
joints.push_back(CreateJointData::run(model.joints[i]));
/* Init for CRBA */
M.fill(0);
- for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
+ for(Model::Index i=0;i<(Model::Index)(model.njoints);++i ) { Fcrb[i].resize(6,model.nv); }
computeLastChild(model);
/* Init for Cholesky */
@@ -305,7 +300,7 @@ namespace se3
{
typedef Model::Index Index;
std::fill(lastChild.begin(),lastChild.end(),-1);
- for( int i=model.njoint-1;i>=0;--i )
+ for( int i=model.njoints-1;i>=0;--i )
{
if(lastChild[(Index)i] == -1) lastChild[(Index)i] = i;
const Index & parent = model.parents[(Index)i];
@@ -319,7 +314,7 @@ namespace se3
inline void Data::computeParents_fromRow (const Model & model)
{
- for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
+ for( Model::Index joint=1;joint<(Model::Index)(model.njoints);joint++)
{
const Model::Index & parent = model.parents[joint];
const int nvj = nv (model.joints[joint]);
diff --git a/src/parsers/lua.cpp b/src/parsers/lua.cpp
index 34451a30e0e4838d9dfe6fb07f7d9c52a4c39953..085ba645fddc3d3632d166ff50a8346c287bf25b 100644
--- a/src/parsers/lua.cpp
+++ b/src/parsers/lua.cpp
@@ -303,7 +303,7 @@ namespace se3
// ???
model.addBodyFrame(randomStringGenerator(8), parent_id, global_placement);
- joint_id = (Model::JointIndex)model.njoint;
+ joint_id = (Model::JointIndex)model.njoints;
fixed_body_table_id_map[body_name] = parent_id;
fixed_placement_map[body_name] = global_placement;
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index c7cb4e2670a099fa7c9603f988606f2e75c548a1..d7799c7c3995ab80d659e4059f0ac62ec83fe19d 100644
--- a/src/parsers/urdf/model.cpp
+++ b/src/parsers/urdf/model.cpp
@@ -16,6 +16,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
+#include "pinocchio/math/matrix.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/multibody/model.hpp"
@@ -75,8 +76,8 @@ namespace se3
// Reference to model.frames[fid] can has changed because the vector
// may have been reallocated.
if (model.frames[fid].parent > 0) {
- assert (!model.inertias[model.frames[fid].parent].lever().hasNaN()
- && !model.inertias[model.frames[fid].parent].inertia().data().hasNaN());
+ assert (!hasNaN(model.inertias[model.frames[fid].parent].lever())
+ && !hasNaN(model.inertias[model.frames[fid].parent].inertia().data()));
}
}
diff --git a/unittest/compute-all-terms.cpp b/unittest/compute-all-terms.cpp
index 368db7155c4f9ca2a3edc2c3f93aa564192642dd..9f8c9cb9c38dffd790600170549a5a9ccf6c6b90 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.njoint; ++k)
+ for (int k=0; k<model.njoints; ++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.njoint; ++k)
+ for (int k=0; k<model.njoints; ++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.njoint; ++k)
+ for (int k=0; k<model.njoints; ++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.njoint; ++k)
+ for (int k=0; k<model.njoints; ++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 1677ac8050f8e7e3a0bb186b629ccafe861f2429..deaf7b9b2ae02c7071c0bacb6d75a7ad39610305 100644
--- a/unittest/finite-differences.cpp
+++ b/unittest/finite-differences.cpp
@@ -208,7 +208,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.njoint-1);
+ Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-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 c3830741df3c229174490f5250b4f603c200f260..7b08529aeed99103f18a769ebd4e797b6d36353d 100644
--- a/unittest/frames.cpp
+++ b/unittest/frames.cpp
@@ -39,8 +39,8 @@ 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.njoint-1);
- const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
+ Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+ const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, -1, framePlacement, OP_FRAME));
se3::Data data(model);
@@ -61,8 +61,8 @@ 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.njoint-1);
- const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
+ Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+ const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, -1, framePlacement, OP_FRAME));
se3::Data data(model);
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 3ad8253ac238b897aac57ce94e6f639ff9a40de6..3ed7e473c4c09c0e238e41cf266458c618fcff2d 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -531,9 +531,9 @@ BOOST_AUTO_TEST_SUITE_END ()
JointPositionsMap_t fillPinocchioJointPositions(const se3::Model& model, const se3::Data & data)
{
JointPositionsMap_t result;
- for (se3::Model::Index i = 0; i < (se3::Model::Index)model.njoint; ++i)
+ for (se3::Model::Index i = 0; i < (se3::Model::Index)model.njoints; ++i)
{
- result[model.getJointName(i)] = data.oMi[i];
+ result[model.names[i]] = data.oMi[i];
}
return result;
}
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index 9c6e4b855610a773d2fc08bfd04613e8c79e98c0..c664eed8c5ef903a746c0893310a730da178c201 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.njoint-1);
+ Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-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.njoint-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-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.njoint-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-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.njoint-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp
index dbefb5c1d188816afdc8752899b5f92958a6d5b7..ef342624c57d04a13a3e4f07510deb88f3619b56 100644
--- a/unittest/joint-composite.cpp
+++ b/unittest/joint-composite.cpp
@@ -268,8 +268,8 @@ BOOST_AUTO_TEST_CASE ( test_R3xSO3)
computeAllTerms(model_composite,data_composite,q,q_dot);
- Model::Index index_joint_R3xSO3 = (Model::Index) model_zero_mass.njoint-1;
- Model::Index index_joint_composite = (Model::Index) model_composite.njoint-1;
+ Model::Index index_joint_R3xSO3 = (Model::Index) model_zero_mass.njoints-1;
+ Model::Index index_joint_composite = (Model::Index) model_composite.njoints-1;
BOOST_CHECK_MESSAGE(data_composite.oMi[index_joint_composite]
diff --git a/unittest/model.cpp b/unittest/model.cpp
index 341a6aeb2e997b69852924c87d2ea2259a18a7c4..eac27fef59898e187fe406627a3f70cf6fb7c219 100644
--- a/unittest/model.cpp
+++ b/unittest/model.cpp
@@ -34,7 +34,7 @@ BOOST_AUTO_TEST_CASE(test_model_subtree)
buildModels::humanoidSimple(model);
Model::JointIndex idx_larm1 = model.getJointId("larm1_joint");
- BOOST_CHECK(idx_larm1<model.njoint);
+ BOOST_CHECK(idx_larm1<model.njoints);
Model::IndexVector subtree = model.subtrees[idx_larm1];
BOOST_CHECK(subtree.size()==6);
diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp
index 377f0e474bc92a388254e7afb30b15375eea64e9..30afec1d16005941541203aced37380934185444 100644
--- a/unittest/visitor.cpp
+++ b/unittest/visitor.cpp
@@ -85,7 +85,7 @@ BOOST_AUTO_TEST_CASE ( test_runal )
model.addJoint(0,se3::JointModelRevoluteUnaligned(0,0,1),se3::SE3::Random());
Data data(model);
- for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoint;++i )
+ for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
{
SimpleVisitor::run(model.joints[i],data.joints[i],
SimpleVisitor::ArgsType(model,data,i));