Lecture 27. Two Big Examples

1

Lecture 27. Two Big Examples

The eleven link diver

The Stanford Arm (Kane & Levinson)

2

The Eleven Link Diver

head

foot

calf

thigh

pelvisabdomen

thorax

hand

lower armupper arm C7

ankle

knee

hip

L1

T6elbow

wristshoulder

C1

Parts of the diver The joints

neck

3

The torques can only control the differential motions, so

(The first three denote the position and orientation of one reference link, here the abdomen.)

Or, as we wrote them last time

4€

q =

y5

z5

ψ 5

ψ 5 −ψ 4

ψ 4 −ψ 3

ψ 3 −ψ 2

ψ 2 −ψ1

ψ 6 −ψ 5

ψ 7 −ψ 6

ψ 8 −ψ 7

ψ 9 −ψ 6

ψ10 −ψ 9

ψ11 −ψ10

⎧

⎨

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

⎩

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

⎫

⎬

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

⎭

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

⇔

abdomen - pelvispelvis − upper leg

upper leg - lower leglower leg - foot

abdomen - thoraxthorax - neckneck − head

thorax - upper armupper arm - lower arm

lower arm - hand

⎧

⎨

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

⎩

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

⎫

⎬

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

⎭

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

5

Characterizing a dive

CM path and rotation determined by initial conditions

Figure of the diver is determined by the diver —the control torques

The diver must find torques that will put the internal angleson specific paths in time

6

€

˙ ̇ e = − ˙ ̇ q d + a qd + e, ˙ q d + ˙ e ( ) + b qd + e( )τ

Control by feedback linearization, which we’ve explored and can review quickly

€

˙ ̇ q = a q, ˙ q ( ) + b q( )τConsider a second order system:

€

q = qd + eWe want q to follow a specific path:

The equation for the error is

€

−2ςχ ˙ e − χ 2eChoose t such that rhs =

€

˙ ̇ e + 2ςχ ˙ e + χ 2e = 0The error then obeys

(you can’t always do this)

7

To apply to our problem note:

There are 13 equations for

€

˙ u i

The torques appear only in equations 4-13

One can use equations 1-3 to eliminate the first three

€

˙ u i

€

˙ u i = ˙ ̇ q di + 2ςχ ˙ q d

i + χ 2qdi − 2ςχui − χ 2qi,i = 4L 13If

then the generalized coordinates will go to their desired paths

AND YOU CAN DO THIS

you don’t actually have to do thisas we saw for the three link diver

8

The next issue is computational

I have 26 first order odes, some of which are very messy

I like to see what I’m doing using Mathematica, butthe big set is very very slow.

I borrow an idea from Kane and Levinson (1983),which swaps complexity for size — the so-called method of Zs

9

The idea is to write the odes symbolically in terms ofcoefficients to be evaluated at each time step

€

pi = ∂L∂ ˙ q i

= Zijuj , ˙ p i = Zij ˙ u j +

∂Zij

∂qk u ju k

26 odes

€

Zij = Zij qn( ), ∂Zij

∂qk = Zij, k qn( )

642 algebraic equations

PLUS

10

Dives

10 meter platform

head

foot

calf

thigh

pelvisabdomen

thorax

hand

lower armupper arm

C7

ankle

knee

hip

L1

T6elbow

wristshoulder

C1

Parts of the diver The joints

(not to scale)11

neck

Recall the model 11 link diver

12

Cobbled together from various sources — he’s big

13

All dives start from an erect position diveron toes with arms fully extended upwards.Diver leaps up and to the left with positive rotation.Analysis starts once the diver has cleared the platform.

t = 0 t = dt

14

Initial conditions:

position and velocity of the center of mass

€

y0,z0( ), ˙ y 0, ˙ z 0( )

determines the path of the diver

15

initial angles and rotations

yi = 0 qi = 0

€

˙ ψ i = Ω initial uniform rotation

determine the initial posture and angular momentum

