From dcb309aefdc4a912c37af3c918e9fd3d044b0abc Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 17 Aug 2016 10:14:40 +0200
Subject: [PATCH] [C++][Bug] Fix geometry placement computation in URDF
---
src/parsers/urdf/geometry.cpp | 9 ++++++---
1 file changed, 6 insertions(+), 3 deletions(-)
diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp
index 0d6d316e3..8d5e6f250 100644
--- a/src/parsers/urdf/geometry.cpp
+++ b/src/parsers/urdf/geometry.cpp
@@ -198,6 +198,10 @@ namespace se3
}
std::vector< boost::shared_ptr< T > > geometries_array = getLinkGeometryArray<T>(link);
+ FrameIndex frame_id = model.getFrameId(link_name);
+ SE3 body_placement = model.frames[frame_id].placement;
+ assert(model.frames[frame_id].type == BODY);
+
std::size_t objectId = 0;
for (typename std::vector< boost::shared_ptr< T > >::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
{
@@ -211,12 +215,11 @@ namespace se3
const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
#endif // WITH_HPP_FCL
- SE3 geomPlacement = convertFromUrdf((*i)->origin);
+ SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
std::ostringstream geometry_object_suffix;
geometry_object_suffix << "_" << objectId;
const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
- assert(model.getFrameType(model.getFrameId(link_name)) == se3::BODY);
- geom_model.addGeometryObject(model, model.getFrameId(link_name), geometry, geomPlacement, geometry_object_name, mesh_path);
+ geom_model.addGeometryObject(model, frame_id, geometry, geomPlacement, geometry_object_name, mesh_path);
++objectId;
}
}
--
GitLab