R3T Optimal Kinodynamic Planning of Nonlinear Hybrid Systems

We introduce R3T a reachability-based variant of the rapidly-exploring random tree algorithm that is suitable for kinodynamic planning in nonlinear hybrid systems The R3T algorithm is similar to RRT except all operations are performed in terms of reachable sets The tree maintains the local reachable sets for each state During extension a sample is drawn from the state space The closest reachable set to the sample is identified the sample is projected onto the set and the tree is extended to the projection The reachable set of the new state is then calculated For asymptotic optimality a rewiring procedure similar to RRT* is performed First, potentially better parents for the new node are checked If the new node is in any other reachable set the parent with the lowest cost-to-go is selected and assigned as the new node’s parent Next, any node contained by the new node’s reachable set is identified The new node is compared against the original parent and the one with the smallest cost-to-go is selected In the subsequent examples, we consider the first path found by R3T so optimality is not guaranteed First, we demonstrate R3T planning with a pendulum swing-up problem Here, the reachable set explored by R3T is shown in phase space As time progresses, R3T exploration covers more and more parts of the state space eventually finding a path to the solution Due to its explicit representation of reachable states when run for a long time R3T can be used for formal reachability verification up to linearization errors To demonstrate R3T’s ability to plan on high dimensional hybrid systems we tested the algorithm with path planning on a 2D hopping robot The robot has 10 states and 2 dynamic modes, contact and flight In this setup the robot has to hop horizontally for 10 meters R3T was used to plan the hip torque and the push-off force on ascending during ground contact R3T was able to produce a path within a median of 106 seconds over 10 consecutive trials R3T is particularly well suited for hybrid systems with many modes Reachability guides the extension algorithm to discover mode sequences that lead to rapid state space explorations In this example we applied R3T on a 2D box manipulation problem A finger from the right is allowed to make different modes of contact with the right side of the box Here, we show the reachable set explored by R3T over time In our model 64 contact modes are possible but usually 4 to 25 are present in a given state Here, we show animations of the box being flipped and pushed from the same initial state using R3T We have found trajectory synthesis using R3T to be orders of magnitude faster than mixed-integer optimization when all possible contact modes are taken into account showing that R3T is a promising method for kinodynamic planning in nonlinear hybrid systems

Be First to Comment

Leave a Reply

Your email address will not be published. Required fields are marked *