Robowflex  v0.1
Making MoveIt Easy
robowflex::darts::World Class Reference

Wrapper for a dart::simulation::World, which contains a set of skeletons (i.e., Robots and Structures). The World is the main object that is used by motion planning as it contains a holistic view of the scene. More...

#include <world.h>

Classes

struct  CollisionInfo
 Collision filter information. More...
 

Public Member Functions

void clearIKModules ()
 Clear all IK modules created in skeletons in the world. More...
 
void lock ()
 Grab internal recursive lock for world. More...
 
void unlock ()
 Unlock internal recursive world lock. More...
 
void openOSGViewer ()
 Open the Open Scene Graph visualization for this world. More...
 
dart::collision::CollisionDetectorPtr getCollider () const
 Get the collision detector for this world. More...
 
std::shared_ptr< dart::collision::CollisionGroup > getSelfCollisionGroup (const std::string &name) const
 Get the self-collision group for a skeleton in this world. More...
 
std::shared_ptr< dart::collision::CollisionGroup > getOtherCollisionGroup (const std::string &name) const
 Get the group for everything other than a given skeleton in this world. More...
 
CollisionInfo getCollisionInfo (const std::string &name) const
 Get the collision info (self and other collision groups) for a skeleton in this world. More...
 
std::shared_ptr< dart::collision::CompositeCollisionFilter > & getWorldCollisionFilter ()
 Get the current world collision filter (composite of all skeleton filters). This is more efficient than constructing a new filter from robowflex::darts::World::getDefaultFilter() or robowflex::darts::World::getAllValidFilter(). More...
 
Constuctors
 World (const std::string &name="world")
 Create an empty world. More...
 
WorldPtr clone (const std::string &suffix="") const
 Clone this world. More...
 
World Entity Management
void addRobot (RobotPtr robot)
 Add a robot to the world. More...
 
void removeRobot (const std::string &name)
 Remove a robot from the world. More...
 
void removeRobot (const RobotPtr &robot)
 Remove a robot from the world. More...
 
RobotPtr getRobot (const std::string &name)
 Get a robot in the world. More...
 
RobotConstPtr getRobotConst (const std::string &name) const
 Get a robot in the world. More...
 
void addStructure (StructurePtr structure)
 Add a structure to the world. More...
 
void removeStructure (const std::string &name)
 Remove a structure from the world. More...
 
void removeStructure (const StructurePtr &structure)
 Remove a structure from the world. More...
 
StructurePtr getStructure (const std::string &name)
 Get a structure in the world. More...
 
StructureConstPtr getStructureConst (const std::string &name) const
 Get a structure in the world. More...
 
const std::map< std::string, StructurePtr > & getStructures ()
 Get the set of structures in the world. More...
 
Getters and Setters
const std::stringgetName () const
 Get the name of this world. More...
 
std::pair< Eigen::Vector3d, Eigen::Vector3d > getWorkspaceBounds () const
 Get the bounds on the workspace. More...
 
Eigen::Vector3d & getWorkspaceLow ()
 Get the lower bounds of the workspace. More...
 
const Eigen::Vector3d & getWorkspaceLowConst () const
 Get the lower bounds of the workspace. More...
 
Eigen::Vector3d & getWorkspaceHigh ()
 Get the upper bounds of the workspace. More...
 
const Eigen::Vector3d & getWorkspaceHighConst () const
 Get the upper bounds of the workspace. More...
 
dart::simulation::WorldPtr getSim ()
 Get the Dart world. More...
 
const dart::simulation::WorldPtr & getSimConst () const
 Get the Dart world. More...
 
unsigned int getSkeletonIndex (const dart::dynamics::SkeletonPtr &skeleton) const
 Get the index in the world of a skeleton. More...
 
Collisions
std::shared_ptr< dart::collision::BodyNodeCollisionFilter > getAllValidFilter () const
 Get a collision filter that allows collisions between all body nodes. Useful for constructing a custom filter to select one or a few objects. More...
 
std::shared_ptr< dart::collision::BodyNodeCollisionFilter > getDefaultFilter () const
 Get a collision filter that is a copy of the default filter. More...
 
