39 from __future__
import print_function
43 from ConstrainedPlanningCommon
import *
49 super(SphereConstraint, self).__init__(3, 1)
51 def function(self, x, out):
52 out[0] = np.linalg.norm(x) - 1
54 def jacobian(self, x, out):
55 nrm = np.linalg.norm(x)
56 if np.isfinite(nrm)
and nrm > 0:
64 def __init__(self, space):
65 super(SphereProjection, self).__init__(space)
73 def project(self, state, projection):
74 projection[0] = math.atan2(state[1], state[0])
75 projection[1] = math.acos(state[2])
79 if x[2] > -0.8
and x[2] < -0.6:
80 if x[1] > -0.05
and x[1] < 0.05:
83 elif x[2] > -0.1
and x[2] < 0.1:
84 if x[0] > -0.05
and x[0] < 0.05:
87 elif x[2] > 0.6
and x[2] < 0.8:
88 if x[1] > -0.05
and x[1] < 0.05:
94 def spherePlanningOnce(cp, plannername, output):
95 cp.setPlanner(plannername,
"sphere")
98 stat = cp.solveOnce(output,
"sphere")
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)
107 cp.dumpGraph(
"sphere")
111 def spherePlanningBench(cp, planners):
112 cp.setupBenchmark(planners,
"sphere")
116 def spherePlanning(options):
122 rvss.setBounds(bounds)
138 cp.setStartAndGoalStates(start, goal)
141 planners = options.planner.split(
",")
142 if not options.bench:
143 spherePlanningOnce(cp, planners[0], options.output)
145 spherePlanningBench(cp, planners)
147 if __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` "
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)
160 spherePlanning(parser.parse_args())
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
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.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...