SE2MultiRigidBodyPlanning.cpp
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/apps/SE2MultiRigidBodyPlanning.h"
14 
16  AppBase<AppType::GEOMETRIC>(std::make_shared<base::CompoundStateSpace>(), Motion_2D), n_(n)
17 {
18  assert (n > 0);
19  name_ = "Multi rigid body planning (2D)";
20  // Adding n SE(2) StateSpaces
21  for (unsigned int i = 0; i < n_; ++i)
22  si_->getStateSpace()->as<base::CompoundStateSpace>()->addSubspace(
23  std::make_shared<base::SE2StateSpace>(), 1.0);
24 }
25 
26 void ompl::app::SE2MultiRigidBodyPlanning::inferEnvironmentBounds()
27 {
28  // Infer bounds for all n SE(2) spaces
29  for (unsigned int i = 0; i < n_; ++i)
30  InferEnvironmentBounds(getGeometricComponentStateSpace(i), *static_cast<RigidBodyGeometry*>(this));
31 }
32 
33 void ompl::app::SE2MultiRigidBodyPlanning::inferProblemDefinitionBounds()
34 {
35  // Make sure that all n SE(2) spaces get the same bounds, if they are adjusted
36  for (unsigned int i = 0; i < n_; ++i)
38  getGeometricStateExtractor(), factor_, add_,
39  n_, getGeometricComponentStateSpace(i), mtype_);
40 }
41 
43 {
44  base::ScopedState<> st(getStateSpace());
45  auto* c_st = st->as<base::CompoundStateSpace::StateType>();
46  for (unsigned int i = 0; i < n_; ++i)
47  {
48  aiVector3D s = getRobotCenter(i);
49  auto* sub = c_st->as<base::SE2StateSpace::StateType>(i);
50  sub->setX(s.x);
51  sub->setY(s.y);
52  sub->setYaw(0.0);
53  }
54  return st;
55 }
56 
58 {
59  assert (index < n_);
60  const auto* st = state->as<base::CompoundStateSpace::StateType>()->as<base::SE2StateSpace::StateType>(index);
61  return static_cast<const base::State*>(st);
62 }
SE2MultiRigidBodyPlanning(unsigned int n)
Constructs an instance of multiple rigid bodies for 2D geometric planning. n is the number of indepen...
base::ScopedState getDefaultStartState() const override
Constructs the default start state where all robots begin at their geometric center....
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.
unsigned int n_
The number of independent rigid bodies to plan for.
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
Definition of a compound state.
Definition: State.h:87
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:54
void setX(double x)
Set the X component of the state.
Definition: SE2StateSpace.h:79
Definition of a scoped state.
Definition: ScopedState.h:57
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:282
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:80