bool inCollision (const std::shared_ptr< dart::collision::CollisionFilter > &filter=nullptr) const
 Checks if world is currently in collision. More...
 
double distanceToCollision () const
 Gets the current signed distance to collision in the world. More...
 
void forceUpdate ()
 Force forward kinematics to update. More...
 

Private Member Functions

void addSkeletonCollider (const std::string &name, const dart::dynamics::SkeletonPtr &skeleton)
 Add a new collision filter (ACM) for a skeleton. More...
 
void removeSkeletonCollider (const std::string &name, const dart::dynamics::SkeletonPtr &skeleton)
 Remove a collision filter (ACM). More...
 

Private Attributes

dart::simulation::WorldPtr world_
 Underlying world. More...
 
Eigen::Vector3d low_ {-5, -5, -5}
 Lower workspace bounds. More...
 
Eigen::Vector3d high_ {5, 5, 5}
 Upper workspace bounds. More...
 
std::map< std::string, RobotPtrrobots_
 Robots in world. More...
 
std::map< std::string, StructurePtrstructures_
 Structures in world. More...
 
dart::collision::CollisionGroupPtr all_
 All collision groups in world. More...
 
std::map< std::string, CollisionInfocollision_
 
std::shared_ptr< dart::collision::CompositeCollisionFilter > filter_
 
dart::collision::CollisionDetectorPtr collider_
 Collision checker. More...
 
const std::string name_
 Name of world. More...
 
std::recursive_mutex mutex_
 Internal lock. More...
 

Detailed Description

Wrapper for a dart::simulation::World, which contains a set of skeletons (i.e., Robots and Structures). The World is the main object that is used by motion planning as it contains a holistic view of the scene.

Definition at line 57 of file world.h.

Constructor & Destructor Documentation

◆ World()

World::World ( const std::string name = "world")

Create an empty world.

Parameters
[in]nameName for the world.

World

Definition at line 36 of file world.cpp.

37  : world_(dart::simulation::World::create(name))
38  , filter_(std::make_shared<dart::collision::CompositeCollisionFilter>())
39  , name_(name)
40 {
41  world_->setGravity(Eigen::Vector3d(0, 0, -9.81));
42 
43  auto *solver = world_->getConstraintSolver();
44 
45  auto &opt = solver->getCollisionOption();
46  opt.collisionFilter = filter_;
47 
48  collider_ = solver->getCollisionDetector();
49 
50  // world_->getConstraintSolver()->setCollisionDetector(collider_->cloneWithoutCollisionObjects());
51 
52  all_ = collider_->createCollisionGroupAsSharedPtr();
53 }
dart::simulation::WorldPtr world_
Underlying world.
Definition: world.h:294
dart::collision::CollisionDetectorPtr collider_
Collision checker.
Definition: world.h:309
const std::string name_
Name of world.
Definition: world.h:310
std::shared_ptr< dart::collision::CompositeCollisionFilter > filter_
Definition: world.h:306
dart::collision::CollisionGroupPtr all_
All collision groups in world.
Definition: world.h:302

Member Function Documentation

◆ addRobot()

void World::addRobot ( RobotPtr  robot)

Add a robot to the world.

Parameters
[in]robotRobot to add.

Definition at line 108 of file world.cpp.

109 {
110  auto it = robots_.find(robot->getName());
111  if (it == robots_.end())
112  {
113  // Add robot to world
114  robots_.emplace(robot->getName(), robot);
115  world_->addSkeleton(robot->getSkeleton());
116 
117  // Add ACM to collision filter
118  filter_->addCollisionFilter(robot->getACM()->getFilter().get());
119  addSkeletonCollider(robot->getName(), robot->getSkeletonConst());
120  }
121 }
std::map< std::string, RobotPtr > robots_
Robots in world.
Definition: world.h:299
void addSkeletonCollider(const std::string &name, const dart::dynamics::SkeletonPtr &skeleton)
Add a new collision filter (ACM) for a skeleton.
Definition: world.cpp:81
Functions for loading and animating robots in Blender.

