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<GEOMETRIC>
24  {
25  public:
27  SE3MultiRigidBodyPlanning(unsigned int n);
28 
29  ~SE3MultiRigidBodyPlanning() override = default;
30 
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 
57  {
58  // Return the zeroth component. All components are the same.
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 }
77 
virtual const base::StateSpacePtr & getGeometricComponentStateSpace(unsigned int index) const
Returns the state space corresponding for the indexth rigid body.
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.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:87
Definition of a scoped state.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::base::StateSpace.
base::ScopedState getDefaultStartState() const override
Constructs the default start state where all robots begin at their geometric center. If robots are all using the same mesh, this state is not likely to be valid.
SE3MultiRigidBodyPlanning(unsigned int n)
Constructs an instance of multiple rigid bodies for 3D geometric planning. n is the number of indepen...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int n_
The number of independent rigid bodies to plan for.
A space to allow the composition of state spaces.
Definition: StateSpace.h:573
Definition of an abstract state.
Definition: State.h:49