diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index c34ccea678c75405a19981f24624730fdcbe12df..1fcfbb548c2174397a93ee5bbcc41f35f2917b2e 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -71,7 +71,7 @@ int main()
package_dirs.push_back(romeo_meshDir);
se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
- se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs, COLLISION);
+ se3::GeometryModel geom_model; se3::urdf::buildGeom(model, romeo_filename, COLLISION, geom_model, package_dirs);
#ifdef WITH_HPP_FCL
geom_model.addAllCollisionPairs();
#endif // WITH_HPP_FCL
@@ -113,7 +113,7 @@ int main()
updateGeometryPlacements(model,data,geom_model,geom_data,qs_romeo[_smooth]);
for (std::vector<se3::CollisionPair>::iterator it = geom_model.collisionPairs.begin(); it != geom_model.collisionPairs.end(); ++it)
{
- geom_data.computeCollision(*it);
+ computeCollision(geom_model,geom_data,std::size_t(it-geom_model.collisionPairs.begin()));
}
}
double collideTime = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
@@ -124,7 +124,7 @@ int main()
timer.tic();
SMOOTH(NBT)
{
- computeCollisions(model,data,geom_model,geom_data,qs_romeo[_smooth], true);
+ computeCollisions(geom_model,geom_data, true);
}
double is_colliding_time = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
std::cout << "Collision Test : robot in collision? = \t" << is_colliding_time
@@ -203,7 +203,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
timer.tic();
SMOOTH(NBT)
{
- computeCollisions(geom_data, true);
+ computeCollisions(geom_model, geom_data, true);
}
double is_romeo_colliding_time_pino = timer.toc(StackTicToc::US)/NBT;
std::cout << "Pinocchio - Collision Test : robot in collision? (G) = \t" << is_romeo_colliding_time_pino
@@ -244,7 +244,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
timer.tic();
SMOOTH(NBD)
{
- computeDistances(geom_data);
+ computeDistances(geom_model, geom_data);
}
computeDistancesTime = timer.toc(StackTicToc::US)/NBD ;
std::cout << "Pinocchio - Compute distances (D) " << geom_model.collisionPairs.size() << " col pairs\t" << computeDistancesTime
diff --git a/bindings/python/algorithm/expose-geometry.cpp b/bindings/python/algorithm/expose-geometry.cpp
index dadf7c06c3b06f5778ed0f0eb6a2ac136034e6de..ff4fc77fad637a03a1566ef945938fbeea818609 100644
--- a/bindings/python/algorithm/expose-geometry.cpp
+++ b/bindings/python/algorithm/expose-geometry.cpp
@@ -36,11 +36,20 @@ namespace se3
return updateGeometryPlacements(*model, *data, *geom_model, *geom_data, q);
}
-#ifdef WITH_HPP_FCL
- static bool computeCollisions_proxy(GeometryDataHandler & data_geom,
+#ifdef WITH_HPP_FCL
+
+ static bool computeCollision_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const PairIndex & pairId)
+ {
+ return computeCollision(*model_geom, *data_geom, pairId);
+ }
+
+ static bool computeCollisions_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
const bool stopAtFirstCollision)
{
- return computeCollisions(*data_geom, stopAtFirstCollision);
+ return computeCollisions(*model_geom, *data_geom, stopAtFirstCollision);
}
static bool computeGeometryAndCollisions_proxy(const ModelHandler & model,
@@ -53,20 +62,29 @@ namespace se3
return computeCollisions(*model,*data,*model_geom, *data_geom, q, stopAtFirstCollision);
}
- static void computeDistances_proxy(GeometryDataHandler & data_geom)
+ static fcl::DistanceResult computeDistance_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const PairIndex & pairId)
+ {
+ return computeDistance(*model_geom, *data_geom, pairId);
+ }
+
+ static std::size_t computeDistances_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom)
{
- computeDistances(*data_geom);
+ return computeDistances(*model_geom, *data_geom);
}
- static void computeGeometryAndDistances_proxy(const ModelHandler & model,
- DataHandler & data,
- const GeometryModelHandler & model_geom,
- GeometryDataHandler & data_geom,
- const Eigen::VectorXd & q
- )
+ static std::size_t computeGeometryAndDistances_proxy(const ModelHandler & model,
+ DataHandler & data,
+ const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const Eigen::VectorXd & q
+ )
{
- computeDistances(*model, *data, *model_geom, *data_geom, q);
+ return computeDistances<true>(*model, *data, *model_geom, *data_geom, q);
}
+
#endif // WITH_HPP_FCL
void exposeGeometryAlgo()
@@ -78,6 +96,12 @@ namespace se3
);
#ifdef WITH_HPP_FCL
+ bp::def("computeCollision", computeCollision_proxy,
+ bp::args("GoometryModel", "GeometryData", "pairIndex"),
+ "Check if the collision objects of a collision pair for a given Geometry Model and Data are in collision."
+ "The collision pair is given by the two index of the collision objects."
+ );
+
bp::def("computeCollisions",computeCollisions_proxy,
bp::args("GeometryData","bool"),
"Determine if collision pairs are effectively in collision."
@@ -89,9 +113,14 @@ namespace se3
"determine if all collision pairs are effectively in collision or not."
);
+ bp::def("computeDistance",computeDistance_proxy,
+ bp::args("GeometryModel","GeometryData", "pairIndex"),
+ "Compute the distance between the two geometry objects of a given collision pair for a GeometryModel and associated GeometryData."
+ );
+
bp::def("computeDistances",computeDistances_proxy,
- bp::args("GeometryData"),
- "Compute the distance between each collision pair."
+ bp::args("GeometryModel","GeometryData"),
+ "Compute the distance between each collision pair for a given GeometryModel and associated GeometryData."
);
bp::def("computeGeometryAndDistances",computeGeometryAndDistances_proxy,
diff --git a/bindings/python/frame.hpp b/bindings/python/frame.hpp
index 4a1984655d6f62de1273da245ddd5c4512756d1c..0b785165bdc03da70e9816476fe3c5fedf6b7f5f 100644
--- a/bindings/python/frame.hpp
+++ b/bindings/python/frame.hpp
@@ -52,11 +52,12 @@ namespace se3
void visit(PyClass& cl) const
{
cl
- .def(bp::init< const std::string&,const JointIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("parent (index)"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
- "Initialize from name, parent id and placement wrt parent joint."))
+ .def(bp::init< const std::string&,const JointIndex, const FrameIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("index of parent joint"), bp::args("index of parent frame"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
+ "Initialize from name, parent joint id, parent frame id and placement wrt parent joint."))
.def_readwrite("name", &Frame::name, "name of the frame")
.def_readwrite("parent", &Frame::parent, "id of the parent joint")
+ .def_readwrite("previousFrame", &Frame::previousFrame, "id of the previous frame")
.add_property("placement",
&FramePythonVisitor::getPlacementWrtParentJoint,
&FramePythonVisitor::setPlacementWrtParentJoint,
diff --git a/bindings/python/geometry-data.hpp b/bindings/python/geometry-data.hpp
index e8b380f0d57c62ce643d4ce64cbbe18cd0f752bd..3db94b4947416313c646653a1a3a94b5d6db1299 100644
--- a/bindings/python/geometry-data.hpp
+++ b/bindings/python/geometry-data.hpp
@@ -116,23 +116,6 @@ namespace se3
bp::args("pairIndex (int)"),
"Deactivate pair ID <pairIndex> in geomModel.collisionPairs.")
- .def("computeCollision",&GeometryDataPythonVisitor::computeCollision,
- bp::args("co1 (index)","co2 (index)"),
- "Check if the two collision objects of a collision pair are in collision."
- "The collision pair is given by the two index of the collision objects.")
- .def("computeAllCollisions",&GeometryDataPythonVisitor::computeAllCollisions,
- "Same as computeCollision. It applies computeCollision to all collision pairs contained collision_pairs."
- "The results are stored in collision_results.")
- .def("isColliding",&GeometryDataPythonVisitor::isColliding,
- "Check if at least one of the collision pairs is in collision.")
-
- .def("computeDistance",&GeometryDataPythonVisitor::computeDistance,
- bp::args("co1 (index)","co2 (index)"),
- "Compute the distance result between two collision objects of a collision pair."
- "The collision pair is given by the two index of the collision objects.")
- .def("computeAllDistances",&GeometryDataPythonVisitor::computeAllDistances,
- "Same as computeDistance. It applies computeDistance to all collision pairs contained collision_pairs."
- "The results are stored in collision_distances.")
#endif // WITH_HPP_FCL
.def("__str__",&GeometryDataPythonVisitor::toString)
;
@@ -145,25 +128,15 @@ namespace se3
static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
#ifdef WITH_HPP_FCL
- static std::vector<DistanceResult> & distance_results( GeometryDataHandler & m ) { return m->distance_results; }
- static std::vector<CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collision_results; }
+ 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 CollisionResult computeCollision(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- return m->computeCollision(CollisionPair(co1, co2));
- }
- static bool isColliding(const GeometryDataHandler & m) { return m->isColliding(); }
- static void computeAllCollisions(GeometryDataHandler & m) { m->computeAllCollisions(); }
+
static void activateCollisionPair(GeometryDataHandler & m,
Index pairID) { m->activateCollisionPair(pairID); }
static void deactivateCollisionPair(GeometryDataHandler & m,
Index pairID) { m->deactivateCollisionPair(pairID); }
- static DistanceResult computeDistance(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- return m->computeDistance(CollisionPair(co1, co2));
- }
- static void computeAllDistances(GeometryDataHandler & m) { m->computeAllDistances(); }
#endif // WITH_HPP_FCL
static std::string toString(const GeometryDataHandler& m)
@@ -173,11 +146,11 @@ namespace se3
static void expose()
{
#ifdef WITH_HPP_FCL
- bp::class_< std::vector<DistanceResult> >("StdVec_DistanceResult")
- .def(bp::vector_indexing_suite< std::vector<DistanceResult> >());
+ bp::class_< std::vector<fcl::DistanceResult> >("StdVec_DistanceResult")
+ .def(bp::vector_indexing_suite< std::vector<fcl::DistanceResult> >());
- bp::class_< std::vector<CollisionResult> >("StdVec_CollisionResult")
- .def(bp::vector_indexing_suite< std::vector<CollisionResult> >());
+ bp::class_< std::vector<fcl::CollisionResult> >("StdVec_CollisionResult")
+ .def(bp::vector_indexing_suite< std::vector<fcl::CollisionResult> >());
#endif // WITH_HPP_FCL
bp::class_<GeometryDataHandler>("GeometryData",
"Geometry data linked to a geometry model and data struct.",
diff --git a/bindings/python/geometry-model.hpp b/bindings/python/geometry-model.hpp
index 1dcc106a5d79ded02491bef4c205ec229b3eb7c9..c2b2bfffe50aa250516f7e816fc841b804ec0e47 100644
--- a/bindings/python/geometry-model.hpp
+++ b/bindings/python/geometry-model.hpp
@@ -146,7 +146,6 @@ namespace se3
bp::enum_<GeometryType>("GeometryType")
.value("VISUAL",VISUAL)
.value("COLLISION",COLLISION)
- .value("NONE",NONE)
;
bp::class_<GeometryModelHandler>("GeometryModel",
diff --git a/bindings/python/geometry-object.hpp b/bindings/python/geometry-object.hpp
index cd9506845e11acf9da5b8c0b4c3f5fdeb4796a15..5117a77a4d10a2bb2b456a99b94ef1411e30800f 100644
--- a/bindings/python/geometry-object.hpp
+++ b/bindings/python/geometry-object.hpp
@@ -53,11 +53,12 @@ namespace se3
{
cl
.def_readwrite("name", &GeometryObject::name, "Name of the GeometryObject")
- .def_readwrite("parent", &GeometryObject::parent, "Index of the parent joint")
+ .def_readwrite("parentJoint", &GeometryObject::parentJoint, "Index of the parent joint")
+ .def_readwrite("parentFrame", &GeometryObject::parentFrame, "Index of the parent frame")
.add_property("placement", &GeometryObjectPythonVisitor::getPlacementWrtParentJoint,
&GeometryObjectPythonVisitor::setPlacementWrtParentJoint,
"Position of geometry object in parent joint's frame")
- .def_readonly("mesh_path", &GeometryObject::mesh_path, "Absolute path to the mesh file")
+ .def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
;
}
@@ -67,7 +68,7 @@ namespace se3
static void expose()
{
bp::class_<GeometryObject>("GeometryObject",
- "A wrapper on a collision geometry including its parent joint, placement in parent frame.\n\n",
+ "A wrapper on a collision geometry including its parent joint, parent frame, placement in parent joint's frame.\n\n",
bp::no_init
)
.def(GeometryObjectPythonVisitor())
diff --git a/bindings/python/model.hpp b/bindings/python/model.hpp
index 6e16e9686eca03af940726c8d487730b21d70cec..d8f09c450f7843739be2e58d1d13b8ade3e572c3 100644
--- a/bindings/python/model.hpp
+++ b/bindings/python/model.hpp
@@ -124,7 +124,7 @@ namespace se3
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","body_name"),"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("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")
@@ -133,9 +133,7 @@ namespace se3
.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")
- .def("getFrameParent", &ModelPythonVisitor::getFrameParent)
- .def("getFramePlacement", &ModelPythonVisitor::getFramePlacement)
- .def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, 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 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.")
@@ -183,7 +181,7 @@ namespace se3
const SE3_fx & body_placement,
const std::string & body_name)
{
- model->appendBodyToJoint(joint_parent_id,inertia,body_placement,body_name);
+ model->appendBodyToJoint(joint_parent_id,inertia,body_placement);
}
static Eigen::VectorXd neutralConfiguration(ModelHandler & m) {return m->neutralConfiguration;}
@@ -192,13 +190,10 @@ namespace se3
static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;}
static Eigen::VectorXd upperPositionLimit(ModelHandler & m) {return m->upperPositionLimit;}
- static Model::JointIndex getFrameParent( ModelHandler & m, const std::string & name ) { return m->getFrameParent(name); }
- static SE3 getFramePlacement(ModelHandler & m, const std::string & name) { return m->getFramePlacement(name); }
-
static bool addFrame(ModelHandler & m, const Frame & frame) { return m->addFrame(frame); }
- static bool addFrame( ModelHandler & m, const std::string & frameName, const JointIndex parent, const SE3_fx & placementWrtParent, const FrameType & type)
+ 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(frameName,parent,placementWrtParent,type);
+ return m->addFrame(Frame(frameName,parentJoint,parentFrame,placementWrtParent,type));
}
static Model::FrameIndex getFrameId(const ModelHandler & m, const std::string & frame_name)
{ return m->getFrameId(frame_name); }
diff --git a/python/bindings_frame.py b/python/bindings_frame.py
index 2643ced00d7e41279b25394c223e4323f9e52d7f..69fa1a417659ab2c140b1647ae9c061ea68f71f3 100644
--- a/python/bindings_frame.py
+++ b/python/bindings_frame.py
@@ -24,19 +24,19 @@ class TestFrameBindings(unittest.TestCase):
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
def test_type_get_set(self):
- f = self.robot.model.frames[2]
+ f = self.robot.model.frames[5]
self.assertTrue(f.type == se3.FrameType.JOINT)
f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self):
- f = self.robot.model.frames[2]
+ f = self.robot.model.frames[5]
self.assertTrue(f.name == 'LHipYaw')
f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self):
- f = self.robot.model.frames[2]
+ f = self.robot.model.frames[5]
self.assertTrue(f.parent == 2)
f.parent = 5
self.assertTrue(f.parent == 5)
diff --git a/python/bindings_geometry_object.py b/python/bindings_geometry_object.py
index c9868b6dd923225b08723d0354387aa0aef0a155..e64bc1783ec07fa2d3a9966f607277ae33e6e64e 100644
--- a/python/bindings_geometry_object.py
+++ b/python/bindings_geometry_object.py
@@ -32,9 +32,9 @@ class TestGeometryObjectBindings(unittest.TestCase):
def test_parent_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
- self.assertTrue(col.parent == 4)
- col.parent = 5
- self.assertTrue(col.parent == 5)
+ self.assertTrue(col.parentJoint == 4)
+ col.parentJoint = 5
+ self.assertTrue(col.parentJoint == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
@@ -47,7 +47,7 @@ class TestGeometryObjectBindings(unittest.TestCase):
def test_meshpath_get(self):
expected_mesh_path = os.path.join(self.pinocchio_models_dir,'meshes/romeo/collision/LHipPitch.dae')
col = self.robot.collision_model.geometryObjects[1]
- self.assertTrue(col.mesh_path == expected_mesh_path)
+ self.assertTrue(col.meshPath == expected_mesh_path)
if __name__ == '__main__':
unittest.main()
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index c186d0ab963b31e0acbeeef313bf5eaae2cecefd..3140567b81435d8d16389e2f9e64e128b7ae89bc 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -181,8 +181,6 @@ namespace se3
///
/// \brief Append a body to a given joint of the kinematic tree.
///
- /// \remark This method also adds a Frame of same name to the vector of frames.
- ///
/// \param[in] joint_index Index of the supporting joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index f2c2c11554fb5495670de296943ddd832c9e5e66..8b7fa5fa136b92fd54645a3a7cf6c2b503bf8fa4 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -23,6 +23,7 @@
#include "pinocchio/tools/string-generator.hpp"
#include <boost/bind.hpp>
+#include <boost/utility.hpp>
/// @cond DEV