dynamics & non-holonomic systems

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-finding 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 q t+1 (or an arbitrary velocity ˙q t ). What if we can’t? • Examples: – An air plane flying: 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 (infinite acceleration/torque). • What all examples have in comment: – One can set controls u t (air plane’s control stick, car’s steering wheel, your muscles activations, torque/voltage/current send to a robot’s motors) – But these controls only indirectly influence the dynamics of state, x t+1 = f(x t , u t ) 2/42 Dynamics • The dynamics of a system describes how the controls u t influence the change-of-state of the system x t+1 = f(x t , u t ) – The notation x t refers to the dynamic state of the system: e.g., joint positions and velocities x t = (q t , ˙q t ). – f is an arbitrary function, often smooth • We define a nonholonomic system as one with differential constraints: dim(u t ) < dim(x t ) ⇒ Not all degrees of freedom are directly controllable 3/42 Dynamics • The dynamics of a system describes how the controls u t influence the change-of-state of the system x t+1 = f(x t , u t ) – The notation x t refers to the dynamic state of the system: e.g., joint positions and velocities x t = (q t , ˙q t ). – f is an arbitrary function, often smooth • We define a nonholonomic system as one with differential constraints: dim(u t ) < dim(x t ) ⇒ 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? • IDEA 1 “Always pull the mass towards the goal q ∗ :” u = K p (q ∗ − q) 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 • What’s the effect of IDEA 1? m ¨q = u = K p (q ∗ − q) q = q(t) is a function of time, this is a second order differential equation • Solution: assume q(t) = a + be ωt (an “non-imaginary” alternative would be q(t) = a + b  −λt cos(ωt)) m b ω 2 e ωt = K p q ∗ − K p a − K p b e ωt (m b ω 2 + K p b) e ωt = K p (q ∗ − a) ⇒ (m b ω 2 + K p b) = 0 ∧ (q ∗ − a) = 0 ⇒ ω = i  K p /m q(t) = q ∗ + b e i √ K p /m t This is an oscillation around q ∗ with amplitude b = q(0) − q ∗ and frequency  K p /m! 7/42 1D point mass – proportional feedback • What’s the effect of IDEA 1? m ¨q = u = K p (q ∗ − q) q = q(t) is a function of time, this is a second order differential equation • Solution: assume q(t) = a + be ωt (an “non-imaginary” alternative would be q(t) = a + b  −λt cos(ωt)) m b ω 2 e ωt = K p q ∗ − K p a − K p b e ωt (m b ω 2 + K p b) e ωt = K p (q ∗ − a) ⇒ (m b ω 2 + K p b) = 0 ∧ (q ∗ − a) = 0 ⇒ ω = i  K p /m q(t) = q ∗ + b e i √ K p /m t This is an oscillation around q ∗ with amplitude b = q(0) − q ∗ and frequency  K p /m! 7/42 [...]... example for a non-holonomic system: – The state is x = (q, q) ∈ R2 ˙ – The control u ∈ R is the force       q  ˙  q q ˙      – The dynamics are   = f (u,  ) =        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:... “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... behavior ¨∗ around the reference trajectory! ref qt = qt + Kp (qt − qt ) + Kd (qt − qt ) ¨∗ ¨ref ˙ref ˙ 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:... 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) ∈ Rn×n is positive definite intertia matrix (can be inverted → forward simulation of dynamics) C(q, q) ∈ Rn ˙ are the centripetal and coriolis forces G(q) ∈ Rn u are the gravitational forces are the joint torques • More compact: M (q) q + F (q, q) = u ¨ ˙ 18/42 Computing M and F M (q) . Robotics Dynamics & Non-holonomic Systems 1D point mass, damping & oscillation, PID, general dynamic systems, Newton-Euler, joint space control, reference. robot’s motors) – But these controls only indirectly influence the dynamics of state, x t+1 = f(x t , u t ) 2/42 Dynamics • The dynamics of a system describes how the controls u t influence the change-of-state. system as one with differential constraints: dim(u t ) < dim(x t ) ⇒ Not all degrees of freedom are directly controllable 3/42 Dynamics • The dynamics of a system describes how the controls u t influence