All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RigidBodyPlanningWithIK.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include <ompl/base/spaces/SE3StateSpace.h>
38 #include <ompl/geometric/SimpleSetup.h>
39 #include <ompl/base/goals/GoalLazySamples.h>
40 #include <ompl/geometric/GeneticSearch.h>
41 
42 #include <ompl/config.h>
43 #include <iostream>
44 
45 namespace ob = ompl::base;
46 namespace og = ompl::geometric;
47 
49 // describe an arbitrary representation of a goal region in SE(3)
50 class MyGoalRegion : public ob::GoalRegion
51 {
52 public:
53 
54  MyGoalRegion(const ob::SpaceInformationPtr &si) : ob::GoalRegion(si)
55  {
56  setThreshold(1e-2);
57  }
58 
59  virtual double distanceGoal(const ob::State *state) const
60  {
61  // goal region is given by states where x + y = z and orientation is close to identity
62  double d = fabs(state->as<ob::SE3StateSpace::StateType>()->getX()
63  + state->as<ob::SE3StateSpace::StateType>()->getY()
64  - state->as<ob::SE3StateSpace::StateType>()->getZ())
65  + fabs(state->as<ob::SE3StateSpace::StateType>()->rotation().w - 1.0);
66  return d;
67  }
68 
69 };
70 
71 // Goal regions such as the one above cannot be sampled, so
72 // bi-directional trees cannot be used for solving. However, we can
73 // transform such goal regions into ones that can be sampled. The
74 // caveat is that it should be possible to find states in this region
75 // with some other algorithm. Genetic algorithms that essentially
76 // perform inverse kinematics in the general sense can be used:
77 
78 bool regionSamplingWithGS(const ob::SpaceInformationPtr &si, const ob::ProblemDefinitionPtr &pd, const ob::GoalRegion *region, const ob::GoalLazySamples *gls, ob::State *result)
79 {
80  og::GeneticSearch g(si);
81 
82  // we can use a larger time duration for solve(), but we want to demo the ability
83  // of GeneticSearch to continue from where it left off
84  bool cont = false;
85  for (int i = 0 ; i < 100 ; ++i)
86  if (g.solve(0.05, *region, result))
87  {
88  cont = true;
89  break;
90  }
91 
92  if (cont)
93  {
94  std::cout << "Found goal state: " << std::endl;
95  si->printState(result);
96  }
97 
98  // we continue sampling while we are able to find solutions, we have found not more than 2 previous solutions and we have not yet solved the problem
99  return cont && gls->maxSampleCount() < 3 && !pd->hasSolution();
100 }
101 
102 void planWithIK(void)
103 {
104  // construct the state space we are planning in
106 
107  // set the bounds for the R^3 part of SE(3)
108  ob::RealVectorBounds bounds(3);
109  bounds.setLow(-1);
110  bounds.setHigh(1);
111 
112  space->as<ob::SE3StateSpace>()->setBounds(bounds);
113 
114  // define a simple setup class
115  og::SimpleSetup ss(space);
116 
117  // create a random start state
119  start->setXYZ(0, 0, 0);
120  start->rotation().setIdentity();
121  ss.addStartState(start);
122 
123  // define our goal region
124  MyGoalRegion region(ss.getSpaceInformation());
125 
126  // bind a sampling function that fills its argument with a sampled state and returns true while it can produce new samples
127  // we don't need to check if new samples are different from ones previously computed as this is pefromed automatically by GoalLazySamples
128  ob::GoalSamplingFn samplingFunction = boost::bind(&regionSamplingWithGS, ss.getSpaceInformation(), ss.getProblemDefinition(), &region, _1, _2);
129 
130  // create an instance of GoalLazySamples:
131  ob::GoalPtr goal(new ob::GoalLazySamples(ss.getSpaceInformation(), samplingFunction));
132 
133  // we set a goal that is sampleable, but it in fact corresponds to a region that is not sampleable by default
134  ss.setGoal(goal);
135 
136  // attempt to solve the problem
137  ob::PlannerStatus solved = ss.solve(3.0);
138 
139  if (solved)
140  {
141  std::cout << "Found solution:" << std::endl;
142  // print the path to screen
143  ss.simplifySolution();
144  ss.getSolutionPath().print(std::cout);
145  }
146  else
147  std::cout << "No solution found" << std::endl;
148 
149  // the region variable will now go out of scope. To make sure it is not used in the sampling function any more
150  // (i.e., the sampling thread was able to terminate), we make sure sampling has terminated
151  goal->as<ob::GoalLazySamples>()->stopSampling();
152 }
154 
155 int main(int, char **)
156 {
157  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
158 
159  planWithIK();
160 
161  return 0;
162 }
Definition of a goal region that can be sampled, but the sampling process can be slow. This class allows sampling the happen in a separate thread, and the number of goals may increase, as the planner is running, in a thread-safe manner.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual double distanceGoal(const State *st) const =0
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
virtual unsigned int maxSampleCount(void) const
Return the maximum number of samples that can be asked for before repeating.
Definition: GoalStates.cpp:90
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:543
const T * as(void) const
Cast this instance to a desired type.
Definition: State.h:74
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:64
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
The lower and upper bounds for an Rn space.
void setThreshold(double threshold)
Set the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:81
Definition of a goal region.
Definition: GoalRegion.h:50
A boost shared pointer wrapper for ompl::base::Goal.
boost::function< bool(const GoalLazySamples *, State *)> GoalSamplingFn
Goal sampling function. Returns false when no further calls should be made to it. Fills its second ar...
Genetic Algorithm for searching valid states.
Definition: GeneticSearch.h:62
A state space representing SE(3)
Definition: SE3StateSpace.h:50