Reca
Recapitula
pitulation:
tion: Newto
Newton-Eule
n-Euler-Met
r-Method
hod
The Newton-Euler-Method is a way to determine the dynamics equations that govern the movement The Newton-Euler-Method is a way to determine the dynamics equations that govern the movement of a
of a robotrobot. . The field of The field of analanalytiytical dynamcal dynamicsics, , or more or more bribriefly dynamefly dynamicsics, , is concernis concerned with the ed with the relrela- a-tionship between motion of bodies and its causes, namely the forces acting on the bodies and the tionship 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 properties of the bodies (particularly mass and moment of inertia) influencing that movement. This is in contrast to
is in contrast to kinemakinematics, where we were not tics, where we were not concerned with the physical phenomconcerned with the physical phenomena causing robotena causing robot movement, but only with the movement itself.
movement, but only with the movement itself.
Overall, the purpose of the Newton-Euler-Method is determining the joint torques
Overall, the purpose of the Newton-Euler-Method is determining the joint torques τ τ that are that are required to achieve a desired motion of the robot. More specifially, if we have a joint trajectory Θ( required to achieve a desired motion of the robot. More specifially, if we have a joint trajectory Θ(tt)) that describe the positions and velocities for a certain trajectory of the robot, then the Newton-Euler that describe the positions and velocities for a certain trajectory of the robot, then the Newton-Euler method will allow us to compute corresponding
method will allow us to compute corresponding τ τ ((tt) that will, in an ideal world (no friction or other) that will, in an ideal world (no friction or other disturbances for the moving robot), cause the robot to carry out the desired trajectory.
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: The fundamental physical relationships that are used for deriving this method are:
F
F == m m ˙˙vvC C N
N == C C I I ˙ ˙ωω + + ω ω × × C C IIωω The first
The first equequatioation n is is the the reguregular lar phphysiysical cal rulrule e for for forcforces: es: “F“Force equals mass orce equals mass timtimes es acceaccelerleratioation”.n”. This formula holds for the force that applies to the center of mass of an object. The second equation 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,
is the rotational equivalent of force, the torque. Here, C C I I is the inertia tensor that is computed with is the inertia tensor that is computed with respect to the center of mass, a 3
respect to the center of mass, a 3×× 3 matrix that describes the distribution of masses in a link. The3 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. 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. 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 In the first phase, the velocities and accelerations (both rotational and linear) are computed for each joint.
joint. The The formulas formulas for for the the rotational rotational velocities velocities and and accelerations, accelerations, in in case case of of a a rotational rotational jointjointii + 1, + 1, are: are: ii+1+1 ω ωii+1+1 = = ii+1+1 ii RR · · i iωω ii + + ˙˙ΘΘii+1+1 · · i i+1+1Z Z ii+1+1 ii+1+1ω˙˙ω ii+1+1 = = ii+1+1 ii RR · · i iω˙˙ω ii + + i iii+1+1RR · · i iωω ii × × ˙˙ΘΘii+1+1 · · i i+1+1Z Z ii+1+1 + + ¨¨ΘΘii+1+1ii+1+1Z Z ii+1+1 And if joint
And if joint i i + 1 is prismatic, the formulas simplify to + 1 is prismatic, the formulas simplify to ii+1+1 ω ωii+1+1 = = ii+1+1 ii RR · · i iωω ii ii+1+1 ˙˙ω ωii+1+1 = = ii+1+1 ii RR · · i iω˙˙ω ii
For linear accelerations, the formulas are: (rotational joint
For linear accelerations, the formulas are: (rotational joint ii + 1): + 1): ii+1+1
˙˙vvii+1+1 = =
ii+1+1
ii RR(( iiω˙˙ω
ii × × i iP P ii+1+1 + + i iωωii × × ( (iiωωii × × i iP P ii+1+1) +) + i i˙˙vvii)) If joint
If joint ii + 1 is prismatic, the formula becomes substantially more complicated: + 1 is prismatic, the formula becomes substantially more complicated: ii+1+1
˙˙vvii+1+1 = =
ii+1+1
ii RR(( iiω˙˙ω
ii × × i iP P ii+1+1 + + i iωωii × × ( (iiωωii × × i iP P ii+1+1) +) + i i ˙˙vvii) + 2) + 2 · · i i+1+1ωωii+1+1 × × ˙˙ddii+1+1ii+1+1Z Z ii+1+1 + + ¨¨ddii+1+1ii+1+1Z Z ii+1+1
During the computation, it is necessary to find the linear acceleration of the center of mass of each 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:
link, which can be found using the following relationship: ii˙˙vv
C
C ii = = iiω˙˙ωii × × i iP P C C ii + +
i iωω ii × × ( (iiωωii × × i iP P C C ii) +) + i i˙˙vv ii
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: iF
i = m · i˙vC i
iN
i = C iI i · i ˙ωi + iωi × C iI i · iωi
Based on those forces and moments, we can compute f and n for the joints of the robot: if i = ii+1R · i+1 f i+1 + iF i in i = iN i + ii+1R · i+1 ni+1 + iP C i × iF i + iP i+1 × ii+1R i+1 f i+1
As in the static case, the values of τ i, compute as τ i = inTi ·iZ i for a rotational joint and τ i = if iT·iZ 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:
τ = M (Θ) ¨Θ + V (Θ, ˙Θ) + G(Θ)
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 summands that contain ¨Θ. Similarily, V is determined by factorizing all 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):
τ = M (Θ) ¨Θ + B(Θ)[ ˙Θ ˙Θ] + C (Θ)[ ˙Θ2] + G(Θ)
B(Θ) is a matrix of dimension n × n(n − 1)/2, and [ ˙Θ ˙Θ] is an abbreviation for the vector ( ˙Θ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 products ΘiΘ j with i = j . The matrix C has dimension n × n, and [ ˙Θ2] in an abbreviation for the vector ( ˙Θ2 1, ˙Θ 2 2, . . . , ˙Θ 2 n) 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: 1P
C 1 = (
l
2, 0, 0)
Figure 1: Example robot
• Inertia tensor for the first arm: C 1I
1 = I (Identity matrix) • Gravity acceleration: G = (0, −g, 0)T • Rotation matrix 0 1R: 0 1R =
c1 −s1 0 s1 c1 0 0 0 1
, 1 0R =
c1 s1 0 −s1 c1 0 0 0 1
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 = 10R · 0 ω0 + ˙Θ1 · 1Z 1 = 0 + ˙Θ1 ·
0 0 1
=
0 0 ˙ Θ1
1˙ω 1 = 10R · 0 ˙ω0 + 10R · 0ω0 × 1ω1 + ¨Θ1 · 1Z 1 = 0 + 0 +
0 0 ¨ Θ1
1 ˙v1 = 10R · ( 0 ˙ω0 × 0P 1 + 0ω0 × (0ω0 × 0P 1) + 0 ˙v0) = 1 0R · (0 + 0 + − G) =
s1g c1g 0
1˙v C 1 =
0 0 ¨ Θ1
×
l 2 0 0
+
0 0 ˙ Θ1
×
0 0 ˙ Θ1
×
l 2 0 0
+
s1g c1g 0
=
0 ¨ Θ1 · 2l 0
+
0 0 ˙ Θ1
×
0 ˙ Θ1 · 2l 0
+
s1g c1g 0
=
− ˙Θ2 1 · l 2 + s1g ¨ Θ1 · 2l + c1g 0
The next step is the computation of forces and moments on the centers of mass of the links:
1 F 1 = m1 · 1 ˙vC 1 1N 1 = C 1I 1 · 1 ˙ω1 + 1ω1 × C 1I 1 · 1ω1 =
0 0 ¨ Θ1
And in the backwards iteration, the forces and moments can be determined:
1 f 1 = 0 + 1F 1 1n 1 =
0 0 ¨ Θ1
+
l 2 0 0
×
−m1( ˙Θ21 · l 2 − gs1) m1( ¨Θ1 · 2l + gc1) 0
=
0 0 ¨ Θ1 + 2l · m1( ¨Θ1 · 2l + gc1)
All in all, the result is then:
τ 1 = 1n T 1 · 1Z 1 = ¨Θ1
1 + l2 4 · m
+ l 2 · m · g · c1Solution 1
The transformation matrices for this robot are as follows:
0 1T =
c1 −s1 0 0 s1 c1 0 0 0 0 1 0 0 0 0 1
1 2T =
1 0 0 l1 0 0 1 d2 0 −1 0 0 0 0 0 1
We know that gravity force applies in negative X 0-direction, and we denote the corresponding
equations of this robot. We start with the forward iteration, determining speeds and accelerations: i = 0 : 1ω 1 = 10R · 0ω 0 + ˙Θ1 · 1Z 1 =
0 0 ˙ Θ1
1˙ω 1 = 0 + 0 + ¨Θ1 · 1Z 1 =
0 0 ¨ Θ1
1 ˙v1 = 10R
0 ˙ω0 × 0P 1 +
g 0 0
=
gc1 −gs1 0
1 ˙vC 1 = 1 ˙ω1 × 1 P C 1 + 1 ω1 × (1ω1 × 1P C 1) + 1 ˙v1 =
−l1 2Θ˙ 2 1 + gc1 l1 2Θ¨1 − gs1 0
The next joint is a prismatic joint, which means that we have to use slightly different formulas:
i = 1 2ω 2 = 21R · 1ω1 = 12R T
0 0 ˙ Θ1
=
0 − ˙Θ1 0
2 ˙ω2 = 21R · 1 ˙ω1 =
1 0 0 0 0 −1 0 1 0
·
0 0 ¨ Θ1
=
0 − ¨Θ1 0
2 ˙v2 = 21R ·
1 ˙ω1 × 1P 2 + 1ω1 × (1ω1 × 1P 2) + 1 ˙v1
+ 2 · 2ω2 × ˙d22Z 2 + ¨d22Z 2 = 1 2R T
0 0 ¨ Θ1
×
l1 d2 0
+
0 0 ˙ Θ1
×
0 0 ˙ Θ1
×
l1 d2 0
+
gc1 −gs1 0
+2
0 − ˙Θ1 0
×
0 0 ˙ d2
+
0 0 ¨ d2
= 12R T ·
−d2Θ¨1 l1Θ¨1 0
+
−l1 ˙Θ21 −d2 ˙Θ21 0
+
gc1 −gs1 0
+
−2 ˙Θ1 ˙d2 0 ¨ d2
=
−d2Θ¨1 − l1 ˙Θ21 + gc1 − 2 ˙Θ1d˙2 0 l1Θ¨1 − d2 ˙Θ21 − gs1 + ¨d2
2˙v C 2 = 2 ˙ω 2 × 2P C 2 + 2ω 2 × (2ω2 × 2P 2C ) + 2 ˙v 2 =
0 − ¨Θ1 0
×
0 0 l2
+
0 − ˙Θ1 0
×
0 − ˙Θ1 0
×
0 0 l2
+
−d2Θ¨1 + l1 ˙Θ21 + gc1 − 2 ˙Θ1d˙2 0 l1Θ¨1 − d2 ˙Θ21 − gs1 + ¨d2
=
−(l2 + d2) ¨Θ1 − l1 ˙Θ21 + gc1 − 2 ˙Θ1d˙2 0 −(l2 + d2) ˙Θ21 + l1Θ¨1 − gs1 + ¨d2
Next, we compute the forces and torques that apply to the centers of mass of the links:
1 F 1 = m1 · 1 ˙vC 1 = m1
−l1 2Θ˙ 2 1 + gc1 l1 2Θ¨1 − gs1 0
1 N 1 = C 1I 1 · 1 ˙ω1 + 1ω1 × C 1I 11ω1 =
0 0 I zz1Θ¨1
+
0 0 ˙ Θ1
×
0 0 I zz1 ˙Θ1
=
0 0 I zz1Θ¨1
2F 2 = m2 · 2 ˙vC 2 = m2 ·
−(l2 + d2) ¨Θ1 − l1 ˙Θ21 + gc1 − 2 ˙Θ1d˙2 0 −(l2 + d2) ˙Θ21 + l1Θ¨1 − gs1 + ¨d2
2N 2 = C 2I 2 · 2 ˙ω2 + 2ω2 × C 2I 22ω2 =
0 −I yy2Θ¨1 0
+
0 − ˙Θ1 0
×
0 I zz1 ˙Θ1 0
=
0 −I yy2Θ¨1 0
To slightly simplify the further computations, we will introduce abbreviations for the components of
2F
2 as follows: 2F 2 = (A, 0, B)
T
. Now we can start with the backwards iteration: i = 2 : 2f 2 = 2F 2 2n 2 = 2N 2 + I · 3n3 + 2P C 2 × 2F 2 + 0 =
0 − ¨Θ1I yy2 0
+
0 0 l1
× 2 F 2 =
0 − ¨Θ1I yy2 − m2l1(l1 + d2) ¨Θ1 − l1l1m2 ˙Θ21 + m2l1gc1 − 2m2l1d˙2 ˙Θ1 0
=:
0 C 0
Thus, we have already determined τ 2 = 2f 2T2Z 2 (remember that the second joint is prismatic):
Continuing with the backwards iteration: i = 1 : 1n 1 = 1N 1 + 12R · 2n2 + 1P C 1 × 1F 1 + 1P 2 × 12R · 2f 2 =
0 0 I zz1Θ¨1
+
0 0 −C
+
0 0 m1 l2 1 4Θ¨1 − gm1 l1 2s1
+
l1 d2 0
×
A B 0
=
0 0 I zz1Θ¨1 − C + m1 l12 4Θ¨1 − gm1 l1 2s1 + l1B − d2A
All in all: τ 1 = 1n T 1 1 Z 1 τ 1 = I zz1Θ¨1 − C + m1 l2 1 4Θ¨1 − gm1 l1 2s1 + l1B − d2A = (I zz1 + I yy2 + m2(d2 + l1)2 + m1 l2 1 4 + m2l 2 1) ¨Θ1 + m2l1 ¨d2 +2m2(l1 + d2) ˙d2 ˙Θ1 −
m1 2 + m2
gl1s1 − m2g(l1 + d2)c1And we have finally determined formulas for computation of the joint torques/forces. Now we are supposed to rewrite these in state space form (MVG form). With Θ = (Θ1, d2)
T
, ˙Θ = ( ˙Θ1, ˙d2)
T
, ¨
Θ = ( ¨Θ1, ¨d2)T, τ = (τ 1, τ 2)T, this amounts to:
M (Θ) =
I zz1 + I yy2 + m2(d2 + l1)2 + m1 l2 1 4 + m2l 2 1 m2l1 m2l1 m2
V (Θ, ˙Θ) =
2m2(l1 + d2) ˙Θ1 ˙d2 −m2(l1 + d2) ˙Θ21
G(Θ) =
−( m1 2 + m2)gl1s1 − m2g(l1 + d2)c1 −m2gs1
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: ki = 1 2miv T C i · vC i + 1 2 iωT i · C iI i · iωi
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:
k = n
i=1
ki Another way to compute kinetic energy is
k(Θ, ˙Θ) = 1 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 · 0g
T
· 0P
C i + uref i
Here, g is the vector of gravity, 0P
C i denotes the center of mass of link i, and uref i 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:
τ = d dt ∂k ∂ ˙Θ − ∂k ∂ Θ + ∂u ∂ Θ
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 dt ∂k ∂ ˙Θi − ∂k ∂ Θi + ∂u ∂ Θi .
Simple dynamics example: Lagrange Method
Here, we will revisit the simple example for which we have computed the dynamics using the Newton-Euler 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 =
c1 · 2l s1 · 2l 0
⇒ d dt 0 P C 1 =
−s1 · ˙Θ1 · 2l c1 · ˙Θ1 · 2l 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:
ki = 1 2miv T C ivC i + 1 2 iω i · C iI i · iωi
The first summand contains only the dot product of the velocity with itself, thus the squared length of the vector vC i. 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
0 P C 1 =
l 2c1 l 2s1 0
.The velocity is then determined simply by differentiating with respect to time: d dt 0 P C 1 =
−l 2s1 ˙Θ1 l 2c1 ˙Θ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:
k1 = 1 2m ·
s2 1 · ˙Θ 2 1 · l2 4 + c 2 1 · ˙Θ 2 1 · l2 4
+ 1 2Θ˙ 2 1 = ˙Θ21
1 2m · l2 4 + 1 2
And the potential energy computes as:
u1 = −m ·
0 −g 0
T
c1 · 2l s1 · 2l 0
= gms1 l 2The 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 dt ∂k ∂ ˙Θ − ∂k ∂ Θ + ∂u ∂ Θ. We compute: ∂k ∂ ˙Θ1 = ˙Θ1(m · l2 4 + 1) d dt ∂k ∂ ˙Θ1 = ¨Θ1(m · l2 4 + 1) ∂k ∂ Θ1 = 0 ∂u ∂ Θ1 = m · c1 · l 2 · g
This yields the overall result
τ 1 = ¨Θ1(m ·
l2
4 + 1) + m · l
2 · g · c1,
Solution 2
We start out by computing the kinetic energies:
0 vC 1 = d dt( 0 P C 1) =
0 0 ˙ d1
1 ω1 = 10R · 0 ω0 + ˙Θ1 · i+1Z i+1 =
0 0 0
k1 = 1 2m1v T C 1 · vC 1 + 1 2 1 ωT 1 · C 1 I 1 · 1ω1 = 1 2m1
0 0 ˙ d1
T
0 0 ˙ d1
+ 0 = 1 2m1d˙ 2 1 For k2: 0 P C 2 =
l2 2c2 l2 2s2 d1
0 vC 2 = d dt( 0 P C 2) =
−l2 2s2 ˙Θ2 l2 2c2 ˙Θ2 ˙ d1
2 ω2 =
0 0 ˙ Θ2
Using these values in the formulas:
k2 = 1 2m2
l2 2Θ˙2
2 + ˙d2 1
+ 1 2
0 0 ˙ Θ2
T
I xx2 0 0 0 I yy2 0 0 0 I zz2
0 0 ˙ Θ2
= 1 8m2l 2 2Θ˙ 2 2 + 1 2m2d˙ 2 1 + 1 2I zz2 ˙Θ 2 2Computation of k3 is done analogously: 0P C 3 =
l2c2 + l23c23 l2s2 + l23s23 d1
0 vC 3 =
−l2s2 ˙Θ2 − l23s23( ˙Θ2 + ˙Θ3) l2c2 ˙Θ2 + l23c23( ˙Θ2 + ˙Θ3) ˙ d1
3ω 3 = 32R · 2ω2 + ˙Θ3 · 1Z 1 =
0 0 ˙ Θ2 + ˙Θ3
k3 = 1 2m3
d˙2 1 +
l 2 + l 2 3 4 + l2l3c3
Θ˙2 2 +
l2 3 2 + l2l3c3
Θ˙ 2 ˙Θ3 + l2 3 4Θ˙ 2 3
+ 1 2I zz3( ˙Θ2 + ˙Θ3) 2The potential energy computes as: u1 = −m1 · G · 0P C 1 = −m1 · (0, 0, −g)
0 0 d1 − l1
= m1(d1 − l1)g u2 = −m2 · G · 0P C 2 = m2d1g u3 = −m3 · G · 0P C 3 = m3d1gThus, we can formulate the total potential energy:
u = m1(d1 − l1)g + (m2 + m3)d1g
Now we can use the results computed so far in the Lagrangian formulation: τ = d dt ∂k ∂ ˙Θ − ∂k ∂ Θ + ∂u ∂ Θ
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 (d1, Θ2, Θ3)T, so ˙Θ = ( ˙d1, ˙Θ2, ˙Θ3)T etc. For τ , we have τ = (τ 1, τ 2, τ 3)T. Then, we compute:
∂k ∂ ˙d1 = m1 ˙d1 + m2d˙1 + m3d˙1 = (m1 + m2 + m3) ˙d1 d dt ∂k ∂ ˙d1 = (m1 + m2 + m3) ¨d1 ∂k ∂d1 = 0 ∂u ∂d1 = m1g + (m2 + m3)g = (m1 + m2 + m3)g
For τ 1, we get the following result:
τ 1 = (m1 + m2 + m3) ¨d1 + (m1 + m2 + m3)g
Now on to the second component of τ . We compute some intermediary results: ∂k ∂ ˙Θ2 =1 4m2l 2 2Θ˙2 + I zz2 ˙Θ2 + 1 2m3
2
l2 2 + l2 3 4 + l2l3c3
Θ˙ 2 +
l2 3 2 + l2l3c3
Θ˙ 3
+ I zz3( ˙Θ2 + ˙Θ3)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 dt ∂k ∂ ˙Θ2 =1 4m2l 2 2Θ¨2 + I zz2Θ¨2+ 1 2m3
2
l2 2 + l2 3 4 + l2l3c3
Θ¨ 2 + 2 ˙Θ2
−l2l3s3 ˙Θ3
+ 1 2m3
l2 3 2 + l2l3c3
Θ¨ 3 +
−l2l3s3 ˙Θ3
Θ˙ 3
+ I zz3Θ¨2 + I zz3Θ¨3The other partial derivatives are substantially easier to compute: ∂k ∂ Θ2 = 0, ∂u ∂ Θ2 = 0 And overall, after rearranging terms, we end up with:
τ 2 =
1 4m2l 2 2 + I zz2 + m3
l22 + l2 3 4 + l2l3c3
+ I zz3
Θ¨2 +
1 2m3
l2 3 2 + l2l3c3
+ I zz3
Θ¨3 − m3l2l3s3 ˙Θ2 ˙Θ3 − 1 2m3l2l3s3 ˙Θ 2 3Finally, for τ 3 we compute:
∂k ∂ ˙Θ3 = 1 2m3
l2 3 2 + l2l3c3
Θ˙ 2 + l2 3 2Θ˙ 3
+ I zz3
Θ˙ 2 + ˙Θ3
Again, we differentiate w.r.t time t, and obtain: d dt ∂k ∂ ˙Θ3 = 1 2m3
l2 3 2Θ¨2 + l2l3c3Θ¨2 − l2l3s3 ˙Θ3 ˙Θ2 + l2 3 2Θ¨3
+ I zz3Θ¨2 + I zz3Θ¨3 ∂k ∂ Θ3 = −1 2m3(l2l3s3 ˙Θ 2 2 + l2l3s3 ˙Θ2 ˙Θ3) ∂u ∂ Θ3 = 0 All in all, τ 3 is then:τ 3 = 1 2m3
l2 3 2 + l2l3c3
Θ¨ 2 + l2 3 2Θ¨3 − l2l3s3 ˙Θ2 ˙Θ3
+1 2m3
l2l3s3 ˙Θ 2 2 + l2l3s3 ˙Θ2 ˙Θ3
+ I zz3Θ¨2 + I zz3Θ¨3Thus, 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:
τ = M (Θ) ¨Θ + V (Θ, ˙Θ) + G(Θ)
To compute M (Θ), we look for terms that contain the second derivative of any joint variable (so we look for ¨d1, ¨Θ2, ¨Θ3), and we determine the corresponding factors. This yields:
M (Θ) =
m1 + m2 + m3 0 0 0 (1 4m2l 2 2 + I zz2 + m3(l 2 2 + l2 3 4 + l2l3c3) + I zz3) ( 1 2m3( l2 3 2 + l2l3c3) + I zz3) 0 (1 2m3( l23 2 + l2l3c3) + I zz3) 1 4m3l 2 3 + I zz3
On to the vector V (Θ, ˙Θ). It is made up of all terms that contain the first derivative of a joint variable. V =
0 −m2l2l3s3 ˙Θ2 ˙Θ3 − 12m2l2l3s3 ˙Θ23 1 2m3l2l3s3 ˙Θ 2 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 V (Θ, ˙Θ). This vector is decomposed as follows:
V (Θ, ˙Θ) = B(Θ)[ ˙Θ ˙Θ] + C (Θ)[ ˙Θ2] where B =
0 0 0 0 0 −m2l2l3s3 0 0 0
and C =
0 0 0 0 0 −1 2m2l2l3s3 0 1 2m3l2l3s3 0
.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 ˙Θ.