Nonholonomic Constraints Examples

Nonholonomic Constraints Examples Basilio Bona DAUIN Politecnico di Torino July 2009 B. Bona (DAUIN) Examples July 2009 1 / 34

Example 1 Given q T = [ x y ] T check that the constraint φ(q) = (2x + siny ye x ) dx dt + (x cos y + cos y + e x ) dy dt = 0 is holonomic. We have, by inspection a 1 (q) = 2x + siny ye x a 2 (q) = x cos y + cos y + e x B. Bona (DAUIN) Examples July 2009 2 / 34

We have to test the following conditions a 1 (q) y a 1(q) q 2? = a 2 (q) q 1 a 2(q) x but since a 1 (q) y a 2 (q) x = cos y e x = cos y e x the two terms are equal, and the constraint is holonomic φ(q) = x 2 + x sinx + ye x + siny = c i.e., x 2 + x sinx + ye x + siny c = 0 B. Bona (DAUIN) Examples July 2009 3 / 34

Example 2 Given q T = [ x y θ ] T, check that the constraint ẋ sinθ ẏ cos θ = 0 [ ] sinθ cos θ 0 dx dy = 0 dθ is nonholonomic. Computing the mixed partial derivative we obtain a 1 (q) y a 2 (q) x a 3 (q) x = 0 = 0 = 0 a 1 (q) θ a 2 (q) θ a 3 (q) y = cos θ = sinθ = 0 B. Bona (DAUIN) Examples July 2009 4 / 34

Hence the constraint is nonholonomic, since a 1 (q) y a 1 (q) θ a 2 (q) θ = a 2(q) x a 3(q) x a 3(q) y Using the integrating factor approach, we have to compute the terms in µa 1 q 2 = sinq 3 µ q 2 µa 2 q 1 = cos q 3 µ q 1 µa 1 q 3 = sinq 3 µ q 3 + µ cos q 3 µa 3 q 1 = 0 µa 2 q 3 = cos q 3 µ q 3 µ sinq 3 µa 3 q 2 = 0 B. Bona (DAUIN) Examples July 2009 5 / 34

and write the system: sinq 3 µ q 2 = cos q 3 µ q 1 sinq 3 µ q 3 = µ cos q 3 cos q 3 µ q 3 = µ sinq 3 Squaring and adding the two last equations yields µ θ in the system above gives = ±µ; substitution µ cos θ = µ sinθ µ sinθ = µ cos θ Since the only possible solution is µ = 0, the constraints are nonholonomic. B. Bona (DAUIN) Examples July 2009 6 / 34

Example 3 Figure: Example of nonholonomic constrained system: a disk rotating on a plane without sliding. B. Bona (DAUIN) Examples July 2009 7 / 34

The previous Figure represents a rigid wheel, with radius ρ and negligible thickness, rolling on a plane π c with a single contact point O c (t), that changes in time according to the wheel motion. The plane π r of the wheel is orthogonal to the axis of the wheel, described by the unit vector j r. This plane forms an angle α(t) wrt the normal of the plane π c, that, for simplicity is horizontal. The plane π r forms an angle β(t) wrt to a conventional direction on π c, e.g., the direction of the unit vector i 0 of the fixed reference frame R 0 (i 0,j 0,k 0 ). This angle may vary with time. The motion constraint implies in the first place that the contact point O c must in any circumstance belong to π c (no jumps allowed). B. Bona (DAUIN) Examples July 2009 8 / 34

In the second place, the non sliding motion implies two condition: (Transversal sliding) the impossibility for the wheel to move transversally; the wheel velocity has no components along the unit vector j c on π c. (Longitudinal sliding) the wheel displacement d(t) on the plane along i c must always be equal to the length ρθ(t) of the arc under θ. If not so, a sliding or slipping condition arise: the wheel moves, but does not rotate, or the wheel rotates but does not move. These conditions imply that the instantaneous velocity of O c or O r must be aligned with the unit vector i, by construction itself aligned with i c. Rotation around k r are allowed, since the rotation takes place around an ideal geometric point O c, that do not imply a sliding effect. B. Bona (DAUIN) Examples July 2009 9 / 34

In conclusion, the wheel translates along a local direction given by i = i c and rotates around the instantaneous rotation axis k r = k The homogeneous matrix T 0 r between the fixed reference frame R 0 and the local reference frame R r is from R 0 to R c : T 0 c = Trasl(d c (t)) Rot( k,β(t)), where d c = [ x c y c 0 ] T represents Oc in R 0 : from R c to R r : T c r = Rot(i,α(t)) Trasl(ρ) Rot(j,θ(t)), where ρ = [ 0 0 ρ ] T Without constraints, the generalized coordinates describing the system are the three components of d c (t) and the three angles α(t), β(t), e θ(t): q T = [ x c y c z c α β θ ] B. Bona (DAUIN) Examples July 2009 10 / 34

It seems convincing that the planar motion reduces the dof from six to five, considering the additional constraint d c k 0 = 0; q 3 (t) = 0, t Therefore the new generalized coordinates are q T c = [ x c y c α β θ ] The non sliding/slipping condition generates a relation between the contact point velocity and the wheel velocity; when we represent all the vectors in a common frame R 0, we have: ḋ c (t) = ρ θ(t)i c B. Bona (DAUIN) Examples July 2009 11 / 34

Using the quantities in Figure, we write ρ θ(t) c β s β d x c y c = 0 0 dt 0 or dx c dy c = ρ θ(t)c β dt = ρ θ(t)s β dt hence dx c dy c = 1 tanβ B. Bona (DAUIN) Examples July 2009 12 / 34

Replacing the infinitesimal displacements with the virtual ones, we have i.e., tanβ δx c + δy c = 0 or sinβ δx c + cos β δy c = 0 (1) δq 1 [ sinq4 cos q 4 0 0 0 ] δq 2 δq 3 δq = 0 4 δq 5 This represent a constraint in Pfaffian form with a 1 = sinβ,a 2 = cos β,a 3 = a 4 = a 5 = 0. Computing the partial derivatives we have a 1 (q) q 4 = cos q 4 ; a 4 (q) q 1 = 0; a 2 (q) q 4 = sinq 4 ; a 4 (q) q 2 = 0 B. Bona (DAUIN) Examples July 2009 13 / 34

It appears immediately that equality relations among partial derivatives are not satisfied, so the constrain is non holonomic. We could ask how many degrees of freedom has the system; relation (1) introduces a constraint that links the virtual displacement δx c to δy c. Recalling the dof s definition as the number of virtual displacements independent and complete, the system has only four of such displacements, i.e., δx c, δα, δβ e δθ, while δy c is connected to δx c by (1) and δz c = 0 due to the plane motion. The accessibility space however has dimension 5, since, it is possible, with a suitable motion, to bring the state from any initial point q 0 R 5 to any final point q f R 5. B. Bona (DAUIN) Examples July 2009 14 / 34

Example: the unicycle The unicycle is illustrated in Figure 2. Figure: The unicycle scheme. B. Bona (DAUIN) Examples July 2009 15 / 34

The unicycle generalized coordinates are q = [ q 1 q 2 q 3 ] T = [ x y θ ] T As seen in a previous example, the non sliding motion on the plane determines the following single (m = 1) constraint ẋs θ ẏc θ = [ s θ c θ 0 ] q 1 q 2 = Aq = 0 }{{} q 3 A Now we compute the vector fields forming a basis of the null space of A; the null space has dimension n m = 2; moreover, since the null space conditions are expressed by the following equation [ sθ c θ 0 ] g 11 g 21 g 12 g 22 = [ s θ c θ 0 ][ ] g 1 g 2 g 13 g 23 we obtain g 1 = c θ s θ0 ; g 2 = 0 0 1 B. Bona (DAUIN) Examples July 2009 16 / 34

Hence the kinematic model of the unicycle becomes ẋ ẏ = Gu = c θ s θ0 u 1 + 0 0 u 2 θ 1 Physical considerations allows to define the following inputs: u 1 = v, where v is the driving velocity of the unicycle, obtained as the product of the wheel radius ρ and the wheel angular speed α, and u 2 = ω, where ω = θ is the steering velocity around the vertical axis. B. Bona (DAUIN) Examples July 2009 17 / 34

The Lie bracket of the two input vector fields is [g 1,g 2 ] = g 2 q g 1 g 1 q g 2 = 0 0 0 0 0 0 g 1 0 0 s θ 0 0 c θ g 2 = s θ c θ 0 0 0 0 0 0 0 We can see that [g 1,g 2 ] g 3 is linearly independent from g 1 and g 2. We can also compute the next term [g 1,[g 1,g 2 ]] as [g 1,g 3 ] = g 3 q g 1 g 1 q g 3 = 0 0 c θ 0 0 s θ 0 0 0 0 s θ 0 0 c θ s θ c θ = c θ s θ0 0 0 0 1 0 0 0 0 that is no more independent from the previous fields. B. Bona (DAUIN) Examples July 2009 18 / 34

If we consider the iterative procedure 1, 2,, κ = κ+1 to obtain the involutive closure, i.e, the accessibility distribution A, and recall that κ is called the degree of nonholonomy, we have 1 = = span {g 1,g 2 } 2 = span {g 1,g 2,[g 1,g 2 ]} 3 = span {g 1,g 2,[g 1,g 2 ],[g 1,[g 1,g 2 ]]} = 2 Hence 2 = 3, the accessibility distribution A is with dim A = 3 and κ = 2. A = 2 = span {g 1,g 2,[g 1,g 2 ]} B. Bona (DAUIN) Examples July 2009 19 / 34

Chained form: the unicycle Recalling the kinematic model of the unicycle ẋ ẏ = Gu = c θ s θ0 v + 0 0 ω θ 1 with the following coordinates change and the following input change we obtain the chained form z 1 = θ; z 2 = xc θ + ys θ ; z 3 = xs θ yc θ v u 1 = v 2 + z 3 v 1 ω u 2 = v 1 ż 1 = v 1 ż 2 = v 2 ż 3 = z 2 v 1 Notice that the new coordinates z 2,z 3 give the position of the unicycle in a reference frame that rotates with the robot body. B. Bona (DAUIN) Examples July 2009 20 / 34

Example: the bicycle The bicycle is illustrated in Figure 3, where C is the instantaneous rotation center, θ the body orientation, φ the steering wheel angle, and x,y the cartesian coordinates of the rear (passive) wheel contact point. Figure: The bicycle scheme. B. Bona (DAUIN) Examples July 2009 21 / 34

Possible bicycle generalized coordinates are q = [ q 1 q 2 q 3 q 4 ] T = [ x y θ φ ] T The vehicle motion is constrained by the two non-sliding constraints at the contact points of the posterior and anterior wheels: ẋ a s 34 ẏ a c 34 = 0 ẋs 3 ẏc 3 = 0 where x a,y a are the cartesian coordinates of the anterior wheel, s 3 = sinθ, c 3 = cos θ s 34 = sin(θ + φ), and c 34 = cos(θ + φ). The bicycle body fix the relation between the position of the two wheels x a = x + lc 3 ẋ a = ẋ ls 3 y a = y + ls 3 ẏ a = ẏ + lc 3 B. Bona (DAUIN) Examples July 2009 22 / 34

The constrains can be rewritten as ẋs 34 ẏc 34 l θc 4 = 0 ẋs 3 ẏc 3 = 0 The associated A(q) matrix is [ ] s3 c A(q) = 3 0 0 s 34 c 34 lc 4 0 whose rank is ρ(a) = 2; the null space N(A) dimension is ν = n ρ = 2. A basis of the null space can be found setting A [ ] g 1 g 2 = 0. Consider g 11 g 21 g g 1 = 12 g g g 2 = 22 13 g G = [ ] g 1 g 2 23 g 14 g 24 B. Bona (DAUIN) Examples July 2009 23 / 34

The condition AG = O gives the following constraints s 3 g 11 c 3 g 12 = 0 s 3 g 21 c 3 g 22 = 0 s 3 g 11 c 3 g 12 lc 4 g 13 = 0 s 3 g 21 c 3 g 22 lc 4 g 23 = 0 Since we have eight unknowns, but only four equations, we apply some heuristics: we set g 2 = [ 0 0 0 1 ] T and g14 = 0, so we reduce to two equations in three unknowns. We can verify that the choice c 3 c 4 0 s g 1 (q) = 3 c 4 0 s 4 /l g 2 (q) = 0 0 1 satisfies the constraints, yielding to the kinematic model of the bicycle ẋ c 3 c 4 0 ẏ θ = s 3 c 4 0 s 4 /l u 1 + 0 u 2 φ 0 1 B. Bona (DAUIN) Examples July 2009 24 / 34

Consider the geometric relations among the command variables, if the bicycle is driven by the anterior wheel, we set u 2 as the steering velocity ω, while u 1 is the anterior driving velocity v. In this case ẋ c 3 c 4 0 ẏ θ = s 3 c 4 0 s 4 /l v + 0 ω φ 0 1 If, on the contrary, the driving velocity v is applied by the rear wheel, we have the geometric relation u 1 c 4 = v, and consequently ẋ c 3 c 4 0 ẋ c 3 0 ẏ θ = s 3 c 4 0 s 4 /l u 1 + 0 ω ẏ θ = s 3 s 4 /c 4 l v 0 + 0 ω φ 0 1 φ 0 1 B. Bona (DAUIN) Examples July 2009 25 / 34

Chained form: the bicycle Assume the rear wheel drive bicycle model ẋ c 3 0 ẏ θ = s 3 t 4 l v 0 + 0 ω φ 0 1 where t 4 = tanq 4. Introducing the following coordinates change z 1 = x; z 2 = 1 l sec3 θ tanφ; z 3 = tanφ; z 4 = y B. Bona (DAUIN) Examples July 2009 26 / 34

and the following input transformation v = v 1 cos θ we obtain the chained form ω = 3 l v 1 sec θ sin 2 φ + 1 l v 2 cos 3 θ cos 2 φ ż 1 = v 1 ż 2 = v 2 ż 3 = z 2 v 1 ż 4 = z 3 v 1 B. Bona (DAUIN) Examples July 2009 27 / 34

Example: a planar space robot B. Bona (DAUIN) Figure: The planar Examples satellite scheme. July 2009 28 / 34 The planar satellite in Figure 4 consists of a free-flying main body, with two rotating arms; for simplicity their mass is concentrated at end of the arm.

Consider the following generalized coordinates: x,y the position of center-of-mass (com), θ its orientation wrt a fixed frame, φ 1,φ 2 the orientation of the two arms wrt to the body frame. Hence q = [ x y φ 1 φ 2 θ ] T Other parameters are: r the distance of the arm revolute joints from com and l the length of the two arms; M is the main body mass, Γ its inertia moment wrt to the com, and m are the equal masses of the two arms. With the usual angle convention (positive if anti-clockwise), the two masses have coordinates x 1 = x + r cos θ + lcos(θ + φ 1 ); y 1 = y + r sinθ + lsin(θ + φ 1 ) x 2 = x r cos θ lcos(θ + φ 1 ); y 2 = y r sinθ lsin(θ + φ 1 ) B. Bona (DAUIN) Examples July 2009 29 / 34

The Lagrangian function, assuming no gravity, is simply the kinetic energy T = 1 2 [M(ẋ2 + ẏ 2 ) + Γ θ 2 + m(ẋ1 2 + ẏ2 1 ) + m(ẋ2 2 + ẏ2 2 )] but it can also be written in simplified form as T = 1 2 (M + 2m) ẋ 2 + 1 [ φ 1 φ1 φ2 θ] M φ ; M R 2 2 θ 3 3 where ẋ is the com velocity vector and M is diagonal, with the following elements m ij : m 11 = ml 2 m 12 = 0 m 13 = ml 2 + mr cos φ 1 m 22 = ml 2 m 23 = ml 2 + mr cos φ 2 m 33 = Γ + 2m(r 2 + l 2 ) + 2mlr(cos φ 1 + cos φ 2 ) We notice that L does not depend on θ, and we can reduce the generalized coordinates to the following q = [ φ 1 φ 2 θ ] T. B. Bona (DAUIN) Examples July 2009 30 / 34

If the satellite is free-floating and no other generalized force/torque is applied, and assuming that the satellite com remains in (0,0) for t t 0, then T reduces to T = 1 [ φ 2 1 φ 2 θ ] φ 1 M φ ; M R 2 θ 3 3 and the Lagrange equations reduce to d L L dt θ }{{} θ =0 = 0 since we know (from Hamilton equations) that L is the angular θ momentum p B. Bona (DAUIN) Examples July 2009 31 / 34

We have p = L θ = m 13 φ 1 + m 23 φ2 + m 33 θ If the initial angular momentum is zero, we have the constraint m 13 (q) q 1 + m 23 (q) q 2 + m 33 (q) q 3 = 0 Since the actuated coordinates are u 1 = φ 1 and u 2 = φ 2, we have q = g 1 (q)u 1 + g 2 (q)u 2 with q 1 q2 = q 3 1 0 m 13 m 33 u 1 + 0 1 m 23 m 33 u 2 B. Bona (DAUIN) Examples July 2009 32 / 34

It is sufficient to compute m 13 = 0 q 2 m 13 = 0 q 3 m 23 = 0 q 3 m 23 = 0 q 1 m 33 = 2mlr sinq 1 q 1 m 33 = 2mlr sinq 2 q 2 to verify that the constraints are nonholonomic. B. Bona (DAUIN) Examples July 2009 33 / 34

References [01] F. Bullo, A.D. Lewis, Geometric Control of Mechanical Systems, Springer, 2005. [02] H.K. Khalil, Nonlinear Systems, III Ed. Springer, 2002. [03] R.M. Murray, Z. Li, S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994. [04] S. Sastry, Nonlinear Systems: Analysis, Stability, and Control, Springer, 1999. [05] B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009. [06] Z. Qu, Cooperative Control of Dynamical Systems: Applications to Autonomous Vehicles, Springer, 2009. B. Bona (DAUIN) Examples July 2009 34 / 34