Recursive Dynamics - Human Motion Simulation: Predictive Dynamics - page 84

4.6.3 Backward recursive dynamics

0 1

2 00 m 2 ðl 2 2

I 2 1 m 2 ðl 2 2 L 2 Þ L 2 Þ

@ A

0 0 0 0

J 2 5 (4.64)

0 0 0 0

m 2 ðl 2 2 L 2 Þ 00 m 2

0
@

1
A

2 00 m 1 ðl 1 2 L 1 Þ

I 1 1 m 1 ðl 1 2 L 1 Þ

0 0 0 0

J 1 5

(4.65)

0 0 0 0

m 1 ðl 1 2 L 1 Þ 00 m 1

D 2 5 J 2 C 2 ; D 1 5 J 1 C 1 1 T 2 D 2 (4.66)

T T

E 2 5 m 2 ð l 2 2 L 2 001 Þ ; E 1 5 m 1 ð l 1 2 L 1 001 Þ

1 T 2 E 2

(4.67)

T

F 2 5 ð 0001 Þ F 1 5 T 2 F 2 (4.68)

T T

5 ð 0

g 00 Þ

f 2 5 ð 0

f 00 Þ

g 2 2 (4.69)

And the torques are expressed as:

2
4

3
5 2

tr @

A 1
@q 1 D 1

g T @

A 1
@q 1 E 1 2

f 2 T @

A 1
@q 1 F 1

τ 1 5

θ 2 ÞÞθ 1 1 ðI 2 1 θ 2 Þθ 2 m 1 l 1 1 m 2 ðL 1 1 l 2 1 m 2 l 2 1

5 ðI 1 1

I 2 1 2 L 1 l 2 cos m 2 L 1 l 2 cos

2
2 sin

2 m 2 L 1 l 2 θ 1 θ 2 sin

m 2 L 1 l 2 θ θ 2 2 θ 2 1 m 2 gl 2 cos ðθ 1 1 θ 2 Þ 1 m 1 gl 1 cos θ 1

2

m 2 gL 1 cos θ 1 1 fL 2 cos ðθ 1 1 θ 2 Þ 1 fL 1 cos θ 1

1

2
4

3
5 2

(4.70)

tr @

A 2
@

g T @

A 2
@

f 2 T @

A 2
@

τ 2 5

q 2 D 2 q 2 E 2 2 q 2 F 2

(4.71)

2
1 sin

m 2 l 2 Þθ 2 1 ðI 2 1 θ 2 Þθ 1 1 m 2 L 1 l 2 θ m 2 l 2 1

5 ðI 2 1

m 2 L 1 l 2 cos θ 2

m 2 gl 2 cos ðθ 1 1 θ 2 Þ 1 fL 2 cos ðθ 1 1 θ 2 Þ

1

4.6.4 Gradients
Explicit gradients of torque with respect to state variables are derived as follows:

0 1

2 A 1 2 A 1 2 A 1

@τ 1
@θ 1 5

@

q 1 D 1 1 @ A 1 q 1 @ D 1

@

f 2 @

@ A 2

g T tr

q 1 E 1 2 q 1 F 1 @ q 1 @ @ @ q 1 @ q 1 @ @ q 1 @

52 m 2 gl 2 sin ðθ 1 1 θ 2 Þ 2 m 1 gl 1 sin θ 1 2 m 2 gL 1 sin θ 1 2 fL 2 sin ðθ 1 1 θ 2 Þ 2 fL 1 sin θ 1

(4.72)