SE3MultiRigidBodyPlanning.h
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2012, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ryan Luna */
12 
13 #include "omplapp/config.h"
14 #include "omplapp/apps/AppBase.h"
15 #include <ompl/base/spaces/SE3StateSpace.h>
16 
17 namespace ompl
18 {
19  namespace app
20  {
21 
23  class SE3MultiRigidBodyPlanning : public AppBase<AppType::GEOMETRIC>
24  {
25  public:
27  SE3MultiRigidBodyPlanning(unsigned int n);
28 
29  ~SE3MultiRigidBodyPlanning() override = default;
30 
33  base::ScopedState<> getDefaultStartState() const override;
34 
35  void inferEnvironmentBounds() override;
36 
37  void inferProblemDefinitionBounds() override;
38 
39  bool isSelfCollisionEnabled() const override
40  {
41  // Make sure that self collision is enabled to avoid inter-rigid body collision
42  return true;
43  }
44 
45  base::ScopedState<> getFullStateFromGeometricComponent(const base::ScopedState<> &state) const override
46  {
47  return state;
48  }
49 
51  virtual const base::StateSpacePtr& getGeometricComponentStateSpace(unsigned int index) const
52  {
53  return getStateSpace()->as<base::CompoundStateSpace>()->getSubspace(index);
54  }
55 
56  const base::StateSpacePtr& getGeometricComponentStateSpace() const override
57  {
58  // Return the zeroth component. All components are the same.
59  return getGeometricComponentStateSpace(0);
60  }
61 
62  unsigned int getRobotCount() const override
63  {
64  return n_;
65  }
66 
67  protected:
69  const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int index) const override;
70 
72  unsigned int n_;
73  };
74 
75  }
76 }
unsigned int n_
The number of independent rigid bodies to plan for.
A space to allow the composition of state spaces.
Definition: StateSpace.h:638
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:156
SE3MultiRigidBodyPlanning(unsigned int n)
Constructs an instance of multiple rigid bodies for 3D geometric planning. n is the number of indepen...
const base::State * getGeometricComponentStateInternal(const base::State *state, unsigned int index) const override
Returns the state corresponding to the indexth rigid body in the compound state.
Definition of a scoped state.
Definition: ScopedState.h:121
base::ScopedState getDefaultStartState() const override
Constructs the default start state where all robots begin at their geometric center....
Main namespace. Contains everything in this library.
Definition: AppBase.h:22