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<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 {
46  for (unsigned int i = 0; i < n_; ++i)
47  {
48  aiVector3D s = getRobotCenter(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 base::SE3StateSpace::StateType* st = state->as<base::CompoundStateSpace::StateType>()->as<base::SE3StateSpace::StateType>(index);
62  return static_cast<const base::State*>(st);
63 }
64 
virtual const base::StateSpacePtr & getGeometricComponentStateSpace(unsigned int index) const
Returns the state space corresponding for the indexth rigid body.
Definition of a compound state.
Definition: State.h:86
double add_
The value to add to inferred environment bounds (default 0)
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 state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:53
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.
STL namespace.
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:97
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:274
void setZ(double z)
Set the Z component of the state.
SE3MultiRigidBodyPlanning(unsigned int n)
Constructs an instance of multiple rigid bodies for 3D geometric planning. n is the number of indepen...
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
double factor_
The factor to multiply inferred environment bounds by (default 1)
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot&#39;s center (average of all the vertices of all its parts)
unsigned int n_
The number of independent rigid bodies to plan for.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void setY(double y)
Set the Y component of the state.
Definition: SE3StateSpace.h:97
A space to allow the composition of state spaces.
Definition: StateSpace.h:573
const SO3StateSpace::StateType & rotation() const
Get the rotation component of the state.
Definition: SE3StateSpace.h:79
void setIdentity()
Set the state to identity – no rotation.
void setX(double x)
Set the X component of the state.
Definition: SE3StateSpace.h:91
Definition of an abstract state.
Definition: State.h:49