◆ addSkeletonCollider()

void World::addSkeletonCollider ( const std::string name,
const dart::dynamics::SkeletonPtr &  skeleton 
)
private

Add a new collision filter (ACM) for a skeleton.

Parameters
[in]nameName for collision filter.
[in]skeletonSkeleton collision filter is for.

Definition at line 81 of file world.cpp.

82 {
83  // Collect collision groups
84  CollisionInfo info;
85  info.self = collider_->createCollisionGroupAsSharedPtr();
86  info.self->addShapeFramesOf(skeleton.get());
87  all_->addShapeFramesOf(skeleton.get());
88 
89  info.others = collider_->createCollisionGroupAsSharedPtr();
90  for (const auto &entry : collision_)
91  info.others->addShapeFramesOf(entry.second.self.get());
92 
93  collision_.emplace(name, info);
94 }
std::map< std::string, CollisionInfo > collision_
Definition: world.h:303

◆ addStructure()

void World::addStructure ( StructurePtr  structure)

Add a structure to the world.

Parameters
[in]structureStructure to add.

Definition at line 163 of file world.cpp.

164 {
165  auto it = structures_.find(structure->getName());
166  if (it == structures_.end())
167  {
168  structures_.emplace(structure->getName(), structure);
169  world_->addSkeleton(structure->getSkeleton());
170 
171  filter_->addCollisionFilter(structure->getACM()->getFilter().get());
172  addSkeletonCollider(structure->getName(), structure->getSkeletonConst());
173  }
174 }
std::map< std::string, StructurePtr > structures_
Structures in world.
Definition: world.h:300

◆ clearIKModules()

void World::clearIKModules ( )

Clear all IK modules created in skeletons in the world.

Definition at line 355 of file world.cpp.

356 {
357  for (std::size_t i = 0; i < world_->getNumSkeletons(); ++i)
358  {
359  auto skel = world_->getSkeleton(i);
360  skel->clearIK();
361 
362  for (auto *bn : skel->getBodyNodes())
363  bn->clearIK();
364  }
365 }

◆ clone()

WorldPtr World::clone ( const std::string suffix = "") const

Clone this world.

Parameters
[in]suffixSuffix to append to all names in the world (i.e., robots).

Definition at line 55 of file world.cpp.

56 {
57  auto world = std::make_shared<World>(name_ + suffix);
58 
59  for (const auto &robot : robots_)
60  world->addRobot(robot.second->cloneRobot(robot.first + suffix));
61 
62  for (const auto &structure : structures_)
63  world->addStructure(structure.second->cloneStructure(structure.first + suffix));
64 
65  // for (std::size_t i = 0; i < world_->getNumSkeletons(); ++i)
66  // {
67  // auto skel = world_->getSkeleton(i);
68  // auto state = skel->getState();
69  // auto other_skel = world->getSim()->getSkeleton(skel->getName() + suffix);
70  // other_skel->setState(state);
71  // }
72 
73  return world;
74 }

◆ distanceToCollision()

double World::distanceToCollision ( ) const

Gets the current signed distance to collision in the world.

Returns
The signed distance to collision for the current state of the world.

Definition at line 312 of file world.cpp.

313 {
314  dart::collision::DistanceOption option;
315  option.distanceLowerBound = -std::numeric_limits<double>::infinity();
316  option.distanceFilter = std::make_shared<DistanceCollisionWrapper>(filter_);
317 
318  dart::collision::DistanceResult result;
319  double d = collider_->distance(all_.get(), option, &result);
320 
321  const auto &shape_node1 = result.shapeFrame1->asShapeNode();
322  const auto &shape_node2 = result.shapeFrame2->asShapeNode();
323 
324  const auto &body_node1 = shape_node1->getBodyNodePtr();
325  const auto &body_node2 = shape_node2->getBodyNodePtr();
326 
327  RBX_INFO("Distance B1:%s:%s -> B2:%s:%s = %d", //
328  shape_node1->getName(), body_node1->getName(), //
329  shape_node2->getName(), body_node2->getName(), //
330  d);
331 
332  return d;
333 }
T infinity(T... args)
#define RBX_INFO(fmt,...)
Output a info logging message.
Definition: log.h:118

