Newton-Euler Lagrange for Robotics

Recapitula Recapitulation: tion: Newton-Eule Newton-Euler-Met r-Method hod The Newton-Euler-Method is a way to determine the dynamics equations that govern the movement of a robot. robot. The field of analyti analytical cal dynamics dynamics,, or more briefly briefly dynamics dynamics,, is concerned concerned with the relarelationship between motion of bodies and its causes, namely the forces acting on the bodies and the properties of the bodies (particularly mass and moment of inertia) influencing that movement. This is in contrast to kinematics, kinematics, where we were not concerned with the physical phenomena phenomena causing robot movement, but only with the movement itself. Overall, the purpose of the Newton-Euler-Method is determining the joint torques τ   τ   that are required to achieve a desired motion of the robot. More specifially, if we have a joint trajectory Θ(t Θ(t) that describe the positions and velocities for a certain trajectory of the robot, then the Newton-Euler method will allow us to compute corresponding τ  corresponding τ ((t) that will, in an ideal world (no friction or other disturbances for the moving robot), cause the robot to carry out the desired trajectory. The fundamental physical relationships that are used for deriving this method are: F  = m v˙ C  N  = C I  ˙ ω˙  + ω  + ω  ×  C I ω The first equatio equation n is the regular regular physi physical cal rule rule for forces: forces: “Force “Force equals mass times times acceler acceleratio ation”. n”. This formula holds for the force that applies to the center of mass of an object. The second equation is the rotational equivalent of force, the torque. Here, C I  is   is the inertia tensor that is computed with respect to the center of mass, a 3 × 3 matrix that describes the distribution of masses in a link. The equations are named ”‘Newton’s equation”’ and ”‘Euler’s equation”’, hence the name of the method. The Newton-Euler-Method is composed of two phases: A forward phase and a backwards phase. In the first phase, the velocities and accelerations (both rotational and linear) are computed for each  joint. The formulas for the rotational velocities and accelerations, in case of a rotational jointi joint i + 1, are: ˙ i+1  ·  i +1 Z i+1 R ·  i ωi  + Θ i+1 ˙ i+1  ·  i +1 Z i+1  + Θ ¨ i+1 i+1 Z i+1 ω˙ i+1  = ii+1 R ·  i ω˙ i  +  ii+1 R ·  i ωi × Θ i+1

ωi+1  =

i+1 i

And if joint i joint  i +  + 1 is prismatic, the formulas simplify to i+1

ωi+1  =

i+1

ω˙ i+1  =

i+1 i i+1 i

R ·  i ωi R ·  i ω˙ i

For linear accelerations, the formulas are: (rotational joint i + 1): i+1

v˙ i+1  =

i+1 i

R (i ω˙ i ×  i P i+1 +  i ωi  ×  (i ωi  ×  i P i+1 ) +  i v˙ i )

If joint i + 1 is prismatic, the formula becomes substantially more complicated: i+1

v˙ i+1  =

i+1 i

R (i ω˙ i  ×  i P i+1  +  i ωi  ×  (i ωi ×  i P i+1 ) +  i v˙ i ) + 2  ·  i +1 ω i+1 × d˙i+1 i+1 Z i+1  + d¨i+1 i+1 Z i+1

During the computation, it is necessary to find the linear acceleration of the center of mass of each link, which can be found using the following relationship: i

 i i  i  i v˙ C i  = i ω˙ i  ×  i P C  ˙i C   + ωi  ×  ( ωi  × P C  C  ) + v i

i

Here, C i  denotes the center of mass of link i. This equation holds for both rotational and prismatic  joint i + 1. In the next step, we can compute the forces and torques that apply to the center of mass of each link from the accelerations and speeds that we have just computed: i

F i = m  ·  i v˙ C 

i

C i

i

N i =

 i

I i · ω˙ i  +  i ωi  ×  C  I i  ·  i ωi i

Based on those forces and moments, we can compute f  and n for the joints of the robot: i i

f i = ii+1 R ·  i +1 f i+1  +  i F i

ni = i N i +  ii+1 R  ·  i +1 ni+1  +  i P C   ×  i F i  +  i P i+1  ×  ii+1 R i+1 f i+1 i

As in the static case, the values of  τ i , compute as τ i = i ni · i Z i  for a rotational joint and τ i = i f i · i Z i for a translational joint. We also need to take into account the effect of gravity on the links of the robot. This can be done through setting 0 v˙ 0 =  − G where G is the gravitational acceleration. By doing so, we simulate the effect of gravity by having the robot move “upwards”, which exactly simulates the effect of  gravitation. The equations of movement that have been computed this way can be rearranged into the so-called state-space equation (or M-V-G-form). This looks as follows: T

T

¨ + V  (Θ, Θ) ˙ + G(Θ) τ  = M (Θ)Θ M  is a n  ×  n   matrix, and V   as well as G are n × 1 vectors. Note that in general, M , V  , G are complex-valued. The matrix M   can be determined from the dynamics equations by factorizing all ¨ Similarily, V  is determined by factorizing all summands that contain Θ. ˙ summands that contain Θ. Finally, G is determined by factorizing all summands that contain g. V   can be further decomposed into components B and C , yielding the configuration-space equation (or M-B-C-G-form): ¨ + B(Θ)[Θ  ˙ Θ]  ˙ + C (Θ)[Θ  ˙ 2 ] + G(Θ) τ  = M (Θ)Θ  ˙ Θ]  ˙ is an abbreviation for the vector B(Θ) is a matrix of dimension n × n(n − 1)/2, and [Θ  ˙ 1  ˙Θ2 , Θ ˙ 1  ˙Θ3 , . . . , Θ ˙ n−1  ˙Θn ) (Θ

T

that has length n(n  −  1)/2. This also explains the dimension of  B: n(n  −  1)/2 is the number of   ˙ 2 ] in an abbreviation for the products Θi Θ j   with i   =  j . The matrix C  has dimension n ×  n, and [Θ vector  ˙ 2 , Θ ˙ 2, . . . , Θ ˙ 2n ) (Θ 1 2 T

with length n. The matrices B and C  can be determined by finding the coefficients of Θi Θ j  and Θ2i , respectively.

A simple example for robot dynamics Because the computations for the regular problems are very lengthy, the two methods for computing robot dynamics are shown here at length for a very simple example robot, shown in Figure 1. The following values are specified:

•  The length of the link: l, the mass of the link (including gripper): m, center of mass of the arm: 1 P C  = ( 2l , 0, 0) T

1

Figure 1:   Example robot 

•  Inertia tensor for the first arm:

C 1

I 1  = I (Identity matrix)

  (0, −g, 0) •   Gravity acceleration: G =

T

•  Rotation matrix 01 R: 0 1

R =

 

 

c1 −s1 0 s1 c1 0 0 0 1

,

1 0

 

 

c1 s1 0 −s1 c1 0 0 0 1

R =

In the following, we will compute the manipulator dynamics using the Newton-Euler method. We simply need to apply the formulas from the Newton-Euler method summary above. Note that the acceleration of the base system 0 v˙ 0   is set to the negative gravitational acceleration, as has been explained there as well. We compute the velocities and accelerations as: 1 ˙ 1 ·  1 Z 1 ω1 = 10 R ·  0 ω0 + Θ

     

˙ 1 · = 0+Θ 1

=

  0 0 ˙1 Θ

¨ 1  ·  1 Z 1 ω˙ 1 = 10 R ·  0 ω˙ 0  +  10 R ·  0 ω0  ×  1 ω1  + Θ =0+0+

1

0 0 1

0 0 ¨1 Θ

v˙ 1 = 10 R · (0 ω˙ 0  ×  0 P 1 +  0 ω0  ×  (0 ω0  ×  0 P 1 ) +  0 v˙ 0 )   = = 10 R · (0 + 0 + −G)

s1 g c1 g 0

 

1

v˙ C  = 1

=

=

     

0 0 ¨1 Θ

l

     

     

2

×

0 0

+

0 0 l ¨ 1  · + 0 Θ 2 ˙1 0 Θ  ˙ 21  · l  + s1 g −Θ 2 l ¨ Θ1  · 2 + c1 g 0

   

   

0 0 ˙1 Θ

0 0 ˙1 Θ

×

×

0 ˙ 1  · Θ 0

l

2

   

+

l

          2

×

0 0

+

s1 g c1 g 0

s1 g c1 g 0

The next step is the computation of forces and moments on the centers of mass of the links: 1

F 1 = m 1  ·  1 v˙ C 

1

C 1

1

N 1 = =

I 1  · ω˙ 1  +  1 ω1  ×  C  I 1  ·  1 ω1  1

1

  0 0 ¨1 Θ

And in the backwards iteration, the forces and moments can be determined: 1

1

f 1 = 0 +  1 F 1

n1 =

=

 ˙ 21 · l − gs1 ) −m1 (Θ 2 ¨ 1  · l  + gc1 ) m1 ( Θ 2 0

l

            0 0 ¨1 Θ

2

¨ 1  + Θ

l

2

×

0 0

+

0 0 ¨ 1  · l  + gc1 ) · m1 (Θ 2

 

All in all, the result is then: ¨1 τ 1  = 1 n1 ·  1 Z 1  = Θ T



l2 l 1 + · m + · m · g  ·  c1 4 2



Solution 1 The transformation matrices for this robot are as follows:

0 1

T=

 

c1 −s1 s1 c1 0 0 0 0

0 0 1 0

 

0 0 0 1

1 2

T=

 

1 0 0 0 0 −1 0 0

0 l1 1 d2 0 0 0 1

 

We know that gravity force applies in negative X 0 -direction, and we denote the corresponding acceleration with g. Now, we are supposed to apply the Newton-Euler-Method to derive the dynamic

equations of this robot. We start with the forward iteration, determining speeds and accelerations:

1

i = 0 :

1

1

1

            0 0 ˙1 Θ

˙ 1  · Z 1 = ω1  = R  · ω0  + Θ 1 0

 0

 1

0 0 ¨1 Θ

¨ 1  ·  1 Z 1  = ω˙ 1  = 0 + 0 + Θ

1 0

v˙ 1 = R

 

0

 0

ω˙ 0  × P 1  +

g 0 0

gc1 −gs1 0

=

v˙ C  = 1 ω˙ 1  ×  1 P C   +  1 ω1  ×  (1 ω1  ×  1 P C  ) +  1 v˙ 1  = 1

1

1

˙ 2  + gc1 − l2 Θ 1 l ¨ Θ1  −  gs1 2 0

 

1

1

 

The next joint is a prismatic joint, which means that we have to use slightly different formulas:

2

i = 1

2

2

v˙ 2 =

2 1

R  ·

=

1 2

R

1

ω2 =

2 1

ω˙ 2 =

2 1

       

R  ·  1 ω1 = 12 R

 1

R  · ω˙ 1 =

T

0 0 ˙1 Θ

=

1 0 0 0 0 −1 0 1 0

·

=

0 ¨1 −Θ 0

0 0 ¨1 Θ

×

0 × +2 + 0 d¨2 ¨1 −d2 Θ −l1  ˙Θ21 ¨1 = 12 R · + −d2  ˙Θ21 + l1 Θ 0 0 ¨ 1 −  l1 Θ  ˙ 2  + gc1  −  2Θ  ˙ 1 d˙2 −d2 Θ 1 0 = ¨ 1 −  d2  ˙Θ2  −  gs1  + d¨2 l1 Θ 1

gc1 −gs1 0

0  ˙ 1 −Θ 0

×

l1 d2 0

0 0 d˙2

+

0 0 ˙1 Θ

          

0 0 ˙1 Θ

T

0 0 ¨1 Θ

ω˙ 1  ×  1 P 2  +  1 ω1 ×  (1 ω1  ×  1 P 2 ) +  1 v˙ 1 + 2  ·  2 ω2  × d˙2 2 Z 2  + d¨2 2 Z 2

                                     T

       

0  ˙ 1 −Θ 0

l1 d2 0

×

+

 ˙ 1 d ˙2 −2Θ 0 d¨2

    +

gc1 −gs1 0

 

2

ω˙ 2  ×  2 P C   +  2 ω2  ×  (2 ω2  ×  2 P 2 ) +  2 v˙ 2 0 0 0 0 ¨  ˙  ˙ 1 = −Θ1 × 0 + −Θ1 × −Θ l2 0 0 0 ¨ 1  + l1 Θ  ˙ 2  + gc1 −  2Θ  ˙ 1 d˙2 −d2 Θ 1 + 0 ¨  ˙ l1 Θ1  −  d2 Θ21  −  gs1  + d¨2 ¨ 1  −  l1  ˙Θ21  + gc1 −  2Θ  ˙ 1 d˙2 −(l2  + d2 )Θ 0 =  ˙ 2  + l1 Θ ¨ 1  −  gs1  + d¨2 −(l2 + d2 )Θ 1

v˙ C 

=

2

2

2

C 

                            ×

0 0 l2

Next, we compute the forces and torques that apply to the centers of mass of the links: ˙ 2  + gc1 − l2 Θ 1 l ¨ Θ1  −  gs1 2 0

                1

1

F 1 = m1  ·  1 v˙ C  = m 1

1

1

1

N 1 = =

2

C 1

I 1  ·  1 ω˙ 1  +  1 ω1  ×  C  I 1 1 ω1 0 0 0 0 0 + 0 × ¨1 ˙1 I zz 1 Θ Θ I zz 1  ˙Θ1 1

2

N 2 = =

C 2

=

¨ 1  −  l1  ˙Θ2 + gc1  −  2Θ  ˙ 1 d˙2 −(l2  + d2 )Θ 1 0 2  ˙ 1  + l1 Θ ¨ 1 −  gs1  + d¨2 −(l2  + d2 )Θ

             

F 2 = m2  ·  2 v˙ C  = m 2  · 2

0 0 ¨1 I zz 1 Θ

I 2  ·  2 ω˙ 2  +  2 ω2  ×  C  I 2 2 ω2 0 0 ¨ 1 + −Θ  ˙ 1 × −I yy 2 Θ 0 0 2

 

0 I zz 1  ˙Θ1 0

=

0

¨1 −I yy 2 Θ 0

 

 

To slightly simplify the further computations, we will introduce abbreviations for the components of  2 F 2  as follows: 2 F 2  = (A, 0, B) . Now we can start with the backwards iteration: T

i = 2 :

2

f 2 = 2 n2 =

2

F 2 2 N 2  + I  ·  3 n3  +  2 P C   ×  2 F 2  + 0 0 0 ¨ 1 I yy 2 + 0 ×  2 F 2 = −Θ l1 0 =

2

   

   

0 ¨ 1 I yy 2 −  m2 l1 (l1  + d2 )Θ ¨ 1  −  l1 l1 m2  ˙Θ2  + m2 l1 gc1  −  2m2 l1 d˙2  ˙Θ1 −Θ 1 0

    =:

0 C  0

Thus, we have already determined τ 2 = 2 f 2 2 Z 2  (remember that the second joint is prismatic): T

 ˙ 2  + l1 Θ ¨ 1  −  gs1  + d¨2 ) τ 2 = m 2 (−(l1  + d2 )Θ 1

Continuing with the backwards iteration: i = 1 :

1

1

n1 =

1

         

=

2 1

All in all:

τ 1

2 1

       

1

l1 d2 0

×

A B 0

0 0 ¨ 1 −  C  + m1 l Θ ¨ 1  −  gm 1 l s1 + l1 B −  d2 A I zz 1 Θ 4 2

=

τ 1 =

N 1  +  12 R  ·  2 n2  +  1 P C   ×  1 F 1  +  1 P 2  ×  12 R  ·  2 f 2 0 0 0 0 0 0 + + + l l ¨1 ¨ 1  −  gm 1 s1 −C  I zz 1 Θ m1 4 Θ 2

1

1

n1 1 Z 1 T

l12 ¨ l1 ¨ = I zz 1 Θ1  −  C  + m1 Θ1  −  gm 1 s1  + l1 B  −  d2 A 4 2 l12 2 ¨ 1  + m2 l1 ¨ = (I zz 1  + I yy 2  + m2 (d2  + l1 ) + m1 + m2 l12 )Θ d2 4 m1 +2m2 (l1  + d2 )d ˙2  ˙Θ1  − + m2 gl 1 s1 −  m2 g(l1  + d2 )c1 2





And we have finally determined formulas for computation of the joint torques/forces. Now we are ˙ = (Θ  ˙ 1 , d˙2 ) , supposed to rewrite these in state space form (MVG form). With Θ = (Θ1 , d2 ) , Θ ¨ = (Θ ¨ 1 , d¨2 ) , τ  = (τ 1 , τ 2 ) , this amounts to: Θ T

T

T

T



l12

2

2 2 1

M (Θ) = I zz 1  + I yy 2  + m2 (d2  + l1 ) + m1 4 + m l m2 l1 ˙ = V  (Θ, Θ)



 ˙ 1 d ˙2 2m2 (l1  + d2 )Θ  ˙ 2 −m2 (l1  + d2 )Θ 1



−( m2 + m2 )gl 1 s1  −  m2 g(l1  + d2 )c1 G(Θ) = −m2 gs1



1

m2 l1 m2





Recapitulation: Lagrangian Dynamics The Lagrangian dynamic formulation is another method for determining the dynamics of a robot which is derived from energy considerations. For each link of the robot, the kinetic energy can be computed as: 1 1 ki = mi vC   ·  vC   + i ωi ·  C  I i ·  i ωi 2 2 The first term corresponds to the kinetic energy caused by the linear motion of the link, and the second term corresponds to the kinetic energy caused by the rotational velocity of the link. To determine these energies, we need to compute the linear and rotational velocities of the joints. The overall kinetic energy computes then as sum of the kinetic energies of all links: T

T

i

i

i

n

k =



ki

i=1

Another way to compute kinetic energy is ˙ = 1Θ ˙ M (Θ)Θ,  ˙ k(Θ, Θ) 2 T

where M   again is the n  ×  n   mass matrix from the M-V-G form of the dynamics equations. To compute all energies that are present in this system, we also need to take into account the potential energy: ui =  − mi ·  0 g ·  0 P C   + uref  T

i

i

Here, g  is the vector of gravity, 0 P C   denotes the center of mass of link i, and uref   is an arbitrary constant (the constant is added because potential energy depends on height, and a certain base height can be chosen arbitrarily). In the further computations, this constant will not play a role, since only the derivatives of the potential energy are considered - and any constant will vanish when differentiated. The computation of τ  is finally done through the following formula: i

i

τ  =

d ∂k ∂k ∂u + − dt ∂  ˙Θ ∂ Θ ∂ Θ

It is also possible to compute the joint torques τ i  on a per-joint basis, which is more practical in most cases. The formula then becomes: τ i =

d ∂k ∂k ∂u + . − dt ∂  ˙Θi ∂ Θi ∂ Θi

Simple dynamics example: Lagrange Method Here, we will revisit the simple example for which we have computed the dynamics using the NewtonEuler method. This time, we will solve the problem using the Lagrangian formulation of dynamics. The method of Lagrangian dynamics uses a completely different approach, but the results will be the same as with the Newton-Euler method. To successfully apply the Lagrangian method, we need to determine some additional values: 0

P C  = 1

l

    c1  · s1  · 0

d0 P C  = dt

2

l

2

⇒

1

˙ 1  · l −s1 · Θ 2 l ˙ c1 · Θ1  · 2 0

 

 

To compute the kinetic energy, we need to know the velocity of the center of mass. In principle, the system of reference in which the velocities are determined does not matter. This can be seen easily from the formula for kinetic energy: 1 1 ki = mi vC  vC   + i ωi  ·  C  I i ·  i ωi 2 2 T

i

i

i

The first summand contains only the dot product of the velocity with itself, thus the squared length of the vector vC  . This means that only the length of that vector is relevant, and the length is obviously invariant under coordinate transformations. It is usually most convenient to determine the velocities directly in the base system  { 0}  from geometric considerations. In our case, the position of  the center of mass is described by l c 2 1 0 P C  = 2l s1 . 0 i

1

       

The velocity is then determined simply by differentiating with respect to time: d0 P C  = dt 1

− 2l s1  ˙Θ1 l c  ˙Θ 2 1 1 0

The rotational velocity in the first system can be determined as follows: There is only one rotational  joint, and it rotates about the Z 0 -axis. This means that

1

ω1 =

  0 0 ˙1 Θ

The same result can be obtained using the standard formulas for computing angular and linear velocities. Now we have already determined all values that we need for applying the formulas. We substitute the values into the formula and obtain: 2 1 l2 2 2  l 2 2 ˙ ˙ k1 = m · s1  · Θ1  · + c1  · Θ1  · 2 4 4 1 l2  1 2 ˙ = Θ1 m · + 2 4 2









 1 ˙ 2 + Θ 2 1

And the potential energy computes as:

u1  =  − m ·

T

l

      0 −g 0

c1 · s1  · 0

2

l

2

= gms1

l 2

The reference enery has been ignored here, because, as explained in the recapitulation section, it is not relevant for further computations. In the next step, we compute some intermediary results for evaluating the formula for τ . The overall formula is τ  =

d ∂k ∂k ∂u + . − dt ∂  ˙Θ ∂ Θ ∂ Θ

We compute: ∂k ∂  ˙Θ1 d ∂k dt ∂  ˙Θ1 ∂k ∂ Θ1 ∂u ∂ Θ1

l2 ˙ = Θ1 (m · + 1) 4 l2 ¨ = Θ1 (m · + 1) 4 =0 l = m  · c1  ·  ·  g 2

This yields the overall result l2 l ¨ τ 1  = Θ1 (m ·  + 1) + m · · g  ·  c1 , 4 2 which is the same as the result obtained using the Newton-Euler method.

Solution 2 We start out by computing the kinetic energies: 0

d = (0 P C  ) = dt

vC 

1

1

1

  0 0 d˙1

˙ 1  · ω1 = R · ω0  + Θ 1 0

 0

 i +1

T

1

0 0 0

Z i+1  =

1  1 1 k1 = m1 vC   ·  vC   + 1 ω1 ·  C  I 1  ·  1 ω1  = m1 2 2 2 T

       

1

1

For k2 :

0 0 d˙1

T

0 0 d˙1

1 + 0 = m1 d˙21 2

l2

0

    c2 s 2 2 d1

2

l2

P C  = 2

0

d 0 ( P C  ) = dt

vC  = 2

2

2

  0 0 ˙2 Θ

ω2  =

Using these values in the formulas: 1 k2 = m2 2

− l2 s2  ˙Θ2 l c  ˙Θ 2 2 2 d˙1

 

2

2

T

      2

l2 ˙ Θ2 2

+ d˙

2 1

0 0 ˙2 Θ

 1 + 2

 

I xx2 0 0 0 I yy 2 0 0 0 I zz 2

1 ˙ 22  +  1 m2 d˙21  +  1 I zz 2  ˙Θ22 = m2 l22 Θ 8 2 2

    0 0 ˙2 Θ

Computation of k3  is done analogously: 0

P C  = 3

0

vC  = 3

3

l2 c2  + l2 c23 l2 s2  + l2 s23 d1  ˙ 2 + Θ ˙ 3) −l2 s2  ˙Θ2  − l2 s23 (Θ  ˙ 2  + l c23 (Θ  ˙ 2  + Θ ˙ 3) l2 c2 Θ 2 d˙1

   

3 3

  3

3

˙ 3  ·  1 Z 1 = ω3 = 32 R ·  2 ω2  + Θ

 

 

0 0 ˙ 2  + Θ ˙3 Θ

 

1  l 32 2 2 ˙ ˙ 2  + k3 = m3 d1  + l + + l2 l3 c3 Θ 2 2 4

 

 

l32  l 32 ˙ 2  1 ˙  ˙  ˙ 2  + Θ ˙ 3 )2 + l2 l3 c3 Θ2 Θ3  + Θ3 + I zz 3 (Θ 2 4 2





The potential energy computes as:   · P C  =  − m1 ·  (0, 0, −g) u1  =  − m1  · G  0

1

  ·  0 P C  = m 2 d1 g u2  =  − m2  · G   ·  0 P C  = m 3 d1 g u3  =  − m3  · G

    0 0 d1 −  l1

= m 1 (d1  −  l1 )g

2

3

Thus, we can formulate the total potential energy: u = m 1 (d1  −  l1 )g + (m2  + m3 )d1 g Now we can use the results computed so far in the Lagrangian formulation: τ  =

d ∂k ∂k ∂u − + dt ∂  ˙Θ ∂ Θ ∂ Θ

Since the terms will become rather large, we are not going to compute everything at once, but we will rather split the computation into smaller parts. The joint configuration vector Θ is of form ˙ = (d ˙1 , Θ ˙ 2, Θ ˙ 3 ) etc. For τ , we have τ  = (τ 1 , τ 2 , τ 3 ) . Then, we compute: (d1 , Θ2 , Θ3 ) , so Θ T

T

∂k ∂ d˙1 d ∂k dt ∂ d˙1 ∂k ∂d 1 ∂u ∂d 1

T

= m 1 d ˙1  + m2 d˙1 + m3 d˙1  = (m1  + m2  + m3 )d ˙1 = (m1  + m2  + m3 )d¨1 =0 = m 1 g + (m2  + m3 )g = (m1  + m2  + m3 )g

For τ 1 , we get the following result: τ 1 = (m1  + m2  + m3 )d¨1  + (m1  + m2  + m3 )g Now on to the second component of τ . We compute some intermediary results: ∂k 1 1 l32 2 ˙ 2  ˙ ˙ 2  + = m2 l2 Θ2  + I zz 2 Θ2  + m3 2 l2  + + l2 l3 c3 Θ  ˙ 2 4 ∂ Θ2 4



 

l32 ˙ 3 + I zz 3 (Θ  ˙ 2  + Θ ˙ 3) + l2 l3 c3 Θ 2

 

What follows is the computation of derivatives of above expressions, which is quite cumbersome and complicated. In particular, you need to make sure not to overlook abbreviated terms like c3   etc. Using the product rule for derivatives, we can compute: d ∂k 1 ¨ 2  + I zz 2 Θ ¨ 2+ = m2 l22 Θ dt ∂  ˙Θ2 4 1 l32 2 ¨ 2  + 2 Θ  ˙ 2 −l2 l3 s3 Θ  ˙ 3 m3 2 l2  + + l2 l3 c3 Θ + 2 4 1 l32 ¨ 3  + −l2 l3 s3  ˙Θ3 Θ ˙ 3 + I zz 3 Θ ¨ 2  + I zz 3 Θ ¨3 m3 + l2 l3 c3 Θ 2 2

 

  

   

The other partial derivatives are substantially easier to compute: ∂k = 0, ∂ Θ2

∂u =0 ∂ Θ2

And overall, after rearranging terms, we end up with: 1 l32 2 2 ¨ 2 + τ 2 = m2 l2  + I zz 2  + m3 l2  + + l2 l3 c3 + I zz 3 Θ 4 4 1 − m3 l2 l3 s3  ˙Θ2  ˙Θ3  − m3 l2 l3 s3  ˙Θ23 2





    1 m3 2

l32 ¨3 + l2 l3 c3 + I zz 3 Θ 2

 

Finally, for τ 3  we compute: ∂k 1 = m3 2 ∂  ˙Θ3



l32 l32 ˙ ˙ ˙ 2  + Θ ˙3 + l2 l3 c3 Θ2  + Θ3 + I zz 3 Θ 2 2



 



Again, we differentiate w.r.t time t, and obtain: d ∂k 1 l32 ¨ l32 ¨ ¨  ˙  ˙ ¨ 2  + I zz 3 Θ ¨3 = m3 Θ2  + l2 l3 c3 Θ2  −  l2 l3 s3 Θ3 Θ2  + Θ3 + I zz 3 Θ  ˙ dt ∂ Θ3 2 2 2 ∂k 1  ˙ 3 ) =  − m3 (l2 l3 s3  ˙Θ22  + l2 l3 s3  ˙Θ2 Θ ∂ Θ3 2 ∂u =0 ∂ Θ3





All in all, τ 3  is then: 2 1 l32 ¨ 2  + l3 Θ ¨ 3  −  l2 l3 s3  ˙Θ2  ˙Θ3 + l2 l3 c3 Θ τ 3  = m3 2 2 2 1 ¨ 2  + I zz 3 Θ ¨3 + m3 l2 l3 s3  ˙Θ22 + l2 l3 s3  ˙Θ2  ˙Θ3 + I zz 3 Θ 2

 







Thus, we have finally determined the dynamics equations. One more thing remains to be done: We are supposed to bring the equations in state space form as well as configuration space form. Determining the state space form should be straightforward: ¨ + V  (Θ, Θ) ˙ + G(Θ) τ  = M (Θ)Θ To compute M (Θ), we look for terms that contain the second derivative of any joint variable (so we look for d¨1 ,  ¨ Θ2 ,  ¨ Θ3 ), and we determine the corresponding factors. This yields:

M (Θ) =

 

 

m1  + m2  + m3 0 0 l l 0 ( 14 m2 l22 + I zz 2  + m3 (l22 + 4 + l2 l3 c3 ) + I zz 3 ) ( 12 m3 ( 2 + l2 l3 c3 ) + I zz 3 ) l 1 0 ( 12 m3 ( 2 + l2 l3 c3 ) + I zz 3 ) m3 l32 + I zz 3 4 2 3

2 3

2 3

˙ On to the vector V  (Θ, Θ). It is made up of all terms that contain the first derivative of a joint variable. 0 V  = −m2 l2 l3 s3  ˙Θ2  ˙Θ3  − 12 m2 l2 l3 s3  ˙Θ23 1 m3 l2 l3 s3  ˙Θ22 2

 

 

And finally, the vector G(Θ) contains all terms in which the constant g appears: G(Θ) =

 

(m1  + m2  + m3 )g 0 0

 

To further decompose the equations into configuration space form, we need to take a closer look at ˙ This vector is decomposed as follows: V  (Θ, Θ). ˙ = B(Θ)[Θ  ˙ Θ]  ˙ + C (Θ)[Θ  ˙ 2 ] V  (Θ, Θ) where B= and C  =

 

0 0 0

 

0 0 0 0 0 −m2 l2 l3 s3 0 0 0

 

0 0 1 − 2 m2 l2 l3 s3 0 1 m3 l2 l3 s3 0 2

 

.

The advantage of the configuration space equation is that M,B,C,G  all depend only on Θ. This is important for applications where the dynamic equations change due to movement of the robot: For some settings, it might be easier, faster, or more accurate (depending on the robot’s sensors) to ˙ update the dynamics equations only based on Θ, not on Θ.