Loading...
Searching...
No Matches
ConstrainedPlanningSphere.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39from __future__ import print_function
40import argparse
41import math
42import numpy as np
43from 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_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
78def obstacles(x):
79 if x[2] > -0.8 and x[2] < -0.6:
80 if x[1] > -0.05 and x[1] < 0.05:
81 return x[0] > 0
82 return False
83 elif x[2] > -0.1 and x[2] < 0.1:
84 if x[0] > -0.05 and x[0] < 0.05:
85 return x[1] < 0
86 return False
87 elif x[2] > 0.6 and x[2] < 0.8:
88 if x[1] > -0.05 and x[1] < 0.05:
89 return x[0] < 0
90 return False
91 return True
92
93
94def 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
111def spherePlanningBench(cp, planners):
112 cp.setupBenchmark(planners, "sphere")
113 cp.runBenchmark()
114
115
116def spherePlanning(options):
117 # Create the ambient space state space for the problem.
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
147if __name__ == "__main__":
148 parser = argparse.ArgumentParser()
149 parser.add_argument("-o", "--output", action="store_true",
150 help="Dump found solution path (if one exists) in plain text and planning "
151 "graph in GraphML to `sphere_path.txt` and `sphere_graph.graphml` "
152 "respectively.")
153 parser.add_argument("--bench", action="store_true",
154 help="Do benchmarking on provided planner list.")
155 addSpaceOption(parser)
156 addPlannerOption(parser)
157 addConstrainedOptions(parser)
158 addAtlasOptions(parser)
159
160 spherePlanning(parser.parse_args())
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition Constraint.h:76
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
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...
The lower and upper bounds for an Rn space.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition State.h:50
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...