Skip to content

Commit ec9ef9d

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Fix use of compound mesh in FCL
1 parent 82820da commit ec9ef9d

File tree

6 files changed

+106
-29
lines changed

6 files changed

+106
-29
lines changed

‎tesseract_collision/bullet/src/bullet_utils.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -472,10 +472,11 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
472472
{
473473
if (m_shapes[j]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH)
474474
{
475+
int shape_index = shape_id++;
475476
const auto& meshes = std::static_pointer_cast<const tesseract_geometry::CompoundMesh>(m_shapes[j])->getMeshes();
476477
for (const auto& mesh : meshes)
477478
{
478-
std::shared_ptr<btCollisionShape> subshape = createShapePrimitive(mesh, this, shape_id++);
479+
std::shared_ptr<btCollisionShape> subshape = createShapePrimitive(mesh, this, shape_index);
479480
if (subshape != nullptr)
480481
{
481482
manage(subshape);

‎tesseract_collision/bullet/src/create_convex_hull.cpp

Lines changed: 70 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -51,17 +51,76 @@ int main(int argc, char** argv)
5151

5252
namespace po = boost::program_options;
5353
po::options_description desc("Options");
54-
desc.add_options()("help,h", "Print help messages")(
55-
"input,i", po::value<std::string>(&input)->required(), "File path to mesh used to create a convex hull.")(
56-
"output,o", po::value<std::string>(&output)->required(), "File path to save the generated convex hull as a ply.")(
57-
"shrink,s",
58-
po::value<double>(&shrink),
59-
"If positive, the convex hull is shrunken by that amount (each face is moved by 'shrink' length units towards "
60-
"the center along its normal).")("clamp,c",
61-
po::value<double>(&clamp),
62-
"If positive, 'shrink' is clamped to not exceed 'clamp * innerRadius', where "
63-
"'innerRadius' is the minimum distance of a face to the center of the convex "
64-
"hull.");
54+
desc.add_options()(
55+
"help,h", "Print help messages")("input,i",
56+
po::value<std::string>(&input)->required(),
57+
"File path to mesh used to create a convex hull.")("output,o",
58+
po::value<std::string>(
59+
&output)
60+
->required(),
61+
"File path to save the "
62+
"generated convex hull as a "
63+
"ply.")("shrink,s",
64+
po::value<double>(
65+
&shrink),
66+
"If positive, the "
67+
"convex hull is "
68+
"shrunken by that "
69+
"amount (each face "
70+
"is moved by "
71+
"'shrink' length "
72+
"units towards "
73+
"the center along "
74+
"its normal).")("clam"
75+
"p,c",
76+
po::value<
77+
double>(
78+
&clamp),
79+
"If "
80+
"posi"
81+
"tive"
82+
", "
83+
"'shr"
84+
"ink'"
85+
" is "
86+
"clam"
87+
"ped "
88+
"to "
89+
"not "
90+
"exce"
91+
"ed "
92+
"'cla"
93+
"mp "
94+
"* "
95+
"inne"
96+
"rRad"
97+
"ius'"
98+
", "
99+
"wher"
100+
"e "
101+
"'inn"
102+
"erRa"
103+
"dius"
104+
"' "
105+
"is "
106+
"the "
107+
"mini"
108+
"mum "
109+
"dist"
110+
"ance"
111+
" of "
112+
"a "
113+
"face"
114+
" to "
115+
"the "
116+
"cent"
117+
"er "
118+
"of "
119+
"the "
120+
"conv"
121+
"ex "
122+
"hull"
123+
".");
65124

66125
po::variables_map vm;
67126
try

‎tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,8 +64,23 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject<double>
6464
*/
6565
void updateAABB();
6666

67+
/**
68+
* @brief Set the shape index. This is the geometries index in the urdf.
69+
* @param index The index
70+
*/
71+
void setShapeIndex(int index);
72+
73+
/**
74+
* @brief Get the shape index. This is the geometries index in the urdf.
75+
* @return The shape index
76+
*/
77+
int getShapeIndex() const;
78+
6779
protected:
6880
double contact_distance_{ 0 }; /**< @brief The contact distance threshold. */
81+
82+
/** @brief The shape index, which is the geometries index in the urdf. */
83+
int shape_index_{ -1 };
6984
};
7085

7186
} // namespace tesseract_collision::tesseract_collision_fcl

‎tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -113,10 +113,9 @@ class CollisionObjectWrapper
113113
void setCollisionObjectsTransform(const Eigen::Isometry3d& pose)
114114
{
115115
world_pose_ = pose;
116-
for (unsigned i = 0; i < collision_objects_.size(); ++i)
116+
for (auto& co : collision_objects_)
117117
{
118-
CollisionObjectPtr& co = collision_objects_[i];
119-
co->setTransform(pose * shape_poses_[i]);
118+
co->setTransform(pose * shape_poses_[static_cast<std::size_t>(co->getShapeIndex())]);
120119
co->updateAABB(); // This a tesseract function that updates abb to take into account contact distance
121120
}
122121
}
@@ -168,7 +167,7 @@ class CollisionObjectWrapper
168167
* @param co fcl collision shape
169168
* @return links collision shape index
170169
*/
171-
int getShapeIndex(const fcl::CollisionObjectd* co) const;
170+
static int getShapeIndex(const fcl::CollisionObjectd* co);
172171

173172
protected:
174173
std::string name_; // name of the collision object

‎tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,4 +54,12 @@ void FCLCollisionObjectWrapper::updateAABB()
5454
}
5555
}
5656

