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  double distanceGoal(const ob::State *state) const override
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()
103 {
104  // construct the state space we are planning in
105  auto space(std::make_shared<ob::SE3StateSpace>());
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->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
127  // and returns true while it can produce new samples we don't need to
128  // check if new samples are different from ones previously computed as
129  // this is pefromed automatically by GoalLazySamples
130  ob::GoalSamplingFn samplingFunction = [&ss, &region](const ob::GoalLazySamples *gls, ob::State *result)
131  {
132  return regionSamplingWithGS(ss.getSpaceInformation(), ss.getProblemDefinition(), &region,
133  gls, result);
134  };
135 
136  // create an instance of GoalLazySamples:
137  auto goal(std::make_shared<ob::GoalLazySamples>(ss.getSpaceInformation(), samplingFunction));
138 
139  // we set a goal that is sampleable, but it in fact corresponds to a region that is not sampleable by default
140  ss.setGoal(goal);
141 
142  // attempt to solve the problem
143  ob::PlannerStatus solved = ss.solve(3.0);
144 
145  if (solved)
146  {
147  std::cout << "Found solution:" << std::endl;
148  // print the path to screen
149  ss.simplifySolution();
150  ss.getSolutionPath().print(std::cout);
151  }
152  else
153  std::cout << "No solution found" << std::endl;
154 
155  // the region variable will now go out of scope. To make sure it is not used in the sampling function any more
156  // (i.e., the sampling thread was able to terminate), we make sure sampling has terminated
157  goal->as<ob::GoalLazySamples>()->stopSampling();
158 }
160 
161 int main(int /*argc*/, char ** /*argv*/)
162 {
163  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
164 
165  planWithIK();
166 
167  return 0;
168 }
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 shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition: ScopedState.h:56
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...
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
std::function< bool(const GoalLazySamples *, State *)> GoalSamplingFn
Goal sampling function. Returns false when no further calls should be made to it. Fills its second ar...
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
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:75
Definition of a goal region.
Definition: GoalRegion.h:47
Genetic Algorithm for searching valid states.
Definition: GeneticSearch.h:60
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47