collision_env_bullet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Jens Petit
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Jens Petit */
36 
41 #include <functional>
42 #include <bullet/btBulletCollisionCommon.h>
43 
44 namespace collision_detection
45 {
46 namespace
47 {
48 static const std::string NAME = "Bullet";
49 const double MAX_DISTANCE_MARGIN = 99;
50 constexpr char LOGNAME[] = "collision_detection.bullet";
51 } // namespace
52 
53 CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
54  : CollisionEnv(model, padding, scale)
55 {
56  // request notifications about changes to new world
57  observer_handle_ = getWorld()->addObserver(
58  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
59 
60  for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : robot_model_->getURDF()->links_)
61  {
62  addLinkAsCollisionObject(link.second);
63  }
64 }
65 
66 CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world,
67  double padding, double scale)
68  : CollisionEnv(model, world, padding, scale)
69 {
70  // request notifications about changes to new world
71  observer_handle_ = getWorld()->addObserver(
72  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
73 
74  for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : robot_model_->getURDF()->links_)
75  {
76  addLinkAsCollisionObject(link.second);
77  }
78 
79  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
80 }
81 
82 CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world)
83  : CollisionEnv(other, world)
84 {
85  // request notifications about changes to new world
86  observer_handle_ = getWorld()->addObserver(
87  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
88 
89  for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : other.robot_model_->getURDF()->links_)
90  {
91  addLinkAsCollisionObject(link.second);
92  }
93 
94  // get notifications any objects already in the new world
95  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
96 }
97 
99 {
100  getWorld()->removeObserver(observer_handle_);
101 }
102 
104  const moveit::core::RobotState& state) const
105 {
106  checkSelfCollisionHelper(req, res, state, nullptr);
107 }
108 
109 void CollisionEnvBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
110  const moveit::core::RobotState& state,
111  const AllowedCollisionMatrix& acm) const
112 {
113  checkSelfCollisionHelper(req, res, state, &acm);
114 }
115 
117  const moveit::core::RobotState& state,
118  const AllowedCollisionMatrix* acm) const
119 {
120  std::lock_guard<std::mutex> guard(collision_env_mutex_);
121 
122  std::vector<collision_detection_bullet::CollisionObjectWrapperPtr> cows;
123  addAttachedOjects(state, cows);
124 
125  if (req.distance)
126  {
127  manager_->setContactDistanceThreshold(MAX_DISTANCE_MARGIN);
128  }
129 
130  for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : cows)
131  {
132  manager_->addCollisionObject(cow);
133  manager_->setCollisionObjectsTransform(
134  cow->getName(), state.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]);
135  }
136 
137  // updating link positions with the current robot state
138  for (const std::string& link : active_)
139  {
140  manager_->setCollisionObjectsTransform(link, state.getCollisionBodyTransform(link, 0));
141  }
142 
143  manager_->contactTest(res, req, acm, true);
144 
145  for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : cows)
146  {
147  manager_->removeCollisionObject(cow->getName());
148  }
149 }
150 
151 void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
152  const moveit::core::RobotState& state) const
153 {
154  checkRobotCollisionHelper(req, res, state, nullptr);
155 }
156 
157 void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
158  const moveit::core::RobotState& state,
159  const AllowedCollisionMatrix& acm) const
160 {
161  checkRobotCollisionHelper(req, res, state, &acm);
162 }
163 
164 void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
165  const moveit::core::RobotState& state1,
166  const moveit::core::RobotState& state2) const
167 {
168  checkRobotCollisionHelperCCD(req, res, state1, state2, nullptr);
169 }
170 
171 void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
172  const moveit::core::RobotState& state1,
173  const moveit::core::RobotState& state2,
174  const AllowedCollisionMatrix& acm) const
175 {
176  checkRobotCollisionHelperCCD(req, res, state1, state2, &acm);
177 }
178 
179 void CollisionEnvBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res,
180  const moveit::core::RobotState& state,
181  const AllowedCollisionMatrix* acm) const
182 {
183  std::lock_guard<std::mutex> guard(collision_env_mutex_);
184 
185  if (req.distance)
186  {
187  manager_->setContactDistanceThreshold(MAX_DISTANCE_MARGIN);
188  }
189 
190  std::vector<collision_detection_bullet::CollisionObjectWrapperPtr> attached_cows;
191  addAttachedOjects(state, attached_cows);
193 
194  for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows)
195  {
196  manager_->addCollisionObject(cow);
197  manager_->setCollisionObjectsTransform(
198  cow->getName(), state.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]);
199  }
200 
201  manager_->contactTest(res, req, acm, false);
202 
203  for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows)
204  {
205  manager_->removeCollisionObject(cow->getName());
206  }
207 }
208 
209 void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res,
210  const moveit::core::RobotState& state1,
212  const AllowedCollisionMatrix* acm) const
213 {
214  std::lock_guard<std::mutex> guard(collision_env_mutex_);
215 
216  std::vector<collision_detection_bullet::CollisionObjectWrapperPtr> attached_cows;
217  addAttachedOjects(state1, attached_cows);
218 
219  for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows)
220  {
221  manager_CCD_->addCollisionObject(cow);
222  manager_CCD_->setCastCollisionObjectsTransform(
223  cow->getName(), state1.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0],
224  state2.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]);
225  }
226 
227  for (const std::string& link : active_)
228  {
229  manager_CCD_->setCastCollisionObjectsTransform(link, state1.getCollisionBodyTransform(link, 0),
230  state2.getCollisionBodyTransform(link, 0));
231  }
232 
233  manager_CCD_->contactTest(res, req, acm, false);
234 
235  for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows)
236  {
237  manager_CCD_->removeCollisionObject(cow->getName());
238  }
239 }
240 
242  const moveit::core::RobotState& /*state*/) const
243 {
244  ROS_INFO_NAMED(LOGNAME, "distanceSelf is not implemented for Bullet.");
245 }
246 
248  const moveit::core::RobotState& /*state*/) const
249 {
250  ROS_INFO_NAMED(LOGNAME, "distanceRobot is not implemented for Bullet.");
251 }
252 
253 void CollisionEnvBullet::addToManager(const World::Object* obj)
254 {
255  std::vector<collision_detection_bullet::CollisionObjectType> collision_object_types;
256 
257  for (const shapes::ShapeConstPtr& shape : obj->shapes_)
258  {
259  if (shape->type == shapes::MESH)
260  collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL);
261  else
262  collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE);
263  }
264 
265  auto cow = std::make_shared<collision_detection_bullet::CollisionObjectWrapper>(
266  obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, obj->global_shape_poses_,
267  collision_object_types, false);
268 
269  manager_->addCollisionObject(cow);
270  manager_CCD_->addCollisionObject(cow->clone());
271 }
272 
273 void CollisionEnvBullet::updateManagedObject(const std::string& id)
274 {
275  if (getWorld()->hasObject(id))
276  {
277  auto it = getWorld()->find(id);
278  if (manager_->hasCollisionObject(id))
279  {
280  manager_->removeCollisionObject(id);
281  manager_CCD_->removeCollisionObject(id);
282  addToManager(it->second.get());
283  }
284  else
285  {
286  addToManager(it->second.get());
287  }
288  }
289  else
290  {
291  if (manager_->hasCollisionObject(id))
292  {
293  manager_->removeCollisionObject(id);
294  manager_CCD_->removeCollisionObject(id);
295  }
296  }
297 }
298 
299 void CollisionEnvBullet::setWorld(const WorldPtr& world)
300 {
301  if (world == getWorld())
302  return;
303 
304  // turn off notifications about old world
305  getWorld()->removeObserver(observer_handle_);
306 
307  CollisionEnv::setWorld(world);
308 
309  // request notifications about changes to new world
310  observer_handle_ = getWorld()->addObserver(
311  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
312 
313  // get notifications any objects already in the new world
314  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
315 }
316 
317 void CollisionEnvBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Action action)
318 {
319  std::lock_guard<std::mutex> guard(collision_env_mutex_);
320  if (action == World::DESTROY)
321  {
322  manager_->removeCollisionObject(obj->id_);
323  manager_CCD_->removeCollisionObject(obj->id_);
324  }
325  else
326  {
327  updateManagedObject(obj->id_);
328  }
329 }
330 
332  std::vector<collision_detection_bullet::CollisionObjectWrapperPtr>& cows) const
333 {
334  std::vector<const moveit::core::AttachedBody*> attached_bodies;
335  state.getAttachedBodies(attached_bodies);
336 
337  for (const moveit::core::AttachedBody*& body : attached_bodies)
338  {
339  const EigenSTL::vector_Isometry3d& attached_body_transform = body->getGlobalCollisionBodyTransforms();
340 
341  std::vector<collision_detection_bullet::CollisionObjectType> collision_object_types(
343 
344  try
345  {
346  collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper(
347  body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform,
348  collision_object_types, body->getTouchLinks()));
349  cows.push_back(cow);
350  }
351  catch (std::exception&)
352  {
353  ROS_ERROR_STREAM_NAMED("collision_detetction.bullet",
354  "Not adding " << body->getName() << " due to bad arguments.");
355  }
356  }
357 }
358 
359 void CollisionEnvBullet::updatedPaddingOrScaling(const std::vector<std::string>& links)
360 {
361  for (const std::string& link : links)
362  {
363  if (robot_model_->getURDF()->links_.find(link) != robot_model_->getURDF()->links_.end())
364  {
365  addLinkAsCollisionObject(robot_model_->getURDF()->links_[link]);
366  }
367  else
368  {
369  ROS_ERROR_NAMED("collision_detection.bullet", "Updating padding or scaling for unknown link: '%s'", link.c_str());
370  }
371  }
372 }
373 
375  const moveit::core::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const
376 {
377  // updating link positions with the current robot state
378  for (const std::string& link : active_)
379  {
380  // select the first of the transformations for each link (composed of multiple shapes...)
381  manager->setCollisionObjectsTransform(link, state.getCollisionBodyTransform(link, 0));
382  }
383 }
384 
385 void CollisionEnvBullet::addLinkAsCollisionObject(const urdf::LinkSharedPtr& link)
386 {
387  if (!link->collision_array.empty())
388  {
389  const std::vector<urdf::CollisionSharedPtr>& col_array =
390  link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, link->collision) :
391  link->collision_array;
392 
393  std::vector<shapes::ShapeConstPtr> shapes;
395  std::vector<collision_detection_bullet::CollisionObjectType> collision_object_types;
396 
397  for (const auto& i : col_array)
398  {
399  if (i && i->geometry)
400  {
402 
403  if (shape)
404  {
405  if (fabs(getLinkScale(link->name) - 1.0) >= std::numeric_limits<double>::epsilon() ||
406  fabs(getLinkPadding(link->name)) >= std::numeric_limits<double>::epsilon())
407  {
408  shape->scaleAndPadd(getLinkScale(link->name), getLinkPadding(link->name));
409  }
410 
411  shapes.push_back(shape);
412  shape_poses.push_back(collision_detection_bullet::urdfPose2Eigen(i->origin));
413 
414  if (shape->type == shapes::MESH)
415  {
416  collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL);
417  }
418  else
419  {
420  collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE);
421  }
422  }
423  }
424  }
425 
426  if (manager_->hasCollisionObject(link->name))
427  {
428  manager_->removeCollisionObject(link->name);
429  manager_CCD_->removeCollisionObject(link->name);
430  }
431 
432  try
433  {
434  collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper(
435  link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true));
436  manager_->addCollisionObject(cow);
437  manager_CCD_->addCollisionObject(cow->clone());
438  active_.push_back(cow->getName());
439  }
440  catch (std::exception&)
441  {
442  ROS_ERROR_STREAM_NAMED("collision_detetction.bullet", "Not adding " << link->name << " due to bad arguments.");
443  }
444  }
445 }
446 
447 const std::string& CollisionDetectorAllocatorBullet::getName() const
448 {
449  return NAME;
450 }
451 
452 } // namespace collision_detection
collision_detection::CollisionEnvBullet::updateTransformsFromState
void updateTransformsFromState(const moveit::core::RobotState &state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr &manager) const
Updates the poses of the objects in the manager according to given robot state.
Definition: collision_env_bullet.cpp:406
collision_detection::CollisionEnvBullet::checkRobotCollisionHelperCCD
void checkRobotCollisionHelperCCD(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const AllowedCollisionMatrix *acm) const
Definition: collision_env_bullet.cpp:241
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:259
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
collision_detection::CollisionEnvBullet::CollisionEnvBullet
CollisionEnvBullet()=delete
collision_detection::BodyTypes::WORLD_OBJECT
@ WORLD_OBJECT
A body in the environment.
Definition: include/moveit/collision_detection/collision_common.h:97
collision_detection_bullet::constructShape
shapes::ShapePtr constructShape(const urdf::Geometry *geom)
Definition: ros_bullet_utils.h:78
collision_detection::CollisionEnvBullet::setWorld
void setWorld(const WorldPtr &world) override
Definition: collision_env_bullet.cpp:331
collision_detection::CollisionEnvBullet::checkSelfCollision
void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check for robot self collision. Any collision between any pair of links is checked for,...
Definition: collision_env_bullet.cpp:135
ros_bullet_utils.h
collision_detection::CollisionEnvBullet::distanceSelf
void distanceSelf(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
The distance to self-collision given the robot is at state state.
Definition: collision_env_bullet.cpp:273
moveit::core::RobotState::getAttachedBody
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
Definition: robot_state.cpp:1075
collision_detection::World::Action
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:268
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
contact_checker_common.h
collision_detection::LOGNAME
constexpr char LOGNAME[]
Definition: collision_env_fcl.cpp:82
collision_detection::CollisionEnvBullet::updateManagedObject
void updateManagedObject(const std::string &id)
Updates a managed collision object with its world representation.
Definition: collision_env_bullet.cpp:305
obj
CollisionObject< S > * obj
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:179
shapes::MESH
MESH
collision_detection::CollisionRequest::distance
bool distance
If true, compute proximity distance.
Definition: include/moveit/collision_detection/collision_common.h:200
collision_detection::CollisionEnvBullet::distanceRobot
void distanceRobot(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
Compute the distance between a robot and the world.
Definition: collision_env_bullet.cpp:279
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
collision_detection::CollisionEnvBullet::collision_env_mutex_
std::mutex collision_env_mutex_
Definition: collision_env_bullet.h:191
collision_detection::CollisionEnvBullet::checkSelfCollisionHelper
void checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Bundles the different checkSelfCollision functions into a single function.
Definition: collision_env_bullet.cpp:148
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix.
Definition: collision_matrix.h:112
collision_detection::NAME
static const std::string NAME
Definition: collision_env_fcl.cpp:81
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
collision_detection::CollisionEnvBullet::addLinkAsCollisionObject
void addLinkAsCollisionObject(const urdf::LinkSharedPtr &link)
Construts a bullet collision object out of a robot link.
Definition: collision_env_bullet.cpp:417
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:382
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
moveit::core::AttachedBody::getGlobalCollisionBodyTransforms
const EigenSTL::vector_Isometry3d & getGlobalCollisionBodyTransforms() const
Get the global transforms (in world frame) for the collision bodies. The returned transforms are guar...
Definition: attached_body.h:256
collision_detection_bullet::CollisionObjectWrapper
Tesseract bullet collision object.
Definition: bullet_utils.h:173
collision_detection::CollisionEnvBullet::notifyObjectChange
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
Callback function executed for each change to the world environment.
Definition: collision_env_bullet.cpp:349
collision_detection::CollisionEnvBullet::checkRobotCollisionHelper
void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Definition: collision_env_bullet.cpp:211
collision_detection::BodyTypes::ROBOT_ATTACHED
@ ROBOT_ATTACHED
A body attached to a robot link.
Definition: include/moveit/collision_detection/collision_common.h:94
moveit::core::RobotState::getAttachedBodies
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Definition: robot_state.cpp:1113
collision_detection::CollisionEnvBullet::addAttachedOjects
void addAttachedOjects(const moveit::core::RobotState &state, std::vector< collision_detection_bullet::CollisionObjectWrapperPtr > &cows) const
All of the attached objects in the robot state are wrapped into bullet collision objects.
Definition: collision_env_bullet.cpp:363
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
collision_detection::CollisionEnv::getLinkScale
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
Definition: collision_env.cpp:219
collision_detection::CollisionEnvBullet::updatedPaddingOrScaling
void updatedPaddingOrScaling(const std::vector< std::string > &links) override
Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot l...
Definition: collision_env_bullet.cpp:391
collision_detector_allocator_bullet.h
collision_detection::CollisionEnvBullet::manager_CCD_
collision_detection_bullet::BulletCastBVHManagerPtr manager_CCD_
Handles continuous robot world collision checks.
Definition: collision_env_bullet.h:186
collision_detection::CollisionEnvBullet::checkRobotCollision
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
Definition: collision_env_bullet.cpp:183
collision_detection::CollisionEnvBullet::~CollisionEnvBullet
~CollisionEnvBullet() override
Definition: collision_env_bullet.cpp:130
collision_detection::DistanceResult
Result of a distance request.
Definition: include/moveit/collision_detection/collision_common.h:357
moveit::core::RobotState::getCollisionBodyTransform
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1426
collision_detection::CollisionEnvBullet::addToManager
void addToManager(const World::Object *obj)
Adds a world object to the collision managers.
Definition: collision_env_bullet.cpp:285
collision_detection::CollisionEnvBullet::active_
std::vector< std::string > active_
The active links where active refers to the group which can collide with everything.
Definition: collision_env_bullet.h:204
collision_detection::CollisionEnv::getLinkPadding
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
Definition: collision_env.cpp:179
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::CollisionEnv::setWorld
virtual void setWorld(const WorldPtr &world)
Definition: collision_env.cpp:282
collision_detection::CollisionDetectorAllocatorBullet::getName
const std::string & getName() const override
Definition: collision_env_bullet.cpp:479
collision_detection::World::CREATE
@ CREATE
Definition: world.h:258
collision_detection_bullet::AlignedVector
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
Definition: basic_types.h:53
collision_detection_bullet::CollisionObjectType::CONVEX_HULL
@ CONVEX_HULL
Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex it will be c...
collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE
@ USE_SHAPE_TYPE
Infer the type from the type specified in the shapes::Shape class.
collision_detection::DistanceRequest
Representation of a distance-reporting request.
Definition: include/moveit/collision_detection/collision_common.h:241
collision_detection::CollisionEnv::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
Definition: collision_env.h:342
collision_detection::CollisionEnvBullet::manager_
collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_
Handles self collision checks.
Definition: collision_env_bullet.h:181
collision_detection_bullet::urdfPose2Eigen
Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose &pose)
Definition: ros_bullet_utils.h:115
collision_env_bullet.h
collision_detection::CollisionEnv::getWorld
const WorldPtr & getWorld()
Definition: collision_env.h:267
collision_detection::CollisionEnvBullet::observer_handle_
World::ObserverHandle observer_handle_
Definition: collision_env_bullet.h:210
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Dec 24 2024 03:27:14