Skip to content

GitLab

Commit 9471bb34 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[Geometry] [Viewer] add scaling parameter to GeometryObject.

parent a614129c
Hide whitespace changes
Inline Side-by-side
bindings/python/multibody/geometry-object.hpp
View file @ 9471bb34
......@@ -36,13 +36,17 @@ namespace se3
void visit(PyClass& cl) const
{
cl
.def_readwrite("name", &GeometryObject::name, "Name of the GeometryObject")
.def_readwrite("parentJoint", &GeometryObject::parentJoint, "Index of the parent joint")
.def_readwrite("parentFrame", &GeometryObject::parentFrame, "Index of the parent frame")
.def_readwrite("placement",&GeometryObject::placement,
"Position of geometry object in parent joint's frame")
.def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
;
.add_property("meshScale",
bp::make_getter(&GeometryObject::meshScale, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&GeometryObject::meshScale),
"Scaling parameter for the mesh")
.def_readwrite("name", &GeometryObject::name, "Name of the GeometryObject")
.def_readwrite("parentJoint", &GeometryObject::parentJoint, "Index of the parent joint")
.def_readwrite("parentFrame", &GeometryObject::parentFrame, "Index of the parent frame")
.def_readwrite("placement",&GeometryObject::placement,
"Position of geometry object in parent joint's frame")
.def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
;
}
static void expose()
......
bindings/python/scripts/robot_wrapper.py
View file @ 9471bb34
......@@ -174,10 +174,13 @@ class RobotWrapper(object):
self.viewer.gui.createGroup(nodeName)
# iterate over visuals and create the meshes in the viewer
from rpy import npToTuple
for visual in self.visual_model.geometryObjects :
meshName = self.viewerNodeNames(visual)
meshPath = visual.meshPath
meshScale = visual.meshScale
self.viewer.gui.addMesh(meshName, meshPath)
self.viewer.gui.setScale(meshName, npToTuple(meshScale))
# Finally, refresh the layout to obtain your first rendering.
self.viewer.gui.refresh()
......
src/multibody/fcl.hpp
View file @ 9471bb34
......@@ -101,7 +101,7 @@ struct GeometryObject
/// \brief Index of the parent joint
JointIndex parentJoint;
/// \brief The actual cloud of points representing the collision mesh of the object
/// \brief The actual cloud of points representing the collision mesh of the object after scaling.
boost::shared_ptr<fcl::CollisionGeometry> fcl;
/// \brief Position of geometry object in parent joint frame
......@@ -110,15 +110,19 @@ struct GeometryObject
/// \brief Absolute path to the mesh file
std::string meshPath;
/// \brief Scaling vector applied to the fcl object.
Eigen::Vector3d meshScale;
GeometryObject(const std::string & name, const FrameIndex parentF,
const JointIndex parentJ, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
const SE3 & placement, const std::string & meshPath)
const SE3 & placement, const std::string & meshPath, const Eigen::Vector3d & meshScale)
: name(name)
, parentFrame(parentF)
, parentJoint(parentJ)
, fcl(collision)
, placement(placement)
, meshPath(meshPath)
, meshScale(meshScale)
{}
GeometryObject & operator=(const GeometryObject & other)
......@@ -129,6 +133,7 @@ struct GeometryObject
fcl = other.fcl;
placement = other.placement;
meshPath = other.meshPath;
meshScale = other.meshScale;
return *this;
}
......
src/multibody/fcl.hxx
View file @ 9471bb34
......@@ -72,6 +72,7 @@ namespace se3
&& lhs.fcl == rhs.fcl
&& lhs.placement == rhs.placement
&& lhs.meshPath == rhs.meshPath
&& lhs.meshScale == rhs.meshScale
);
}
......@@ -82,6 +83,7 @@ namespace se3
<< "Parent joint ID: \t \n" << geom_object.parentJoint << "\n"
<< "Position in parent frame: \t \n" << geom_object.placement << "\n"
<< "Absolute path to mesh file: \t \n" << geom_object.meshPath << "\n"
<< "Scale for transformation of the mesh: \t \n" << geom_object.meshScale.transpose() << "\n"
<< std::endl;
return os;
}
......
src/parsers/urdf/geometry.cpp
View file @ 9471bb34
......@@ -49,12 +49,14 @@ namespace se3
* @param[in] urdf_geometry A shared pointer on the input urdf Geometry
* @param[in] package_dirs A vector containing the different directories where to search for packages
* @param[out] meshPath The Absolute path of the mesh currently read
* @param[out] meshScale Scale of transformation currently applied to the mesh
*
* @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr< ::urdf::Geometry> urdf_geometry,
const std::vector<std::string> & package_dirs,
std::string & meshPath)
std::string & meshPath,
Eigen::Vector3d & meshScale)
{
boost::shared_ptr<fcl::CollisionGeometry> geometry;
......@@ -67,9 +69,13 @@ namespace se3
meshPath = retrieveResourcePath(collisionFilename, package_dirs);
fcl::Vec3f scale = fcl::Vec3f(collisionGeometry->scale.x,
collisionGeometry->scale.y,
collisionGeometry->scale.z
);
collisionGeometry->scale.y,
collisionGeometry->scale.z
);
meshScale << collisionGeometry->scale.x,
collisionGeometry->scale.y,
collisionGeometry->scale.z;
// Create FCL mesh by parsing Collada file.
PolyhedronPtrType polyhedron (new PolyhedronType);
......@@ -83,6 +89,7 @@ namespace se3
else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
{
meshPath = "CYLINDER";
meshScale << 1,1,1;
boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
double radius = collisionGeometry->radius;
......@@ -95,6 +102,7 @@ namespace se3
else if (urdf_geometry->type == ::urdf::Geometry::BOX)
{
meshPath = "BOX";
meshScale << 1,1,1;
boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
double x = collisionGeometry->dim.x;
......@@ -107,6 +115,7 @@ namespace se3
else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
{
meshPath = "SPHERE";
meshScale << 1,1,1;
boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
double radius = collisionGeometry->radius;
......@@ -187,6 +196,8 @@ namespace se3
if(getLinkGeometry<T>(link))
{
std::string meshPath = "";
Eigen::Vector3d meshScale;
std::string link_name = link->name;
......@@ -203,7 +214,7 @@ namespace se3
{
meshPath.clear();
#ifdef WITH_HPP_FCL
const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, meshPath);
const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, meshPath, meshScale);
#else
boost::shared_ptr < ::urdf::Mesh> urdf_mesh = boost::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
if (urdf_mesh) meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
......@@ -218,7 +229,7 @@ namespace se3
geomModel.addGeometryObject(GeometryObject(geometry_object_name,
frame_id, model.frames[frame_id].parent,
geometry,
geomPlacement, meshPath),
geomPlacement, meshPath, meshScale),
model);
++objectId;
}
......
unittest/geom.cpp
View file @ 9471bb34
......@@ -159,13 +159,13 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
boost::shared_ptr<fcl::Box> sample(new fcl::Box(1));
geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
model.getBodyId("planar1_body"),0,
sample,SE3::Identity(), ""),
sample,SE3::Identity(), "", Eigen::Vector3d::Ones()),
model,true);
boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1));
geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
model.getBodyId("planar2_body"),0,
sample2,SE3::Identity(), ""),
sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()),
model,true);
geomModel.addAllCollisionPairs();
......
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