◆ forceUpdate()

void World::forceUpdate ( )

Force forward kinematics to update.

Definition at line 349 of file world.cpp.

350 {
351  for (std::size_t i = 0; i < world_->getNumSkeletons(); ++i)
352  world_->getSkeleton(i)->computeForwardKinematics();
353 }

◆ getAllValidFilter()

std::shared_ptr< dart::collision::BodyNodeCollisionFilter > World::getAllValidFilter ( ) const

Get a collision filter that allows collisions between all body nodes. Useful for constructing a custom filter to select one or a few objects.

Returns
A collision filter that considers no collisions.

Definition at line 265 of file world.cpp.

266 {
267  auto filter = std::make_shared<dart::collision::BodyNodeCollisionFilter>();
268  for (unsigned int i = 0; i < world_->getNumSkeletons(); ++i)
269  {
270  const auto &si = world_->getSkeleton(i);
271 
272  // Ignore collisions internally
273  for (unsigned int ii = 0; ii < si->getNumBodyNodes(); ++ii)
274  for (unsigned int ij = ii + 1; ij < si->getNumBodyNodes(); ++ij)
275  filter->addBodyNodePairToBlackList(si->getBodyNode(ii), si->getBodyNode(ij));
276 
277  // Ignore collisions on other skeletons
278  for (unsigned int j = i + 1; j < world_->getNumSkeletons(); ++j)
279  {
280  const auto &sj = world_->getSkeleton(j);
281  for (unsigned int ii = 0; ii < si->getNumBodyNodes(); ++ii)
282  for (unsigned int jj = 0; jj < sj->getNumBodyNodes(); ++jj)
283  filter->addBodyNodePairToBlackList(si->getBodyNode(ii), sj->getBodyNode(jj));
284  }
285  }
286 
287  return filter;
288 }

◆ getCollider()

dart::collision::CollisionDetectorPtr robowflex::darts::World::getCollider ( ) const

Get the collision detector for this world.

Returns
A pointer to the world's collision detector.

◆ getCollisionInfo()

World::CollisionInfo World::getCollisionInfo ( const std::string name) const

Get the collision info (self and other collision groups) for a skeleton in this world.

Parameters
[in]nameThe name of the skeleton.
Returns
A struct containing pointers to the skeleton's self and other collision groups.

Definition at line 387 of file world.cpp.

388 {
389  return collision_.at(name);
390 }

◆ getDefaultFilter()

std::shared_ptr< dart::collision::BodyNodeCollisionFilter > World::getDefaultFilter ( ) const

Get a collision filter that is a copy of the default filter.

Returns
A copy of the default collision filter.

Definition at line 290 of file world.cpp.

291 {
292  auto filter = std::make_shared<dart::collision::BodyNodeCollisionFilter>();
293  for (const auto &[name, robot] : robots_)
294  for (const auto &[b1, b2] : robot->getACM()->getDisabledPairs())
295  filter->addBodyNodePairToBlackList(robot->getFrame(b1), robot->getFrame(b2));
296 
297  for (const auto &[name, structure] : structures_)
298  for (const auto &[b1, b2] : structure->getACM()->getDisabledPairs())
299  filter->addBodyNodePairToBlackList(structure->getFrame(b1), structure->getFrame(b2));
300 
301  return filter;
302 }

◆ getName()

const std::string & World::getName ( ) const

Get the name of this world.

Returns
The name of the world.

Definition at line 76 of file world.cpp.

77 {
78  return name_;
79 }

◆ getOtherCollisionGroup()

std::shared_ptr< dart::collision::CollisionGroup > World::getOtherCollisionGroup ( const std::string name) const

Get the group for everything other than a given skeleton in this world.

Parameters
[in]nameThe name of the skeleton.
Returns
A pointer to the other-collision group for the skeleton.

Definition at line 382 of file world.cpp.

