Unverified Commit fb4563cf authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #121 from jmirabel/devel

Add CollisionObject::geometry, replaceGeometryByConvexHull and prepare deprecation of collisionObjects
parents 85a21189 30397793
...@@ -62,6 +62,9 @@ namespace hpp { ...@@ -62,6 +62,9 @@ namespace hpp {
/// Access to pinocchio object /// Access to pinocchio object
const GeometryObject & pinocchio () const; const GeometryObject & pinocchio () const;
GeometryObject & pinocchio (); GeometryObject & pinocchio ();
/// Access to fcl object
CollisionGeometryPtr_t geometry () const;
/// Access to fcl object /// Access to fcl object
FclConstCollisionObjectPtr_t fcl (const GeomData& data) const; FclConstCollisionObjectPtr_t fcl (const GeomData& data) const;
...@@ -90,7 +93,7 @@ namespace hpp { ...@@ -90,7 +93,7 @@ namespace hpp {
/// manually moved, this will return the non-update transform. /// manually moved, this will return the non-update transform.
/// \note If object is not attached to a joint, use move() to update /// \note If object is not attached to a joint, use move() to update
/// transform between hpp and fcl. /// transform between hpp and fcl.
const fcl::Transform3f& getFclTransform () const; fcl::Transform3f getFclTransform () const;
const Transform3f& getTransform () const; const Transform3f& getTransform () const;
const Transform3f& getTransform (DeviceData& d) const; const Transform3f& getTransform (DeviceData& d) const;
......
...@@ -341,6 +341,9 @@ namespace hpp { ...@@ -341,6 +341,9 @@ namespace hpp {
Pool<DeviceData> datas_; Pool<DeviceData> datas_;
}; // class Device }; // class Device
void replaceGeometryByConvexHull (GeomModel& geomModel,
const std::vector<std::string>& geometryNames);
inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device) inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device)
{ return device.print(os); } { return device.print(os); }
......
...@@ -96,6 +96,8 @@ namespace hpp { ...@@ -96,6 +96,8 @@ namespace hpp {
typedef boost::shared_ptr<Body> BodyPtr_t; typedef boost::shared_ptr<Body> BodyPtr_t;
typedef boost::shared_ptr<const Body> BodyConstPtr_t; typedef boost::shared_ptr<const Body> BodyConstPtr_t;
//typedef std::vector<BodyPtr_t> BodyVector_t; //typedef std::vector<BodyPtr_t> BodyVector_t;
typedef fcl::CollisionGeometry CollisionGeometry;
typedef boost::shared_ptr<CollisionGeometry> CollisionGeometryPtr_t;
typedef fcl::CollisionObject FclCollisionObject; typedef fcl::CollisionObject FclCollisionObject;
typedef fcl::CollisionObject * FclCollisionObjectPtr_t; typedef fcl::CollisionObject * FclCollisionObjectPtr_t;
typedef const fcl::CollisionObject * FclConstCollisionObjectPtr_t; typedef const fcl::CollisionObject * FclConstCollisionObjectPtr_t;
......
...@@ -21,6 +21,7 @@ ...@@ -21,6 +21,7 @@
#include <pinocchio/multibody/model.hpp> #include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/geometry.hpp> #include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
#include <hpp/pinocchio/device.hh> #include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh> #include <hpp/pinocchio/joint.hh>
...@@ -63,6 +64,9 @@ namespace hpp { ...@@ -63,6 +64,9 @@ namespace hpp {
::pinocchio::GeometryObject & CollisionObject::pinocchio () ::pinocchio::GeometryObject & CollisionObject::pinocchio ()
{ return geomModel_->geometryObjects[geomInModelIndex]; } { return geomModel_->geometryObjects[geomInModelIndex]; }
CollisionGeometryPtr_t CollisionObject::geometry () const
{ return pinocchio().geometry; }
FclCollisionObjectPtr_t CollisionObject::fcl (GeomData& geomData) const FclCollisionObjectPtr_t CollisionObject::fcl (GeomData& geomData) const
{ {
return & geomData.collisionObjects[geomInModelIndex]; return & geomData.collisionObjects[geomInModelIndex];
...@@ -102,8 +106,8 @@ namespace hpp { ...@@ -102,8 +106,8 @@ namespace hpp {
const Transform3f& CollisionObject:: const Transform3f& CollisionObject::
positionInJointFrame () const { return pinocchio().placement; } positionInJointFrame () const { return pinocchio().placement; }
const fcl::Transform3f& CollisionObject::getFclTransform () const fcl::Transform3f CollisionObject::getFclTransform () const
{ return geomData().collisionObjects[geomInModelIndex].getTransform(); } { return ::pinocchio::toFclTransform3f(geomData().oMg[geomInModelIndex]); }
const Transform3f& CollisionObject::getTransform () const const Transform3f& CollisionObject::getTransform () const
{ return geomData().oMg[geomInModelIndex]; } { return geomData().oMg[geomInModelIndex]; }
const Transform3f& CollisionObject::getTransform (DeviceData& d) const const Transform3f& CollisionObject::getTransform (DeviceData& d) const
......
...@@ -23,6 +23,7 @@ ...@@ -23,6 +23,7 @@
#include <Eigen/Core> #include <Eigen/Core>
#include <hpp/fcl/BV/AABB.h> #include <hpp/fcl/BV/AABB.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <pinocchio/algorithm/geometry.hpp> #include <pinocchio/algorithm/geometry.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch #include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
...@@ -503,5 +504,24 @@ namespace hpp { ...@@ -503,5 +504,24 @@ namespace hpp {
} }
return aabb; return aabb;
} }
void replaceGeometryByConvexHull (GeomModel& gmodel,
const std::vector<std::string>& gnames)
{
for (std::size_t i = 0; i < gnames.size(); ++i) {
if (!gmodel.existGeometryName(gnames[i]))
throw std::invalid_argument("Geometry " + gnames[i] + " does not "
"exist.");
GeomIndex gid = gmodel.getGeometryId(gnames[i]);
GeometryObject& go = gmodel.geometryObjects[gid];
if (go.geometry->getObjectType() == fcl::OT_BVH) {
boost::shared_ptr<fcl::BVHModelBase> bvh =
HPP_DYNAMIC_PTR_CAST(fcl::BVHModelBase, go.geometry);
assert(bvh);
bvh->buildConvexHull(false, "Qx");
go.geometry = bvh->convex;
}
}
}
} // namespace pinocchio } // namespace pinocchio
} // namespace hpp } // namespace hpp
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