ConstrainedPlanningSphere.py
1 #!/usr/bin/env python
2
3
36
37 # Author: Mark Moll
38
39 from __future__ import print_function
40 import argparse
41 import math
42 import numpy as np
43 from ConstrainedPlanningCommon import *
44
45
47
48  def __init__(self):
49  super(SphereConstraint, self).__init__(3, 1)
50
51  def function(self, x, out):
52  out[0] = np.linalg.norm(x) - 1
53
54  def jacobian(self, x, out):
55  nrm = np.linalg.norm(x)
56  if np.isfinite(nrm) and nrm > 0:
57  out[0, :] = x / nrm
58  else:
59  out[0, :] = [1, 0, 0]
60
61
63
64  def __init__(self, space):
65  super(SphereProjection, self).__init__(space)
66
67  def getDimension(self):
68  return 2
69
70  def defaultCellSizes(self):
71  self.cellSizes_ = list2vec([.1, .1])
72
73  def project(self, state, projection):
74  projection[0] = math.atan2(state[1], state[0])
75  projection[1] = math.acos(state[2])
76
77
78 def obstacles(x):
79  if -0.8 < x[2] and x[2] < -0.6:
80  if -0.05 < x[1] and x[1] < 0.05:
81  return x[0] > 0
82  return False
83  elif -0.1 < x[2] and x[2] < 0.1:
84  if -0.05 < x[0] and x[0] < 0.05:
85  return x[1] < 0
86  return False
87  elif 0.6 < x[2] and x[2] < 0.8:
88  if -0.05 < x[1] and x[1] < 0.05:
89  return x[0] < 0
90  return False
91  return True
92
93
94 def spherePlanningOnce(cp, plannername, output):
95  cp.setPlanner(plannername, "sphere")
96
97  # Solve the problem
98  stat = cp.solveOnce(output, "sphere")
99
100  if output:
101  ou.OMPL_INFORM("Dumping problem information to sphere_info.txt.")
102  with open("sphere_info.txt", "w") as infofile:
103  print(cp.spaceType, file=infofile)
104
105  cp.atlasStats()
106  if output:
107  cp.dumpGraph("sphere")
108  return stat
109
110
111 def spherePlanningBench(cp, planners):
112  cp.setupBenchmark(planners, "sphere")
113  cp.runBenchmark()
114
115
116 def spherePlanning(options):
117  # Create the ambient space state space for the problem.
118  rvss = ob.RealVectorStateSpace(3)
119  bounds = ob.RealVectorBounds(3)
120  bounds.setLow(-2)
121  bounds.setHigh(2)
122  rvss.setBounds(bounds)
123
124  # Create our constraint.
125  constraint = SphereConstraint()
126
127  cp = ConstrainedProblem(options.space, rvss, constraint, options)
128  cp.css.registerProjection("sphere", SphereProjection(cp.css))
129
130  start = ob.State(cp.css)
131  goal = ob.State(cp.css)
132  start[0] = 0
133  start[1] = 0
134  start[2] = -1
135  goal[0] = 0
136  goal[1] = 0
137  goal[2] = 1
138  cp.setStartAndGoalStates(start, goal)
139  cp.ss.setStateValidityChecker(ob.StateValidityCheckerFn(obstacles))
140
141  planners = options.planner.split(",")
142  if not options.bench:
143  spherePlanningOnce(cp, planners[0], options.output)
144  else:
145  spherePlanningBench(cp, planners)
146
147 if __name__ == "__main__":
148  parser = argparse.ArgumentParser()
150  help="Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
151  "sphere_path.txt and sphere_graph.graphml respectively.")
153  help="Do benchmarking on provided planner list.")
158
159  spherePlanning(parser.parse_args())
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:75
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition: State.h:49
The lower and upper bounds for an Rn space.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...