Upload
julian-parsons
View
217
Download
4
Tags:
Embed Size (px)
Citation preview
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 - pelvis
pelvis − upper leg
upper leg - lower leg
lower leg - foot
abdomen - thorax
thorax - neck
neck − head
thorax - upper arm
upper 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= Ziju
j , ˙ p i = Zij ˙ u j +∂Zij
∂qku 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 πt
t 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( ) =1
2tanh
t − 0.3
0.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
€
ψ3 = 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 +dZZ
dtK
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 wB 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 wBY
The XX component of the second one gives me wBZ
The YY component of the third one gives me wBX
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!