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 }
Definition of a compound state.
Definition: State.h:87
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 an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void setX(double x)
Set the X component of the state.
Definition: SE3StateSpace.h:89
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:54
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.
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:282
Definition of a scoped state.
Definition: ScopedState.h:57
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:80
base::ScopedState getDefaultStartState() const override
Constructs the default start state where all robots begin at their geometric center....