1+ /* ********************************************************************
2+ * Software License Agreement (BSD License)
3+ *
4+ * Copyright (c) 2025, University of Santa Cruz Hybrid Systems Laboratory
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 University of Santa Cruz nor the names of
18+ * its 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: Beverly Xu */
36+
37+ #include " ompl/control/planners/rrt/HyRRT.h"
38+ #include " ompl/base/StateSpace.h"
39+ #include " ompl/control/spaces/RealVectorControlSpace.h"
40+ #include " ompl/base/spaces/HybridStateSpace.h"
41+ #include " ompl/control/ODESolver.h"
42+
43+ /* * \brief Function computes the Pythagorean distance between two states. */
44+ double distanceFunc (ompl::base::State *state1, ompl::base::State *state2)
45+ {
46+ double dist = 0 ;
47+ dist = sqrt (pow (state1->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [0 ] - state2->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [0 ], 2 ) + pow (state1->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [1 ] - state2->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [1 ], 2 ));
48+ return fabs (dist);
49+ }
50+
51+ /* * \brief Jump set is true whenever the ball is on or below the surface and has a downwards velocity. */
52+ bool jumpSet (ompl::control::HyRRT::Motion *motion)
53+ {
54+ auto *motion_state = motion->state ->as <ompl::base::CompoundState>()->as <ompl::base::RealVectorStateSpace::StateType>(0 );
55+ double velocity = motion_state->values [1 ];
56+ double pos_cur = motion_state->values [0 ];
57+
58+ if (pos_cur <= 0 && velocity <= 0 )
59+ return true ;
60+ else
61+ return false ;
62+ }
63+
64+ /* * \brief Flow set is true whenever the ball is above the surface or has an upwards velocity. */
65+ bool flowSet (ompl::control::HyRRT::Motion *motion)
66+ {
67+ return !jumpSet (motion);
68+ }
69+
70+ /* * \brief Unsafe set is true whenever the ball is above 10 units from the ground, to reduce time spent planning. */
71+ bool unsafeSet (ompl::control::HyRRT::Motion *motion)
72+ {
73+ double u = motion->control ->as <ompl::control::RealVectorControlSpace::ControlType>()->values [0 ];
74+ if (u > 5 || u < 0 )
75+ return true ;
76+ return false ;
77+ }
78+
79+ /* * \brief Represents the flow map, or the first-order derivative of the pinball state when in flow regime.
80+ * The only force applied here is of gravity in the negative y direction. */
81+ void flowODE (const ompl::control::ODESolver::StateType& x_cur, const ompl::control::Control* u, ompl::control::ODESolver::StateType& x_new)
82+ {
83+ (void )u; // No control is applied when a state is in the flow set
84+
85+ // Ensure qdot is the same size as q. Zero out all values.
86+ x_new.resize (x_cur.size (), 0 );
87+
88+ x_new[0 ] = x_cur[1 ]; // x-dot
89+ x_new[1 ] = x_cur[2 ]; // v-dot
90+ x_new[2 ] = 0 ; // a-dot
91+ }
92+
93+ /* * \brief Simulates the dynamics of the ball when in jump regime, with input from the surface. */
94+ ompl::base::State *discreteSimulator (ompl::base::State *x_cur, const ompl::control::Control *u, ompl::base::State *new_state)
95+ {
96+ double velocity = -0.8 * x_cur->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [1 ];
97+
98+ new_state->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [0 ] = x_cur->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [0 ];
99+ new_state->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [1 ] = velocity - u->as <ompl::control::RealVectorControlSpace::ControlType>()->values [0 ];
100+ new_state->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [2 ] = x_cur->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [2 ];
101+ return new_state;
102+ }
103+
104+ // Define goal region as a ball of radius 0.1 centered at {height, velocity} = {0, 0}
105+ class EuclideanGoalRegion : public ompl ::base::Goal
106+ {
107+ public:
108+ EuclideanGoalRegion (const ompl::base::SpaceInformationPtr &si) : ompl::base::Goal(si)
109+ {
110+ }
111+
112+ virtual bool isSatisfied (const ompl::base::State *st, double *distance) const
113+ {
114+ // perform any operations and return a truth value
115+ std::vector<double > goal = {0 , 0 };
116+ double distSqr = 0 ;
117+ for (int i = 0 ; i < 2 ; i++)
118+ {
119+ distSqr +=
120+ pow (st->as <ompl::base::CompoundState>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [i] -
121+ goal[i],
122+ 2 );
123+ }
124+
125+ *distance = sqrt (distSqr);
126+
127+ if (*distance < 0.1 )
128+ return true ;
129+ else
130+ return false ;
131+ }
132+
133+ virtual bool isSatisfied (const ompl::base::State *st) const
134+ {
135+ double distance = 0.0 ;
136+ return isSatisfied (st, &distance);
137+ }
138+ };
139+
140+ int main ()
141+ {
142+ // Set the bounds of space
143+ ompl::base::RealVectorStateSpace *statespace = new ompl::base::RealVectorStateSpace (0 );
144+ statespace->addDimension (-10 , 10 );
145+ statespace->addDimension (-10 , 10 );
146+ statespace->addDimension (-10 , 5 );
147+ ompl::base::StateSpacePtr stateSpacePtr (statespace);
148+
149+ ompl::base::HybridStateSpace *hybridSpace = new ompl::base::HybridStateSpace (stateSpacePtr);
150+ ompl::base::StateSpacePtr hybridSpacePtr (hybridSpace);
151+
152+ // Define control space
153+ ompl::control::RealVectorControlSpace *flowControlSpace = new ompl::control::RealVectorControlSpace (hybridSpacePtr, 1 );
154+ ompl::control::RealVectorControlSpace *jumpControlSpace = new ompl::control::RealVectorControlSpace (hybridSpacePtr, 1 );
155+
156+ ompl::base::RealVectorBounds flowBounds (1 );
157+ flowBounds.setLow (0 , 0 );
158+ flowBounds.setHigh (0 , 0 );
159+ flowControlSpace->setBounds (flowBounds);
160+
161+ ompl::base::RealVectorBounds jumpBounds (1 );
162+ jumpBounds.setLow (0 , 0 );
163+ jumpBounds.setHigh (0 , 5 );
164+ jumpControlSpace->setBounds (jumpBounds);
165+
166+ ompl::control::RealVectorControlUniformSampler flowControlSampler (flowControlSpace);
167+ flowControlSpace->setControlSamplerAllocator ([flowControlSpace](const ompl::control::ControlSpace *space) -> ompl::control::ControlSamplerPtr {
168+ return std::make_shared<ompl::control::RealVectorControlUniformSampler>(space);
169+ });
170+
171+ ompl::control::RealVectorControlUniformSampler jumpControlSampler (jumpControlSpace); // Doesn't do anything because the bounds for jump input are just [0, 0], but here for demonstration
172+ jumpControlSpace->setControlSamplerAllocator ([jumpControlSpace](const ompl::control::ControlSpace *space) -> ompl::control::ControlSamplerPtr {
173+ return std::make_shared<ompl::control::RealVectorControlUniformSampler>(space);
174+ });
175+
176+ ompl::control::ControlSpacePtr flowControlSpacePtr (flowControlSpace);
177+ ompl::control::ControlSpacePtr jumpControlSpacePtr (jumpControlSpace);
178+
179+ ompl::control::CompoundControlSpace *controlSpace = new ompl::control::CompoundControlSpace (hybridSpacePtr);
180+ controlSpace->addSubspace (flowControlSpacePtr);
181+ controlSpace->addSubspace (jumpControlSpacePtr);
182+
183+ ompl::control::ControlSpacePtr controlSpacePtr (controlSpace);
184+
185+ // Construct a space information instance for this state space
186+ ompl::control::SpaceInformationPtr si (new ompl::control::SpaceInformation (hybridSpacePtr, controlSpacePtr));
187+ ompl::control::ODESolverPtr odeSolver (new ompl::control::ODEBasicSolver<> (si, &flowODE));
188+
189+ si->setStatePropagator (ompl::control::ODESolver::getStatePropagator (odeSolver));
190+ si->setPropagationStepSize (0.01 );
191+ si->setup ();
192+
193+ // Set start state to be 1 unit above the floor with zero velocity and gravitaional acceleration of 9.81
194+ ompl::base::ScopedState<> start (hybridSpacePtr);
195+ start->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [0 ] = 1 ;
196+ start->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [1 ] = 0 ;
197+ start->as <ompl::base::HybridStateSpace::StateType>()->as <ompl::base::RealVectorStateSpace::StateType>(0 )->values [2 ] = -9.81 ;
198+
199+ // Set goal state to be on floor with a zero velocity, and tolerance of 0.1
200+ std::shared_ptr<EuclideanGoalRegion> goal = std::make_shared<EuclideanGoalRegion>(si);
201+
202+ // Create a problem instance
203+ ompl::base::ProblemDefinitionPtr pdef (new ompl::base::ProblemDefinition (si));
204+
205+ // Set the start and goal states
206+ pdef->addStartState (start);
207+ pdef->setGoal (goal);
208+
209+ ompl::control::HyRRT cHyRRT (si);
210+
211+ // Set the problem instance for our planner to solve
212+ cHyRRT.setProblemDefinition (pdef);
213+ cHyRRT.setup ();
214+
215+ // Set parameters
216+ cHyRRT.setDiscreteSimulator (discreteSimulator);
217+ cHyRRT.setDistanceFunction (distanceFunc);
218+ cHyRRT.setFlowSet (flowSet);
219+ cHyRRT.setJumpSet (jumpSet);
220+ cHyRRT.setTm (0.5 );
221+ cHyRRT.setFlowStepDuration (0.01 );
222+ cHyRRT.setUnsafeSet (unsafeSet);
223+
224+ // attempt to solve the planning problem within 2 seconds
225+ ompl::time::point t0 = ompl::time::now ();
226+ ompl::base::PlannerStatus solved = cHyRRT.solve (ompl::base::timedPlannerTerminationCondition (2 ));
227+ double planTime = ompl::time::seconds (ompl::time::now () - t0);
228+
229+ if (solved) // If either approximate or exact solution has beenf ound
230+ OMPL_INFORM (" Solution found in %f seconds"
231+ OMPL_INFORM (" Solution status: %s" asString ().c_str ());
232+
233+ // print path
234+ pdef->getSolutionPath ()->as <ompl::control::PathControl>()->printAsMatrix (std::cout);
235+ }
0 commit comments