Desired qi, qdi determine the posture during the dive

We want them to go to a specified value and then return to the initial value of pto ensure straight entry into the water.

16

Let’s look at a couple of examples, and then think about how to do this

17

Mock jackknife

€

Ω=1.76593 rad/sec, ˙ y 0 = −0.3 m/s, ˙ z 0 = 3.13 m/s

€

q1, q2, q3 are uncontrolled

qd4, qd

6 − qd10 are all zero

qd5 = π −α 1( )sin2 π t

t f

⎛

⎝ ⎜ ⎜

⎞

⎠ ⎟ ⎟, qd

11 = α 2 sin2 π tt f

⎛

⎝ ⎜ ⎜

⎞

⎠ ⎟ ⎟

α 1 = 0.527788, α 2 = 0.845088, t f =1.78932 sec

18

19

20

Mock tuck

€

Ω=1.0374 rad/sec, ˙ y 0 = −0.3 m/s, ˙ z 0 = 3.13 m/s

€

q1, q2, q3 are uncontrolled

qd4, qd

7 − qd8 are all zero

qd5 = 0.99πΘ t( ) = qd

11, qd6 = −0.99πΘ t( ) = qd

12, qd10 = 0.25πΘ t( ) = qd

11

Θ t( ) = 12

tanh t − 0.30.1

⎛ ⎝ ⎜

⎞ ⎠ ⎟− tanh t −1.3

0.1 ⎛ ⎝ ⎜

⎞ ⎠ ⎟

⎛ ⎝ ⎜

⎞ ⎠ ⎟

t f =1.78932 sec

21

22

23

We can look at the Mathematica code and at “movies” of these dives

24

The Stanford Arm: Kane & Levinson Revisited

This is a difficult problem

The difficulty lies in figuring out how to represent it

Once you have a decent model, it becomes easy (if possibly tedious)

What are the angular velocities of the links?

What are the generalized forces?

We will roll up our sleeves and tackle it.

25

Note that blindly following the recipes I have given you won’t workor at least won’t work easily

You need to go back to the ideas that lie behind the recipes

So, here we go . . .

26

There are six links, hence 36 possible coordinates.

There are only six degrees of freedom so we have to get rid of 30 coordinates

I don’t like their coordinate system, so let me change the notation

27

They letter their links, and I want to number mineso link A is link 1, link B is link 2, etc

I want to arrange things such that all the rotations are about K axes

This is a holonomic system, but some of the connectivity constraints are awful

Finally we have one prismatic (sliding) joint, which is something we haven’t seen

28

I have a seven simple holonomic constraints

Choose the origin at KL A*, then

€

x1 = y1 = z1 = 0, φ1 = 0 = θ1

z2 = L4

The third link, Cdoes not rotate

about its own K axis

€

y3 = 0

29

Link A rotates about the verticalso make that K1

and let I1 be parallelto

Link B rotates aboutI1, so let that be K2

q1 will be y1

q2 will be y2

30

Link C moves parallelto

Let that be J2

and equal to K3

Link D rotates about that directionLet K4 = K3

q3 = y3

31

Link E rotates aboutwhich we can call I4

and set it equal to K5

q4 = y5

Finally,Link F rotates aboutwhich we can call I5

and set equal to K6

q5 = y6

32

We can summarize all of this as follows

functions of y1

functions of y1, y2

functions of y1, y2

functions of y1, y2, y4

functions of y1, y2, y4, y5

functions of y1, y2, y4, y5, y6

33

Now, how do we incorporate all of this into our usual systems/thoughts?

First note that we can finesse the usual f and q variables out of the picture

We can calculate the angular velocities and momenta in body coordinates without them

Thus we can write the rotational kinetic energy without them

This is a little bit tricky, and we need to think as we go forward

34

Each link can be represented by a vector referred to its body axes

€

V = XXI + YYJ + ZZK

For all but link 3 (link C) the XX, YY and ZZ coefficients are constant(and their values are irrelevant to the argument!)

The angular velocity is represented by the rates of change of the base vectors

€

˙ V = XX˙ I + YY˙ J + ZZ ˙ K + dZZdt

K

where the last term is zero except for link 3

35

We can write any vector as

€

V = l ˆ V ⇒ ˙ V = l ˆ ˙ V + ˙ l ̂ V

The angular velocity term is contained in the change of base vectors

€

˙ V = ωB × l ˆ V + ˙ l ̂ V

In our case, then

€

ω B × V = XX˙ I + YY˙ J + ZZ ˙ K

36

Our job is then to extract the components of ωB from this equation

€

ωB × V =I J K

ωBX ωBY ωBZ

XX YY ZZ=

ωBY ZZ −ωBZYYωBZ XX −ωBX ZZωBXYY −ωBY XX

⎧ ⎨ ⎪

⎩ ⎪

⎫ ⎬ ⎪

⎭ ⎪

Take the body components of the right hand vector

€

I ⋅ ωB × V( ) = ωBY ZZ −ωBZYY

J ⋅ ωB × V( ) = ωBZ XX −ωBX ZZ

K ⋅ ωB × V( ) = ωBXYY −ωBY XX

Write

The ZZ component of the first one gives me ωBY

The XX component of the second one gives me ωBZ

The YY component of the third one gives me ωBX

The values of XX and YY and ZZ are irrelevant!

37

Here it is in Mathematica

38

We need the angular velocities in inertial coordinates for the generalized forces

The base vectors are, of course, defined with respect to the inertial system,so these are indeed exactly what we want

39

Where do we now stand on variables?

We need a kinetic energy and a potential energy

The rotational kinetic energy depends only on the five ys we have defined

The translational kinetic energy depends on the eighteen center of mass coordinates

But we’ve already eliminated four of those

So we have fourteen CM coordinates, five ys and the extension coordinate of link 3

40

So we have twenty coordinates and only six degrees of freedom

The CM coordinates are all connected by fourteen connectivity constraints

They are pretty immense, so I put them into a constraint matrixand go with pseudononholonomic constraints

Before I do that, let us look at the generalized coordinates

41

The variables I care about are the last six

the extension of link 3

42

The fourteen constraints can be written

43

Differentiate the constraints, form the usual constraint matrix, and find a null space matrix

The u vector will have six components, and with the proper choice of Sthese will be the rates of change of the important variables: yi and q6

The only thing we need now to complete the problem is the generalized forces

44

€

T01 = τ 01K1

T12 = τ 12K 2

F23 = f23K 3

T34 = τ 34K 4

T45 = τ 45K 5

T56 = τ 56K 6

acts on link 1

acts on links 2 and 1 (-)

acts on links 3 and 2 (-)

acts on links 4 and 3 (-)

acts on links 5 and 4 (-)

acts on links 6 and 5 (-)

45

So we have

€

˙ W = T01 − T12( ) ⋅ω1 + T12 ⋅ω2 + F23 ⋅ v3 − v2( ) − T34 ⋅ω3

+ T34 − T45( ) ⋅ω4 + T45 − T56( ) ⋅ω5 + T56 ⋅ω6

where

€

v2 = K 2 × ω2, v3 = v2 + ˙ q 6K 3

All the angular velocities here are expressed in the inertial system

46

This leads us to a set of generalized forces, the first fourteen of which are zero

The nonzero terms are

€

Q15 = τ 01 + 2τ 56 cosψ 2 cosψ 5 + sinψ 2 sinψ 4 sinψ 5( ) − 2τ 45 sinψ 2 cosψ 4

Q16 = τ 12 + 2τ 45 sinψ 4 + 2τ 56 cosψ 4 sinψ 5

Q17 = f23

Q18 = τ 34 + 2τ 56 cosψ 5

Q19 = −τ 45

Q20 = τ 56

47

Now we have everything we need to set up the equations and simulate the robotSo, let’s go do it!