383 {
384  return collision_.at(name).others;
385 }

◆ getRobot()

RobotPtr World::getRobot ( const std::string name)

Get a robot in the world.

Parameters
[in]nameName of robot to get.
Returns
The robot, or nullptr if non-existent.

Definition at line 145 of file world.cpp.

146 {
147  auto it = robots_.find(name);
148  if (it != robots_.end())
149  return it->second;
150 
151  return nullptr;
152 }

◆ getRobotConst()

RobotConstPtr World::getRobotConst ( const std::string name) const

Get a robot in the world.

Parameters
[in]nameName of robot to get.
Returns
The robot, or nullptr if non-existent.

Definition at line 154 of file world.cpp.

155 {
156  auto it = robots_.find(name);
157  if (it != robots_.end())
158  return it->second;
159 
160  return nullptr;
161 }

◆ getSelfCollisionGroup()

std::shared_ptr< dart::collision::CollisionGroup > World::getSelfCollisionGroup ( const std::string name) const

Get the self-collision group for a skeleton in this world.

Parameters
[in]nameThe name of the skeleton.
Returns
A pointer to the self-collision group for the skeleton.

Definition at line 377 of file world.cpp.

378 {
379  return collision_.at(name).self;
380 }

◆ getSim()

dart::simulation::WorldPtr World::getSim ( )

Get the Dart world.

Returns
The underlying Dart world.

Definition at line 244 of file world.cpp.

245 {
246  return world_;
247 }

◆ getSimConst()

const dart::simulation::WorldPtr & World::getSimConst ( ) const

Get the Dart world.

Returns
The underlying Dart world.

Definition at line 249 of file world.cpp.

250 {
251  return world_;
252 }

◆ getSkeletonIndex()

unsigned int World::getSkeletonIndex ( const dart::dynamics::SkeletonPtr &  skeleton) const

Get the index in the world of a skeleton.

Parameters
[in]skeletonThe skeleton to get the index of.
Returns
The index of a skeleton in the world.

Definition at line 254 of file world.cpp.

255 {
256  for (unsigned int i = 0; i < world_->getNumSkeletons(); ++i)
257  {
258  if (skeleton == world_->getSkeleton(i))
259  return i;
260  }
261 
262  return -1;
263 }

◆ getStructure()

StructurePtr World::getStructure ( const std::string name)

Get a structure in the world.

Parameters
[in]nameName of structure to get.
Returns
The structure, or nullptr if non-existent.

Definition at line 196 of file world.cpp.

197 {
198  auto it = structures_.find(name);
199  if (it != structures_.end())
200  return it->second;
201 
202  return nullptr;
203 }

◆ getStructureConst()

StructureConstPtr World::getStructureConst ( const std::string name) const

Get a structure in the world.

Parameters
[in]nameName of structure to get.
Returns
The structure, or nullptr if non-existent.

Definition at line 205 of file world.cpp.

206 {
207  auto it = structures_.find(name);
208  if (it != structures_.end())
209  return it->second;
210 
211  return nullptr;
212 }

◆ getStructures()

const std::map< std::string, StructurePtr > & World::getStructures ( )

Get the set of structures in the world.

Returns
A constant reference to the set of structures.

Definition at line 214 of file world.cpp.

215 {
216  return structures_;
217 }

◆ getWorkspaceBounds()

std::pair< Eigen::Vector3d, Eigen::Vector3d > World::getWorkspaceBounds ( ) const

Get the bounds on the workspace.

Returns
A pair of the lower and upper bounds on the workspace.

Definition at line 219 of file world.cpp.

220 {
221  return std::make_pair(low_, high_);
222 }
Eigen::Vector3d high_
Upper workspace bounds.
Definition: world.h:297
Eigen::Vector3d low_
Lower workspace bounds.
Definition: world.h:296
T make_pair(T... args)

◆ getWorkspaceHigh()

Eigen::Vector3d & World::getWorkspaceHigh ( )

Get the upper bounds of the workspace.

Returns
A 3D vector of the upper X-, Y-, and Z-bounds on the workspace.