57+
void FCLCollisionObjectWrapper::setShapeIndex(int index) { shape_index_ = index; }
58+
59+
int FCLCollisionObjectWrapper::getShapeIndex() const
60+
{
61+
assert(shape_index_ >= 0);
62+
return shape_index_;
63+
}
64+
5765
} // namespace tesseract_collision::tesseract_collision_fcl

‎tesseract_collision/fcl/src/fcl_utils.cpp

Lines changed: 8 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -246,8 +246,8 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
246246
ContactResult contact;
247247
contact.link_names[0] = cd1->getName();
248248
contact.link_names[1] = cd2->getName();
249-
contact.shape_id[0] = static_cast<int>(cd1->getShapeIndex(o1));
250-
contact.shape_id[1] = static_cast<int>(cd2->getShapeIndex(o2));
249+
contact.shape_id[0] = CollisionObjectWrapper::getShapeIndex(o1);
250+
contact.shape_id[1] = CollisionObjectWrapper::getShapeIndex(o2);
251251
contact.subshape_id[0] = static_cast<int>(fcl_contact.b1);
252252
contact.subshape_id[1] = static_cast<int>(fcl_contact.b2);
253253
contact.nearest_points[0] = fcl_contact.pos;
@@ -307,8 +307,8 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
307307
ContactResult contact;
308308
contact.link_names[0] = cd1->getName();
309309
contact.link_names[1] = cd2->getName();
310-
contact.shape_id[0] = cd1->getShapeIndex(o1);
311-
contact.shape_id[1] = cd2->getShapeIndex(o2);
310+
contact.shape_id[0] = CollisionObjectWrapper::getShapeIndex(o1);
311+
contact.shape_id[1] = CollisionObjectWrapper::getShapeIndex(o2);
312312
contact.subshape_id[0] = static_cast<int>(fcl_result.b1);
313313
contact.subshape_id[1] = static_cast<int>(fcl_result.b2);
314314
contact.nearest_points[0] = fcl_result.nearest_points[0];
@@ -365,6 +365,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
365365
collision_geometries_.push_back(subshape);
366366
auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
367367
co->setUserData(this);
368+
co->setShapeIndex(static_cast<int>(i));
368369
co->setTransform(shape_poses_[i]);
369370
co->updateAABB();
370371
collision_objects_.push_back(co);
@@ -380,6 +381,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
380381
collision_geometries_.push_back(subshape);
381382
auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
382383
co->setUserData(this);
384+
co->setShapeIndex(static_cast<int>(i));
383385
co->setTransform(shape_poses_[i]);
384386
co->updateAABB();
385387
collision_objects_.push_back(co);
@@ -389,16 +391,9 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
389391
}
390392
}
391393

392-
int CollisionObjectWrapper::getShapeIndex(const fcl::CollisionObjectd* co) const
394+
int CollisionObjectWrapper::getShapeIndex(const fcl::CollisionObjectd* co)
393395
{
394-
auto it = std::find_if(collision_objects_.begin(), collision_objects_.end(), [&co](const CollisionObjectPtr& c) {
395-
return c.get() == co;
396-
});
397-
398-
if (it != collision_objects_.end())
399-
return static_cast<int>(std::distance(collision_objects_.begin(), it));
400-
401-
return -1;
396+
return static_cast<const FCLCollisionObjectWrapper*>(co)->getShapeIndex();
402397
}
403398

404399
} // namespace tesseract_collision::tesseract_collision_fcl

0 commit comments

Comments
 (0)