SE3MultiRigidBodyPlanning.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/SE3MultiRigidBodyPlanning.h"
14 
16  AppBase<AppType::GEOMETRIC>(std::make_shared<base::CompoundStateSpace>(), Motion_3D), n_(n)
17 {
18  assert (n > 0);
19  name_ = "Multi rigid body planning (3D)";
20  // Adding n SE(3) StateSpaces
21  for (unsigned int i = 0; i < n_; ++i)
22  si_->getStateSpace()->as<base::CompoundStateSpace>()->addSubspace(
23  std::make_shared<base::SE3StateSpace>(), 1.0);
24 }
25 
26 void ompl::app::SE3MultiRigidBodyPlanning::inferEnvironmentBounds()
27 {
28  // Infer bounds for all n SE(3) spaces
29  for (unsigned int i = 0; i < n_; ++i)
30  InferEnvironmentBounds(getGeometricComponentStateSpace(i), *static_cast<RigidBodyGeometry*>(this));
31 }
32 
33 void ompl::app::SE3MultiRigidBodyPlanning::inferProblemDefinitionBounds()
34 {
35  // Make sure that all n SE(3) 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::SE3StateSpace::StateType>(i);
50  sub->setX(s.x);
51  sub->setY(s.y);
52  sub->setZ(s.z);
53  sub->rotation().setIdentity();
54  }
55  return st;
56 }
57 
59 {
60  assert (index < n_);
61  const auto* st = state->as<base::CompoundStateSpace::StateType>()->as<base::SE3StateSpace::StateType>(index);
62  return static_cast<const base::State*>(st);
63 }
unsigned int n_
The number of independent rigid bodies to plan for.
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.
base::ScopedState getDefaultStartState() const override
Constructs the default start state where all robots begin at their geometric center....
SE3MultiRigidBodyPlanning(unsigned int n)
Constructs an instance of multiple rigid bodies for 3D geometric planning. n is the number of indepen...
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(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:54
void setX(double x)
Set the X component of the state.
Definition: SE3StateSpace.h:89
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