Definition at line 234 of file world.cpp.

235 {
236  return high_;
237 }

◆ getWorkspaceHighConst()

const Eigen::Vector3d & World::getWorkspaceHighConst ( ) const

Get the upper bounds of the workspace.

Returns
A 3D vector of the upper X-, Y-, and Z-bounds on the workspace.

Definition at line 239 of file world.cpp.

240 {
241  return high_;
242 }

◆ getWorkspaceLow()

Eigen::Vector3d & World::getWorkspaceLow ( )

Get the lower bounds of the workspace.

Returns
A 3D vector of the lower X-, Y-, and Z-bounds on the workspace.

Definition at line 224 of file world.cpp.

225 {
226  return low_;
227 }

◆ getWorkspaceLowConst()

const Eigen::Vector3d & World::getWorkspaceLowConst ( ) const

Get the lower bounds of the workspace.

Returns
A 3D vector of the lower X-, Y-, and Z-bounds on the workspace.

Definition at line 229 of file world.cpp.

230 {
231  return low_;
232 }

◆ getWorldCollisionFilter()

std::shared_ptr< dart::collision::CompositeCollisionFilter > & robowflex::darts::World::getWorldCollisionFilter ( )

Get the current world collision filter (composite of all skeleton filters). This is more efficient than constructing a new filter from robowflex::darts::World::getDefaultFilter() or robowflex::darts::World::getAllValidFilter().

Returns
A pointer to the current world collision filter.

Definition at line 392 of file world.cpp.

393 {
394  if (filter_ == nullptr)
395  {
396  throw std::runtime_error("World collision filter is not initialized!");
397  }
398 
399  return filter_;
400 }

◆ inCollision()

bool World::inCollision ( const std::shared_ptr< dart::collision::CollisionFilter > &  filter = nullptr) const

Checks if world is currently in collision.

Parameters
[in]filterCustom collision filter to use. If null, will use default collision filter.
Returns
True if the world is currently in collision, false otherwise.

Definition at line 304 of file world.cpp.

305 {
306  dart::collision::CollisionOption option;
307  option.collisionFilter = (filter) ? filter : filter_;
308 
309  return collider_->collide(all_.get(), option, nullptr);
310 }

◆ lock()

void World::lock ( )

Grab internal recursive lock for world.

Definition at line 367 of file world.cpp.

368 {
369  mutex_.lock();
370 }
std::recursive_mutex mutex_
Internal lock.
Definition: world.h:312

◆ openOSGViewer()

void World::openOSGViewer ( )

Open the Open Scene Graph visualization for this world.

Definition at line 335 of file world.cpp.

336 {
337  ::osg::ref_ptr<dart::gui::osg::RealTimeWorldNode> node = new dart::gui::osg::RealTimeWorldNode(world_);
338 
339  auto viewer = dart::gui::osg::Viewer();
340  viewer.addWorldNode(node);
341 
342  viewer.setUpViewInWindow(0, 0, 1080, 1080);
343  viewer.getCameraManipulator()->setHomePosition(
344  ::osg::Vec3(2.57, 3.14, 1.64), ::osg::Vec3(0.00, 0.00, 0.00), ::osg::Vec3(-0.24, -0.25, 0.94));
345  viewer.setCameraManipulator(viewer.getCameraManipulator());
346  viewer.run();
347 }

◆ removeRobot() [1/2]

void World::removeRobot ( const RobotPtr robot)

Remove a robot from the world.

Parameters
[in]robotRobot to remove.

Definition at line 140 of file world.cpp.

141 {
142  removeRobot(robot->getName());
143 }
void removeRobot(const std::string &name)
Remove a robot from the world.
Definition: world.cpp:123

◆ removeRobot() [2/2]

void World::removeRobot ( const std::string name)

Remove a robot from the world.

Parameters
[in]nameName of robot to remove.

Definition at line 123 of file world.cpp.

