Dynamic Planning / Optimal Control ECE 383 / ME 442

Context: Model-based control Model-free control: choose u(x,t), avoid modeling dynamics (ex: PID control) Good for simple problems Low computational complexity Requires parameter tuning Difficult to choose suitable policy for complex systems Has no performance guarantees Model-based control: model dynamics and choose u to yield a “good” future trajectory. (ex: gravity compensation, optimal control) Able to achieve better performance (e.g., optimality) Dynamics may be hard to model Must usually be combined with model-free control or model adaptation to compensate for un-modeled errors (implementation difficulty)

Context: Myopic vs Predictive A model-based controller is myopic (or greedy) if the decisions 𝑢(𝑥,𝑡) only depend on predicting 𝑥 =𝑓(𝑥,𝑢) at the current state 𝑥. Ex: gravity compensation tells you how to compensate for gravity now Ex: potential fields Ex: operational space control A model-based controller is predictive if the decisions 𝑢(𝑥,𝑡) depend on both the current state and predictions of the future trajectory taken under the time-evolution of dynamics, under the controls Ex: motion planning Ex: optimal control For most non-trivial problems, good myopic strategies are hard to devise

Agenda (2 day) Today Next time Principles Ch 7.2 Concept of model predictive control Controllability Kinematic reductions & maneuvers in motion planning Sampling-based planning with dynamics Next time Principles Ch 8 Optimal control Trajectory optimization Bellman equation

Model predictive control (MPC) Idea: repeatedly compute feedforward control using model of the dynamics, prediction In other words, rapid replanning Feedforward calculation u=uff Plant xdes x(t)

Kinematic Planning So far, all constraints on motion have been represented as C-space obstacles Dynamic constraints: omnidirectional motion in C-space not permitted

Kinematic reductions Can we plan kinematically-feasible paths and then convert them to dynamically-feasible ones? Sometimes Kinematic reduction

Holonomic systems If m>=n, and for all x, all dimensions of 𝑥 =𝑓(𝑥,𝑢) can be spanned by a suitably chosen control u, then the system is holonomic I.e., 𝜕𝑓 𝜕𝑢 (𝑥,𝑢) is full rank at all x E.g., letting 𝑥 = 𝑓 0 𝑥 + 𝑓 1 𝑥 𝑢 1 +…+ 𝑓 𝑚 𝑥 𝑢 𝑚 need [ 𝑓 1 𝑥 |…| 𝑓 𝑚 𝑥 ] to be full rank for all x Otherwise the system is non-holonomic Omnidirectional mobile base: holonomic Differential drive: non-holonomic

Planning for Holonomic Systems = Kinematic planning Given any kinematic path x(t), can choose 𝑢(𝑡) at 𝑥(𝑡) to derive path velocity 𝑥 (𝑡) Given bounds on control 𝑢∈𝑈, can find the control in 𝑈 that maximizes forward progression along path Most interesting dynamic systems are nonholonomic… but maybe we can do this approximately

Sea horses By: Saya h.

Path Deformation Holonomic path Nonholonomic path 11

Example: 1D Point Mass x v dx/dt = dv dv/dt = f / M Solution: v(t) = v(0)+t f /M x(t) = x(0)+t v(0) + ½ t2 f / M |f|<=fmax x

Example: Car-Like Robot y x L q f dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F dx sinq – dy cosq = 0 Configuration space is 3-dimensional: q = (x, y, q) But control space is 2-dimensional: (v, f) with |v| = sqrt[(dx/dt)2+(dy/dt)2] 13

Example: Car-Like Robot y x L q f dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F dx sinq – dy cosq = 0 q = (x,y,q) q’= dq/dt = (dx/dt,dy/dt,dq/dt) dx sinq – dy cosq = 0 is a particular form of f(q,q’)=0 A robot is nonholonomic if its motion is constrained by a non-integrable equation of the form f(q,q’) = 0 14

Example: Car-Like Robot y x L q f dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F dx sinq – dy cosq = 0 Lower-bounded turning radius 15

How Can This Work? Tangent Space/Velocity Space x L q f x y q (x,y,q) (dx,dy,dq) (dx,dy) dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F q 16

How Can This Work? Tangent Space/Velocity Space x L q f x y q (x,y,q) (dx,dy,dq) (dx,dy) dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F q 17

Kinematic Reduction of 1D Point Mass If: Constraints are time-invariant Starts and stops at rest Then we only need to consider position x1 x1+e 1. Speed up 2. Slow down

Dynamic Execution of Kinematic Paths For fully actuated N-D robots, a collision-free path can be executed “slowly enough” v x

Optimal Straight-line Following Given a particle with velocity and acceleration bounds, what is the optimal motion between two points, starting and stopping at rest?

Optimal Straight-line Following Given a particle with velocity and acceleration bounds, what is the optimal motion between two points, starting and stopping at rest? Max acceleration, (optional) max velocity, max deceleration Trapezoidal velocity profile x a- v+ a+ t v vmax t

Optimization of Dynamic Paths for Robot Arms Hauser and Ng Thow Hing (2010)

Small-Time Local Controllability A system is small-time locally controllable if at a given configuration q, it can reach an open neighborhood of q with “small” motions e q SLTC Not SLTC Not SLTC

Small-Time Local Controllability A system is small-time locally controllable if at a given configuration q, it can reach an open neighborhood of q with “small” motions Surprise: simple car model is STLC e q SLTC Not SLTC Not SLTC

Lie Bracket Maneuver made of 4 motions -X -Y Y X (dt) Assuming this is just a repetition of the first presentation, otherwise, need more details… 25

Lie Bracket Maneuver made of 4 motions For example: dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F X: Going straight Y: Turning, angle f T Assuming this is just a repetition of the first presentation, otherwise, need more details… 26

Lie Bracket Maneuver made of 4 motions For example: -X X (dt) Y -X -Y [X,Y] (dt2 ) X: Going straight Y: Turning, angle f T Assuming this is just a repetition of the first presentation, otherwise, need more details… Lie bracket 27

Lie Bracket [X,Y] = dY.X – dX.Y X1/x X1/y X1/q dX = X2/x X2/y X2/q X3/x X3/y X3/q X (dt) Y -X -Y [X,Y] (dt2 ) Assuming this is just a repetition of the first presentation, otherwise, need more details… [X,Y]  Lin(X,Y)  the motion constraint is nonholonomic Lie bracket 28

Lie Bracket -X -Y Y X (dt) [X,Y] (dt2 ) [X,Y] = dY.X – dX.Y = 0 0 -v sin θ 0 0 v cos θ 0 0 0 0 0 -v sin θ 0 0 v cos θ 0 0 0 dX = dY = X (dt) Y -X -Y [X,Y] (dt2 ) X: Going straight Y: Turning, angle f T Assuming this is just a repetition of the first presentation, otherwise, need more details… v2/L sin θ tan φ -v2/L cos θ tan φ [X,Y] = dY.X – dX.Y = 29

Sufficient condition for STLC Let S = {Xi}, i=1,…,k be control vector fields over n dimensional manifold For each pair Xi,Xj in S: Compute lie bracket U = [Xi,Xj] If U  Lin(S), then add U to S and repeat step 2 If |S|=n, then STLC

Tractor-Trailer Example 4-D configuration space 2-D control/velocity space  two independent velocity vectors X and Y U = [X,Y]  Lin(X,Y) V = [X,U]  Lin(X,Y,U) 31

Lie Bracket Maneuvers Problem: To move distance d along vector field U=[X,Y] using Lie bracket, may need to move much farther in C-space Solution: Use better maneuvers Assuming this is just a repetition of the first presentation, otherwise, need more details… 32

Type 1 Maneuver CYL(x,y,dq,h) h = 2r tandq d = 2r(1/cosdq - 1) > 0 (x,y,q) dq h h (x,y) q r dq dq r When dq  0, so does d and the cylinder becomes arbitrarily small  Allows sideways motion 33

Type 2 Maneuver  Allows pure rotation 34

Combination 35

Coverage of a Path by Cylinders q + q q’ y x 36

Path Examples 37

Drawbacks of Two-phase Planning Final path can be far from optimal Not applicable to robots that are not locally controllable (e.g., car that can only move forward) 38

Reeds and Shepp Paths 39

Reeds and Shepp Paths CC|C0 CC|C C|CS0C|C Given any two configurations, the shortest RS paths between them is also the shortest path 40

Example of Generated Path Holonomic Nonholonomic 41

Path Optimization 42

Perspective If one can determine kinematic reductions or steering functions, usually the best method is to reduce to kinematic planning + postprocessing If these cannot be found, must resort to the approach of control-based planning in state space

Control-Based Sampling PRM sampling technique: Pick each milestone in some region Control-based sampling: Pick control vector (at random or not) Integrate equation of motion over short duration (picked at random or not) If the motion is collision-free, then the endpoint is the new milestone Tree-structured roadmaps Need for endgame regions 44

Example dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F 1. Select a milestone m 2. Pick v, f, and dt 3. Integrate motion from m  new milestone m’ 45

Example Indexing array: A 3-D grid is placed over the configuration space. Each milestone falls into one cell of the grid. A maximum number of milestones is allowed in each cell (e.g., 2 or 3). Asymptotic completeness: If a path exists, the planner is guaranteed to find one if the resolution of the grid is fine enough. 46

Computed Paths Tractor-trailer Car That Can Only Turn Left jmax=45o, jmin=22.5o jmax=45o 47

Control-Sampling RRT Configuration generator f(x,u) Build a tree T of configurations Extend: Sample a configuration xrand from X at random Find the node xnear in T that is closest to xrand Pick a control u that brings f(xnear,u) close to xrand Add f(xnear,u) as a child of xnear in T

Weaknesses of RRT’s strategy Depends on the domain from which xrand is sampled (sampling strategy) Depends on the notion of “closest” (metric sensitivity) A tree that is grown “badly” by accident can greatly slow convergence (history dependence)

Next time Considering optimality Principles Ch 8