CS 326A: Motion PlanningKinodynamic Planning ProblemProblemNonholonomic vs. Dynamic Constraints Similarity of MethodExample: Space RobotNavigation Among Moving ObstaclesModel, Sensing, and ControlSlide 9Moving Obstacles in ConfigurationxTime SpaceTraditional PRMKinodynamic Planning with PRM in State x Time SpacePRM in State x Time SpaceSampling StrategyBi-Directional Search: Forward & Backward IntegrationExample RunOther ExamplesBut executing this trajectory is likely to fail ...Slide 19Dealing with UncertaintySlide 21Slide 22Dealing with Planning TimePlanning an Escape TrajectoryAre we done?Slide 26Experimental Run with Re-PlanningSlide 28Another Experimental RunSlide 30Example in SimulationIs this guaranteed to work?CS 326A: Motion PlanningKynodynamic Planning+ Dealing with Moving Obstacles+ Dealing with Uncertainty+ Dealing with Real-Time Issues2Kinodynamic Planning ProblemPlan a robot’s trajectory that satisfies a dynamic equation of motion of the form: s’ = f(s,u)where:–s is robot’s state configuration x velocity at time t–u is control input at time tavoids collision with moving obstaclesOutcome: (Piecewise-constant) control function u(t)3ProblemPlan a robot’s trajectory that satisfies a dynamic motion constraint of the form: s’ = f(s,u)where:–s is robot’s state configuration x velocity at time t–u is control input at time tavoids collision with moving obstaclesOutcome: (Piecewise-constant) control function u(t)Note similarity with non-holonomic constraintq’ = f(q,u)Nonholonomic vs. Dynamic ConstraintsNonholonomic constraint: f(q,q’) = 0Dynamic constraints: f(q,q’,q’’) = 0 s = (q,q’)  g(s,s’) = 0Kinodynamic planning is done in state (configuration x velocity) space rather than in configuration spaceThe dimensionality of state space is twice that of configuration space5 Similarity of MethodConstruction of a tree, by integrating equation of motion with selected control inputsHowever, planning occurs in state space, instead of configuration spaceHence, PRM-like planner rather than deterministic one, to deal with greater dimensionality of the space6air bearinggas tankair thrustersRobot created to study issues in robot control and planning in no-gravityspace environmentExample: Space Robot7Navigation Among Moving Obstacles8Model, Sensing, and Control The robot and the obstacles are represented as disks moving in the planeThe position and velocity of each disc are measured by an overhead camera every 1/30 secxxyrobotobstacles9Model, Sensing, and Control The robot and the obstacles are represented as disks moving in the planeThe position and velocity of each disc are measured by an overhead camera within 1/30 secThe robot controls the magnitude f and the orientation  of the total pushing force exerted by the thrustersxxyfrobotobstaclesaaa�Mq=(x,y)s=(q,q')u=(f , )1x"= f cosm1y"= f sinmfxytMoving Obstacles in ConfigurationxTime Spacestraight line segmentsTraditional PRM12sgKinodynamic Planning with PRM in State x Time SpaceThe roadmap is a tree oriented along the time axis (because of the moving obstacles)Bi-directional search?t=1t=2t=3t=2t=1t=4t=2t=3t=4t=5t=313sgPRM in State x Time SpaceThe roadmap is a tree oriented along the time axisendgame regionCompare!endgame regionSampling Strategy111211112Goal RegionBi-Directional Search: Forward & Backward Integration16Example RuntxyObstacle map to cylinders in configurationtime space17Other Examples18But executing this trajectory is likely to fail ...1) The measured velocities of the obstacles are inaccurate 2) Tiny particles of dust on the table afect trajectories and contribute further to deviation Obstacles are likely to deviate from their expected trajectories3) Planning takes time, and during this time, obstacles keep moving The computed robot trajectory is not properly synchronized with those of the obstacles The robot may hit an obstacle before reaching its goal[Robot control is not perfect but “good” enough for the task]19But executing this trajectory is likely to fail ...1) The measured velocities of the obstacles are inaccurate 2) Tiny particles of dust on the table afect trajectories and contribute further to deviation Obstacles are likely to deviate from their expected trajectories3) Planning takes time, and during this time, obstacles are moving The computed robot trajectory is not properly synchronized with those of the obstacles The robot may hit an obstacle before reaching its goal[Robot control is not perfect but “good” enough for the task]Planning must take both uncertainty in world state and time constraints into account20Dealing with UncertaintyThe robot can handle uncertainty in an obstacle position by representing the set of all positions of the obstacle that the robot think possible at each timeFor example, this set can be a disc whose radius grows linearly with timet = 0 t = T t = 2TInitial set ofpossible positionsSet of possible positions at time 2TSet of possible positions at time T21Dealing with UncertaintyThe robot can handle uncertainty in an obstacle position by representing the set of all positions of the obstacle that the robot think possible at each timeFor example, this set can be a disc whose radius grows linearly with timet = 0 t = T t = 2TThe robot must plan to be outside this disc at time t = T22Dealing with UncertaintyThe robot can handle uncertainty in an obstacle position by representing the set of all positions of the obstacle that the robot think possible at each time (belief state)For example, this set can be a disc whose radius grows linearly with timeThe forbidden regions in configurationtime space are cones, instead of cylindersThe trajectory planning method remains essentially unchanged23Dealing with Planning Time Let t=0 the time when planning starts. A time limit  is given to the plannerThe planner computes the states that will be possible at t  and use them as the possible initial statesIt returns a trajectory at some t , whose execution will start at t  Since the PRM planner isn’t absolutely guaranteed to find a solution within , it computes two trajectories using the same roadmap: one to the goal, the other to any position where the robot will be safe for at least an additional . Since