124 {
125  auto it = robots_.find(name);
126  if (it != robots_.end())
127  {
128  // Remove robot from world
129  auto robot = it->second;
130  world_->removeSkeleton(robot->getSkeleton());
131 
132  // Remove ACM from filter
133  filter_->removeCollisionFilter(robot->getACM()->getFilter().get());
134  removeSkeletonCollider(robot->getName(), robot->getSkeletonConst());
135 
136  robots_.erase(it);
137  }
138 }
void removeSkeletonCollider(const std::string &name, const dart::dynamics::SkeletonPtr &skeleton)
Remove a collision filter (ACM).
Definition: world.cpp:96

◆ removeSkeletonCollider()

void World::removeSkeletonCollider ( const std::string name,
const dart::dynamics::SkeletonPtr &  skeleton 
)
private

Remove a collision filter (ACM).

Parameters
[in]nameName for collision filter.
[in]skeletonSkeleton collision filter is for.

Definition at line 96 of file world.cpp.

97 {
98  // Remove from collision groups
99  auto ic = collision_.find(name);
100  collision_.erase(ic);
101 
102  for (auto &entry : collision_)
103  entry.second.others->removeShapeFramesOf(skeleton.get());
104 
105  all_->removeShapeFramesOf(skeleton.get());
106 }

◆ removeStructure() [1/2]

void World::removeStructure ( const std::string name)

Remove a structure from the world.

Parameters
[in]nameName of structure to remove.

Definition at line 176 of file world.cpp.

177 {
178  auto it = structures_.find(name);
179  if (it != structures_.end())
180  {
181  auto structure = it->second;
182  world_->removeSkeleton(structure->getSkeleton());
183 
184  filter_->removeCollisionFilter(structure->getACM()->getFilter().get());
185  removeSkeletonCollider(structure->getName(), structure->getSkeletonConst());
186 
187  structures_.erase(it);
188  }
189 }

◆ removeStructure() [2/2]

void World::removeStructure ( const StructurePtr structure)

Remove a structure from the world.

Parameters
[in]structureStructure to remove.

Definition at line 191 of file world.cpp.

192 {
193  removeStructure(structure->getName());
194 }
void removeStructure(const std::string &name)
Remove a structure from the world.
Definition: world.cpp:176

◆ unlock()

void World::unlock ( )

Unlock internal recursive world lock.

Definition at line 372 of file world.cpp.

373 {
374  mutex_.unlock();
375 }
T unlock(T... args)

Member Data Documentation

◆ all_

dart::collision::CollisionGroupPtr robowflex::darts::World::all_
private

All collision groups in world.

Definition at line 302 of file world.h.

◆ collider_

dart::collision::CollisionDetectorPtr robowflex::darts::World::collider_
private

Collision checker.

Definition at line 309 of file world.h.

◆ collision_

std::map<std::string, CollisionInfo> robowflex::darts::World::collision_
private

Map of skeleton names to collision filters.

Definition at line 303 of file world.h.

◆ filter_

std::shared_ptr<dart::collision::CompositeCollisionFilter> robowflex::darts::World::filter_
private

Composite collision filter of skeleton filters.

Definition at line 306 of file world.h.

◆ high_

Eigen::Vector3d robowflex::darts::World::high_ {5, 5, 5}
private

Upper workspace bounds.

Definition at line 297 of file world.h.

◆ low_

Eigen::Vector3d robowflex::darts::World::low_ {-5, -5, -5}
private

Lower workspace bounds.

Definition at line 296 of file world.h.

◆ mutex_

std::recursive_mutex robowflex::darts::World::mutex_
private

Internal lock.

Definition at line 312 of file world.h.

◆ name_

const std::string robowflex::darts::World::name_
private

Name of world.

Definition at line 310 of file world.h.

◆ robots_

std::map<std::string, RobotPtr> robowflex::darts::World::robots_
private

Robots in world.

Definition at line 299 of file world.h.

◆ structures_

std::map<std::string, StructurePtr> robowflex::darts::World::structures_
private

Structures in world.

Definition at line 300 of file world.h.

◆ world_

dart::simulation::WorldPtr robowflex::darts::World::world_
private

Underlying world.

Definition at line 294 of file world.h.


The documentation for this class was generated from the following files: