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

[C++] Fixed compilation errors due to changes of API

parent 60498e46
......@@ -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
......
......@@ -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,
......
......@@ -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,
......
......@@ -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.",
......
......@@ -146,7 +146,6 @@ namespace se3
bp::enum_<GeometryType>("GeometryType")
.value("VISUAL",VISUAL)
.value("COLLISION",COLLISION)
.value("NONE",NONE)
;
bp::class_<GeometryModelHandler>("GeometryModel",
......
......@@ -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())
......
......@@ -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); }
......
......@@ -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)
......
......@@ -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()
......@@ -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.
......
......@@ -23,6 +23,7 @@
#include "pinocchio/tools/string-generator.hpp"
#include <boost/bind.hpp>
#include <boost/utility.hpp>
/// @cond DEV
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment