Robotics

Dynamics & Non-holonomic Systems

1D point mass, damping & oscillation, PID, general dynamic systems, Newton-Euler, joint space control, reference trajectory following, optimal operational space control, car system equation, path-nding for non-holonomic systems, control-based sampling, Dubins curves

Marc Toussaint FU Berlin

 So far we always assumed we could arbitrarily set the next robot state qt+1 (or an arbitrary velocity q t ). What if we cant? Examples: An air plane ying: You cannot command it to hold still in the air, or to move straight up. A car: you cannot command it to move side-wards. Your arm: you cannot command it to throw a ball with arbitrary speed (force limits). A torque controlled robot: You cannot command it to instantly change velocity (innite acceleration/torque). What all examples have in comment: One can set controls ut (air planes control stick, cars steering wheel, your muscles activations, torque/voltage/current send to a robots motors) But these controls only indirectly inuence the dynamics of state, xt+1 = f (xt , ut )
2/42

Dynamics
The dynamics of a system describes how the controls ut inuence the change-of-state of the system xt+1 = f (xt , ut ) The notation xt refers to the dynamic state of the system: e.g., joint positions and velocities xt = (qt , q t ). f is an arbitrary function, often smooth

3/42

Dynamics
The dynamics of a system describes how the controls ut inuence the change-of-state of the system xt+1 = f (xt , ut ) The notation xt refers to the dynamic state of the system: e.g., joint positions and velocities xt = (qt , q t ). f is an arbitrary function, often smooth We dene a nonholonomic system as one with differential constraints: dim(ut ) < dim(xt ) Not all degrees of freedom are directly controllable

3/42

Outline
We discuss three basic examples: 1D point mass A general dynamic robot A non-holonomic car model We discuss how previous methods can be extended to the dynamic/non-holonomic case: inverse kinematics control optimal operational space control trajectory optimization trajectory optimization RRTs RRTs with special tricks

4/42

The dynamics of a 1D point mass

Start with simplest possible example: 1D point mass (no gravity, no friction, just a single mass)

State x(t) = (q (t), q (t)) is described by: position q (t) R velocity q (t) R The controls u(t) is the force we apply on the mass point The system dynamics is: q (t) = u(t)/m
5/42

1D point mass proportional feedback

Assume current position is q . The goal is to move it to the position q . What can we do?

6/42

1D point mass proportional feedback

Assume current position is q . The goal is to move it to the position q . What can we do? IDEA 1 Always pull the mass towards the goal q : u = K p (q q )

6/42

1D point mass proportional feedback

Whats the effect of IDEA 1? mq = u = Kp (q q ) q = q (t) is a function of time, this is a second order differential equation

7/42

1D point mass proportional feedback

Whats the effect of IDEA 1? mq = u = Kp (q q ) q = q (t) is a function of time, this is a second order differential equation Solution: assume q (t) = a + bet (an non-imaginary alternative would be q (t) = a + b
t

cos(t))

7/42

1D point mass proportional feedback

Whats the effect of IDEA 1? mq = u = Kp (q q ) q = q (t) is a function of time, this is a second order differential equation Solution: assume q (t) = a + bet (an non-imaginary alternative would be q (t) = a + b m b 2 et = Kp q Kp a Kp b et (m b 2 + Kp b) et = Kp (q a) (m b 2 + Kp b) = 0 (q a) = 0 Kp /m q (t) = q + b ei Kp /m t This is an oscillation around q with amplitude b = q (0) q and frequency Kp /m! =i
t

cos(t))

7/42

1D point mass proportional feedback

mq = u = Kp (q q ) q (t) = q + b ei Kp /m t Oscillation around q with amplitude b = q (0) q and frequency Kp /m

1 0.5 0 -0.5 -1 0 2 4 6 8 10 12

cos(x)

14

8/42

1D point mass derivative feedback

That didnt solve the problem! IDEA 2 Pull less, when were heading the right direction already: Damp the system: u = Kp (q q ) + Kd (q q ) q is a desired goal velocity For simplicity we set q = 0 in the following.

9/42

1D point mass derivative feedback

Whats the effect of IDEA 2? mq = u = Kp (q q ) + Kd (0 q ) Solution: again assume q (t) = a + bet m b 2 et = Kp q Kp a Kp b et Kd b et (m b 2 + Kd b + Kp b) et = Kp (q a) (m 2 + Kd + Kp ) = 0 (q a) = 0 = Kd
t 2 4mK Kd p 2m

q (t) = q + b e

Kd The term 2 m in is real exponential decay (damping)

10/42

1D point mass derivative feedback

q (t) = q + b e t , =
K d

2 4mK Kd p 2m

Effect of the second term

2 Kd < 4mKp

2 4mK /2m in : Kd p

has imaginary part 2 2 oscillating with frequency Kp /m Kd /4m q (t) = q + beKd /2m t ei
2 /4m2 t Kp /mKd

2 Kd > 4mKp

real strongly damped second term zero only exponential decay

2 = 4mKp Kd

11/42

1D point mass derivative feedback

illustration from O. Brocks lecture

12/42

1D point mass derivative feedback

Alternative parameterization: Instead of the gains Kp and Kd it is sometimes more intuitive to set the wave length =
1 0

1 Kp /m

, ,

Kp = m/2 Kd = 2m/

damping ratio = Kd

4mKp

Kd 2m

> 1: over-damped = 1: critically dampled < 1: oscillatory-damped q (t) = q + be/ t ei0

1 2 t

13/42

1D point mass integral feedback

IDEA 3 Pull if the position error accumulated large in the past:
t

u = Kp (q q ) + Kd (q q ) + Ki
s=0

(q (s) q (s)) ds

This is not a linear control law (not linear w.r.t. (q, q )) Analytically solving the euqation is hard! (no explicit discussion here)

14/42

1D point mass PID control

t

u = Kp (q q ) + Kd (q q ) + Ki
s=0

(q q (s)) ds

PID control Proportional Control (Position Control) f Kp (q q ) Derivative Control (Damping) f Kd (q q ) (x = 0 damping) Integral Control (Steady State Error) t f Ki s=0 (q (s) q (s)) ds
15/42

Controlling a 1D point mass lessons learnt

Proportional and derivative feedback (PD control) are like adding a spring and damper to the point mass. PD control is a linear control law q ) : (q, q ) u = Kp (q q ) + Kd (q (linear in the dynamic system state x = (q, q )) With such linear control laws we can design approach trajectories (by tuning the gains). but no optimality principle behind such motions yet

16/42

Controlling a 1D point mass lessons learnt

The point mass is a basic example for a non-holonomic system: The state is x = (q, q ) R2 The control u R is force the q q q = f (u, ) = The dynamics are u/m q q

17/42

General robot system dynamics

State x = (q, q ) R2n joint positions q Rn joint velocities q Rn Controls u Rn are the torques generated in each motor. The system dynamics are: M (q ) q + C (q, q ) q + G(q ) = u M (q ) Rnn C (q, q ) Rn G(q ) Rn u More compact: M (q ) q + F (q, q ) = u
18/42

is positive denite intertia matrix (can be inverted forward simulation of dynamics) are the centripetal and coriolis forces are the gravitational forces are the joint torques

Computing M and F
M (q ) q + F (q, q ) = u Recall: We have an algorithm to compute all positions and orientations given q : TW i (q ) = TW A TAA (q ) TA B TB B (q ) TB C TC C (q ) TC i

19/42

Computing M and F
M (q ) q + F (q, q ) = u Recall: We have an algorithm to compute all positions and orientations given q : TW i (q ) = TW A TAA (q ) TA B TB B (q ) TB C TC C (q ) TC i This can be extended easily to compute also: linear velocities vi angular velocities wi linear accelerations v i angular accelerations w i for any link i in the robot (via forward propagation; see geometry notes for details).

19/42

Newton-Euler (roughly)
focussing at the ith link: Newtons equation expresses the force acting at the center of mass for an accelerated body: fi = mv i Eulers equation expresses the torque acting on a rigid body given an angular velocity and angular acceleration: ui = Ii w + w Iw Idea of Newton-Euler algorithm Force and torque balance at every link Recursion through all links

20/42

Newton-Euler (roughly)
focussing at the ith link: Newtons equation expresses the force acting at the center of mass for an accelerated body: fi = mv i Eulers equation expresses the torque acting on a rigid body given an angular velocity and angular acceleration: ui = Ii w + w Iw Idea of Newton-Euler algorithm Force and torque balance at every link Recursion through all links Bottom line: There exist algorithms to efciently compute M (q ) and F (q, q ) for any dynamic state (q, q ).
20/42

Controlling a general robot joint space approach

If we know the desired q for each joint, the eqn. M (q ) q + F (q, q ) = u gives the desired torques.

21/42

Controlling a general robot joint space approach

If we know the desired q for each joint, the eqn. M (q ) q + F (q, q ) = u gives the desired torques. Where could we get the desired q from? ref Assume we have a nice smooth reference trajectory q0: T (generated with some motion prole or alike), we can at each t read off the desired acceleration as 1 ref q t := [(qt+1 qt )/ (qt qt-1 )/ ] = (qt-1 + qt+1 2qt )/ 2 However, tiny errors in acceleration will accumulate greatly over time! This is Instable!!

21/42

Controlling a general robot joint space approach

If we know the desired q for each joint, the eqn. M (q ) q + F (q, q ) = u gives the desired torques. Where could we get the desired q from? ref Assume we have a nice smooth reference trajectory q0: T (generated with some motion prole or alike), we can at each t read off the desired acceleration as 1 ref q t := [(qt+1 qt )/ (qt qt-1 )/ ] = (qt-1 + qt+1 2qt )/ 2 However, tiny errors in acceleration will accumulate greatly over time! This is Instable!! Choose a desired acceleration q t that implies a PD-like behavior around the reference trajectory!
ref ref ref q t =q t + Kp (qt qt ) + Kd (q t q t )

This is a standard and very convenient heuristic to track a reference trajectory when the robot dynamics are known: All joints will exactly behave like a 1D point particle around the reference trajectory! 21/42

Controlling a robot operational space approach

Recall the inverse kinematics problem: We know the desired step y (or velocity y ) of the endeffector. Which step q (or velocities q ) should we make in the joints? Equivalent dynamic problem: We know how the desired acceleration y of the endeffector. What controls u should we apply?

22/42

Optimal operational space control

Given current state (qt , q t ) and yt = (qt ), y t = J q t given desired y compute a torque u such that 1) 2) ||u||2 is small y is close to y

be lazy accelerate endeector

We translate this to the objective function:

-1 y f (u) = ||u||2 ||2 H + ||JM (u F ) + J q C

note: y =

d y dt

d (J q ) dt

q q = Jq + J = JM -1 (u F ) + J

23/42

Optimal operational space control

Optimum: (analogous derivation to kinematic case) q u = T ( y J + TF) with T = JM -1 , T = (T CT + H )-1 T C

(C T = H -1 T (T H -1 T )-1 ) This generalizes the good old q =J y to the dynamic case, taking account also of intrinsic forces (coriolis and . gravity) and J

24/42

Controlling a robot operational space approach

Where could we get the desired y from? ref Reference trajectory y0:T in operational space! PD-like behavior in each operational space: ref ref ref y t =y t + Kp (yt yt ) + Kd (y t y t )

25/42

Controlling a robot operational space approach

Where could we get the desired y from? ref Reference trajectory y0:T in operational space! PD-like behavior in each operational space: ref ref ref y t =y t + Kp (yt yt ) + Kd (y t y t )

Operational space control: Let the system behave as if we could directly apply 1D point mass behavior to the endeffector.

25/42

3rd example: A car

Note: The previous examples were dynamic systems in the sense x = (q, q ) The car example is just kinematic (no mass/inertia/forces) But the treatment is very much the same because a car is also a non-holonomic system! (Just like a car cannot instantly move sidewards, a dynamic system cannot instantly change its position q : the current change in position is constrained by the current velocity q .)

26/42

Car example

x = v cos y = v sin = (v/L) tan || <

x State q = y

v Controls u =

Sytem equation

x v cos y v sin = (v/L) tan

27/42

Car example
The car is a non-holonomic system: not all DoFs are controlled, dim(u) < dim(q ) We have the differential constraint q : x sin y cos = 0 A car cannot move directly lateral!

28/42

Car example
The car is a non-holonomic system: not all DoFs are controlled, dim(u) < dim(q ) We have the differential constraint q : x sin y cos = 0 A car cannot move directly lateral!
General denition of a differential constraint: For any given state x, let Ux be the tangent space that is generated by controls: Ux = {x : x = f (x, u), u U } (non-holonomic dim(Ux ) < dim(x)) The elements of Ux are elements of Tx subject to additional differential constraints.
28/42

Path nding with a non-holonomic system

Could a car follow this trajectory?

This is generated with a PRM in the state space q = (x, y, ) ignoring the differntial constraint.

29/42

Path nding with a non-holonomic system

This is a solution we would like to have:

The path respects differential constraints. Each step in the path corresponds to setting certain controls.

30/42

Path nding with a non-holonomic system

Control-based sampling: fulls none of the nice exploration properties of RRTs but fulls the differential constraints: 1) Select a q T from tree of current congurations 2) Pick control vector u at random 3) Integrate equation of motion over short duration (picked at random or not) 4) If the motion is collision-free, add the endpoint to the tree

31/42

Control-based sampling for the car

1) Select a q T 2) Pick v , , and 3) Integrate motion from q 4) Add result if collision-free

32/42

J. Barraquand and J.C. Latombe. Nonholonomic Multibody Robots: Controllability and Motion Planning in the Presence of Obstacles. Algorithmica, 10:121-155, 1993.

car parking

33/42

car parking
34/42

parking with only left-steering

35/42

with a trailer
36/42

Better control-based exploration: RRTs revisited

RRTs with differential constraints: Input: qstart , number k of nodes, time interval Output: tree T = (V, E ) 1: initialize V = {qstart }, E = 2: for i = 0 : k do 3: qtarget random sample from Q 4: qnear nearest neighbor of qtarget in V 5: use local planner to compute controls u that steer qnear towards qtarget 6: qnew qnear + t=0 q (q, u)dt 7: if qnew Qfree then V V {qnew }, E E {(qnear , qnew )} 8: end for Crucial questions: How meassure near in nonholonomic systems? How nd controls u to steer towards target?

37/42

Metrics
Standard/Naive metrics: Comparing two 2D rotations/orientations 1 , 2 SO(2): a) Euclidean metric between ei1 and ei2 b) d(1 , 2 ) = min{|1 2 |, 2 |1 2 |} Comparing two congurations (x, y, )1,2 in R2 : Eucledian metric on (x, y, ei ) Comparing two 3D rotations/orientations r1 , r2 SO(3): Represent both orientations as unit-length quaternions r1 , r2 R4 : d(r1 , d2 ) = min{|r1 r2 |, |r1 + r2 |} where | | is the Euclidean metric. (Recall that r1 and r1 represent exactly the same rotation.)

38/42

Metrics
Standard/Naive metrics: Comparing two 2D rotations/orientations 1 , 2 SO(2): a) Euclidean metric between ei1 and ei2 b) d(1 , 2 ) = min{|1 2 |, 2 |1 2 |} Comparing two congurations (x, y, )1,2 in R2 : Eucledian metric on (x, y, ei ) Comparing two 3D rotations/orientations r1 , r2 SO(3): Represent both orientations as unit-length quaternions r1 , r2 R4 : d(r1 , d2 ) = min{|r1 r2 |, |r1 + r2 |} where | | is the Euclidean metric. (Recall that r1 and r1 represent exactly the same rotation.)

Ideal metric: Optimal cost-to-go between two states x1 and x2 : Use optimal trajectory cost as metric This is as hard to compute as the original problem, of course!! Approximate, e.g., by neglecting obstacles.

38/42

Dubins curves
Dubins car: constant velocity, and steer [, ] Neglecting obstacles, there are only six types of trajectories that connect any conguration R2 S1 : {LRL, RLR, LSL, LSR, RSL, RSR} annotating durations of each phase: {L R L , , R L R , L Sd L , L Sd R , R Sd L , R Sd R } with [0, 2 ), (, 2 ), d 0

39/42

Dubins curves

By testing all six types of trajectories for (q1 , q2 ) we can dene a Dubins metric for the RRT and use the Dubins curves as controls!

40/42

Dubins curves

By testing all six types of trajectories for (q1 , q2 ) we can dene a Dubins metric for the RRT and use the Dubins curves as controls! Reeds-Shepp curves are an extension for cars which can drive back. (includes 46 types of trajectories, good metric for use in RRTs for cars)

40/42

Summary
We discuss 3 examples for dynamic/non-holonomic systems: 1D point mass We learnt about PID and the corresponding damped/oscilatory trajectories General robot dynamics M (q ) q + C (q, q ) q + G(q ) = u We learnt how to follow a reference trajectory q0:T (joint space approach) We learnt about optimal operational space control and following a task space trajectory y0:T (operational space control) A car We learnt about path nding under differential constraints This generalizes also to path nding for dynamic robots (see LaValles Planning Algorithms)

41/42

Summary
We discuss 3 examples for dynamic/non-holonomic systems: 1D point mass We learnt about PID and the corresponding damped/oscilatory trajectories General robot dynamics M (q ) q + C (q, q ) q + G(q ) = u We learnt how to follow a reference trajectory q0:T (joint space approach) We learnt about optimal operational space control and following a task space trajectory y0:T (operational space control) A car We learnt about path nding under differential constraints This generalizes also to path nding for dynamic robots (see LaValles Planning Algorithms) We did not talk about trajectory optimization. But the general principle is obvious from slide 23: f (u0:T ) = t ||ut ||2 H + t (xt ) t (xt ) where xt depends on the controls u0:t-1 , and t (xt ) can be any task vector containing positional (as in the kinematic case) or velocity-type task error.
41/42

Summary
State general notation Kinematic control 1D Point mass General dynamic system Car x x=q q x= q q x= q x q = y

Controls u u=q u = force u = torque

Dynamics x = f (x, u) q = u (can be set) q x = u/m q x = M -1 (u F ) v cos q = v sin (v/L) tan

v u=

Only kinematic control has dim(u) = dim(x), all others dim(u) < dim(x)
42/42