Loading...
Searching...
No Matches
RigidBodyPlanningWithODESolverAndControls.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39from math import sin, cos, tan
40from functools import partial
41try:
42 from ompl import base as ob
43 from ompl import control as oc
44except ImportError:
45 # if the ompl module is not in the PYTHONPATH assume it is installed in a
46 # subdirectory of the parent directory called "py-bindings."
47 from os.path import abspath, dirname, join
48 import sys
49 sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings'))
50 from ompl import base as ob
51 from ompl import geometric as og # needed for asGeometric()
52 from ompl import control as oc
53
54def kinematicCarODE(q, u, qdot):
55 theta = q[2]
56 carLength = 0.2
57 qdot[0] = u[0] * cos(theta)
58 qdot[1] = u[0] * sin(theta)
59 qdot[2] = u[0] * tan(u[1]) / carLength
60
61
62def isStateValid(spaceInformation, state):
63 # perform collision checking or check if other constraints are
64 # satisfied
65 return spaceInformation.satisfiesBounds(state)
66
67def plan():
68 # construct the state space we are planning in
69 space = ob.SE2StateSpace()
70
71 # set the bounds for the R^2 part of SE(2)
72 bounds = ob.RealVectorBounds(2)
73 bounds.setLow(-1)
74 bounds.setHigh(1)
75 space.setBounds(bounds)
76
77 # create a control space
78 cspace = oc.RealVectorControlSpace(space, 2)
79
80 # set the bounds for the control space
81 cbounds = ob.RealVectorBounds(2)
82 cbounds.setLow(-.3)
83 cbounds.setHigh(.3)
84 cspace.setBounds(cbounds)
85
86 # define a simple setup class
87 ss = oc.SimpleSetup(cspace)
88 validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
89 ss.setStateValidityChecker(validityChecker)
90 ode = oc.ODE(kinematicCarODE)
91 odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
92 propagator = oc.ODESolver.getStatePropagator(odeSolver)
93 ss.setStatePropagator(propagator)
94
95 # create a start state
96 start = ob.State(space)
97 start().setX(-0.5)
98 start().setY(0.0)
99 start().setYaw(0.0)
100
101 # create a goal state
102 goal = ob.State(space)
103 goal().setX(0.0)
104 goal().setY(0.5)
105 goal().setYaw(0.0)
106
107 # set the start and goal states
108 ss.setStartAndGoalStates(start, goal, 0.05)
109
110 # attempt to solve the problem
111 solved = ss.solve(120.0)
112
113 if solved:
114 # print the path to screen
115 print("Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix())
116
117if __name__ == "__main__":
118 plan()
The lower and upper bounds for an Rn space.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
Definition of an abstract state.
Definition: State.h:50
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:200
A control space representing Rn.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...