Robotics I February 6, 214 Exercise 1 A pan-tilt 1 camera sensor, such as the commercial webcams in Fig. 1, is mounted on the fixed base of a robot manipulator and is used for pointing at a (point-wise) target in the 3D Cartesian space. The tilt rotation is typically limited to maximum ±9 w.r.t. the vertical axis, or slightly more. The motion of the optical axis of the camera can be described with the Denavit-Hartenberg (DH) formalism, as that of a 2-dof simple robot manipulator. z z x y x y Figure 1: Pan-tilt cameras, with placement of the reference frame RF (shown for two of them) Assign the frames according to the DH convention and provide the associated table of parameters. Use mandatorily the reference frame RF = {x, y, z } as indicated in Fig. 1. One of the axes of the last frame should be coincident with the optical axis of the camera. Determine the unit vector of the pointing axis as a function of the joint angles q R 2. For a given target position p T = ( ) x T y T T z T, with z T (and sufficiently large), determine the value(s) of q that solve the pointing task. Is this inverse kinematics problem always solvable or well defined? Exercise 2 In a pick-and-place task, beside the desired initial Cartesian pose r at time t = t and the final pose r f at t = t f of the robot, it is common to assign two intermediate robot poses, r 1 at t = t 1 and r 2 at t = t 2 (with t < t 1 < t 2 < t f ), so as to shape the total robot motion in three phases: Lift off from r to r 1 ; Travel from r 1 to r 2 ; and, Set down from r 2 to r f. Smoothness of the trajectory is requested, with continuity up to the acceleration at any t [t, t f ]. The first and last phases are in guarded move, and should be performed with caution because of the closeness to environmental surfaces. Therefore, we require also zero velocity and acceleration at the initial and final poses. The four Cartesian poses r, r 1, r 2, and r f have been transformed into four configurations q, q 1, q 2, and q f of a robot with revolute joints by means if its inverse kinematics. axis. 1 Panning refers to left/right rotations around the vertical axis, tilting to up/down rotations around an horizontal 1
Define a 4-3-4 trajectory for a generic joint q addressing the given task. Give the expressions used for each polynomial tract, formulate the problem, and provide the value of all coefficients using: t =, t 1 = 2, t 2 = 4, t 1 = 6 q = q(t ) =, q 1 = q(t 1 ) = 1, q 2 = q(t 2 ) = 8, q f = q(t f ) = 9 [deg]. Hint: You can solve the problem numerically (by a direct method) or analytically (by proper choosing the structure of the polynomials, so as to ease the analysis). Either way is fine. The former approach is rather straightforward, but only if you use a (simple) Matlab code (do not include this, just report the results as requested). The latter is more complex, though quite elegant (you can reduce the problem to the solution of a suitable linear system of two equations in two unknowns, which is indeed solvable in closed form). Exercise 3 Consider a robot manipulator with n joints, its configuration vector q R n, and a task described by r = r(q), with r R m. Assume that m = n and a desired task trajectory r d (t) is given. Prove that the kinematic control law q = J 1 (q) ṙ d + k J T (q) (r d r(q)), (1) with the task Jacobian matrix J(q) = r(q)/ q and a scalar k >, will force any initial error e() = r d () r(q()) at time t = to converge to zero, as long as no singularities of J(q) are encountered. Discuss the above controller in terms of off-line versus on-line computations needed. Next, assume that m < n. How can the kinematic control law (1) be modified in order to guarantee the same previous features? [21 minutes for all exercises; open books] 2
Solutions February 6, 214 Exercise 1 The DH frame assignment is shown in Fig. 2, with the associated parameters in Table 1. Note that a non-zero offset d 1 > is present, since the reference frame cannot be assigned arbitrarily, as specified by the text of the problem (otherwise, we could move up the origin O along z until intersecting the second joint axis). z x 2 z 1 = z 2 O 1 = O 2 x 1 d 1 O x y Figure 2: Assignment of Denavit-Hartenberg frames for a pan-tilt camera i α i a 1 d i θ i 1 π/2 d 1 q 1 2 q 2 Table 1: Denavit-Hartenberg parameters for the pan-tilt camera The optical axis of the camera is the unit axis x 2 of last frame, whose expression is given by 1 cos q 1 sin q 1 cos q 2 sin q 2 1 x 2 = R 1 (q 1 ) 1 R 2 (q 2 ) = sin q 1 cos q 1 sin q 2 cos q 2 1 1 = cos q 1 cos q 2 sin q 1 cos q 2 sin q 2. For the pointing task, observe first that any 3D position p T = ( x T y T z T ) T of a target having z T is mapped into a single value of two task variables, namely the pan angle α and 3
the tilt angle β, as α = ATAN2 { y T, x T }, β = ATAN2 { z T, x 2 T + yt 2 }. (2) When the target is on the (positive) z axis, α is not defined 2. Otherwise, the mapping in (2) is surjective (but not injective, since all Cartesian points on a half-ray from the origin give the same values of (α, β)). If there were no base offset (d 1 = ), these two angles would also coincide with the values (q 1, q 2 ), in the same order, solution of the inverse kinematics problem. Nevertheless, simple geometry shows that the inverse kinematics solution for the two-dimensional pointing task is given by q 1 = ATAN2 { y T, x T }, q 2 = ATAN2 { z T d 1, x 2 T + yt 2 }, (3) except for the singular case y T = x T =, where the angle q 1 is not specified (any value of q 1 satisfies the problem). Note that there is no choice of signs for angle q 2, because of the range limitation of this joint (moreover, the target position is assumed to lie in the Cartesian region with positive values of z T d 1 ). Exercise 2 A numerical procedure for obtaining the solution is detailed in steps A E below. We may call this a brute force approach. A. Define the interpolating polynomials for the three phases L = Lift off (degree 4), T = Travel (degree 3), and S = Set down (degree 4): q L (t) = a L + a L1 t + a L2 t 2 + a L3 t 3 + a L4 t 4 t [t, t 1 ] q T (t) = a T + a T 1 t + a T 2 t 2 + a T 3 t 3 t [t 1, t 2 ] q S (t) = a S + a S1 t + a S2 t 2 + a S3 t 3 + a S4 t 4 t [t 1, t f ]. The 4-3-4 trajectory will be the concatenation of these three polynomials for the motion interval t [t, t f ]: q L (t) for t [t, t 1 ] q 434 (t) = q T (t) for t [t 1, t 2 ] (5) q S (t) for t [t 2, t f ]. B. The 14 unknown coefficients are organized in the vector x T = ( a L a L1 a L2 a L3 a L4 a T a T 1 a T 2 a T 3 a S a S1 a S2 a S3 a S4 ). (6) C. Impose the requested 14 boundary conditions on the 14 coefficients: (4) q L (t ) = q q L (t ) = q L (t ) = q L (t 1 ) = q 1 q L (t 1 ) = q T (t 1 ) q L (t 1 ) = q T (t 1 ) (continuity of velocity and acceleration at t 1 ) q T (t 1 ) = q 1 q T (t 2 ) = q 2 q T (t 2 ) = q S (t 2 ) q T (t 2 ) = q S (t 2 ) (continuity of velocity and acceleration at t 2 ) q S (t 2 ) = q 2 q S (t f ) = q f q S (t f ) = q S (t f ) =. (7) 2 In order to be interpreted as trigonometric values (sine and cosine of q some angle), the arguments q of each of the two ATAN2 functions in the eqs. (2) should be divided, respectively by x 2 T + yt 2 and by x 2 T + yt 2 + zt 2. When x 2 T + yt 2, this division by a (positive) value can be avoided. Instead, for x 2 T = yt 2 =, the pan angle α would remain anyway undefined. So, such divisions are skipped altogether in eqs. (2), while introducing a preliminary warning in (or close to) a singularity. Similar arguments hold for eqs. (3). 4
D. Following the same order, the conditions (7) can be written in matrix form as A x = b, (8) with A = 1 t t 2 t 3 t 4 1 2t 3t 2 4t 3 2 6t 12t 2 1 t 1 t 2 1 t 3 1 t 4 1 1 2t 1 3t 2 1 4t 3 1 1 2t 1 3t 2 1 2 6t 1 12t 2 1 2 6t 1 1 t 1 t 2 1 t 3 1 1 t 2 t 2 2 t 3 2 1 2t 2 3t 2 2 1 2t 2 3t 2 2 4t 3 2 2 6t 2 2 6t 2 12t 2 2 1 t 2 t 2 2 t 3 2 t 4 2 1 t f t 2 f t 3 f t 4 f 1 2t f 3t 2 f 4t 3 f (9) 2 6t f 12t 2 f and b = ( q q 1 q 1 q 2 q 2 q f ) T. It can be shown that matrix A in (9) is always non-singular as long as t < t 1 < t 2 < t f, a condition which is satisfied by assumption. Using the problem data, we have 1 1 2 1 2 4 8 16 1 4 12 32 1 4 12 2 12 48 2 12 1 2 4 8 A = 1 4 16 64 1 8 48 1 8 48 256 2 24 2 24 192 1 4 16 64 256 1 6 36 216 1296 1 12 18 864 2 36 432 and b = ( 1 1 8 8 9 ) T. 5
E. By using Matlab, the linear system of equations (8) is solved as x = A 1 b, providing the desired coefficients of the three polynomials: x T = ( a L a L1 a L2 a L3 a L4 a T a T 1 a T 2 a T 3 a S a S1 a S2 a S3 a S4 ) = (.625.9375 9 125.5 56.25 6.25 99 742.5 191.25 21.875.9375 ). (1) Figure 3 shows the position, velocity, and acceleration of the obtained 4-3-4 trajectory. It can be noted that the trajectory is fully symmetric because of the specific data distribution. As requested, the solution trajectory q 434 (t) is continuous up to the acceleration, and has zero initial and final values of the first two time derivatives. Once the above numerical procedure has been coded (e.g., in Matlab), it can be used for other sets of data. For instance, we can change just the timing through the same knots, i.e., t =, t 1 = 1, t 2 = 4, t 1 = 6 q =, q 1 = 1, q 2 = 8, q f = 9 [deg]. (11) Figure 4 shows the resulting 4-3-4 trajectory. Symmetry of motion is now lost. Matrix A in (9) is clearly sparse. As a matter of fact, no special structure has been exploited in order to reduce the number of independent equations to be solved numerically. A more careful definition of the interpolating polynomials can lead instead to a complete analytical solution in closed form. For this, normalization of time is used in each motion phase, together with a symmetric definition of the first and last quartic polynomials (reversing time for the latter). Moreover, a similar idea is used as in spline interpolation in order to break the problem in three separate parts. The analytical solution is found by the procedure a f below. For the time being, assume that q q 1 and q 2 q f. Special cases will be treated at the end. a. Define the quartic polynomial in the Lift off phase as q L (τ 1 ) = q + (q 1 q ) ( a 13 τ1 3 + a 14 τ1 4 ), τ1 = t t [, 1]. (12) t 1 t It is easy to see that, by construction, q L (τ 1 ) satisfies the three initial boundary conditions at t = t (or, τ 1 = ) on position, velocity, and acceleration. Moreover, q L (1) = q + (q 1 q ) (a 13 + a 14 ) = q 1 a 13 + a 14 = 1. Therefore, for the derivatives and their values at t = t 1 (or, τ 1 = 1) we have and q L (τ 1 ) = q 1 q ( 3 a13 τ1 2 + 4 a 14 τ 3 ) 1 t 1 t q L (τ 1 ) = q 1 q ( 6 a13 (t 1 t ) 2 τ 1 + 12 a 14 τ1 2 ) b. Define the quartic polynomial in the Set down phase as q L (1) = q 1 q t 1 t (3 + a 14 ) (13) q L (1) = 6(q 1 q ) (t 1 t ) 2 (1 + a 14). (14) q S (τ 3 ) = q f + (q 2 q f ) ( a 33 (1 τ 1 ) 3 + a 34 (1 τ 1 ) 4), τ 3 = t t 2 [, 1]. (15) 6
By construction, q S (τ 3 ) satisfies the three final boundary conditions at t = t f (or, τ 3 = 1) on position, velocity, and acceleration. Moreover, q S () = q f + (q 2 q f ) (a 33 + a 34 ) = q 2 a 33 + a 34 = 1. Therefore, for the derivatives and their values at t = t 2 (or, τ 3 = ) we have and q S (τ 3 ) = q f q 2 ( 3 a33 (1 τ 3 ) 2 + 4 a 34 (1 τ 3 ) 3) q S () = q f q 2 (3 + a 34 ) (16) q S (τ 3 ) = q f q 2 ( ) 2 ( 6 a33 (1 τ 3 ) + 12 a 34 (1 τ 3 ) 2) q S () = 6(q f q 2 ) ( ) 2 (1 + a 34). c. For the cubic polynomial in the Travel phase, we choose the symmetric form q T (τ 2 ) = q 2 τ 2 + q 1 (1 τ 2 ) + a 21 τ 2 (1 τ 2 ) 2 + a 22 τ 2 2 (1 τ 2 ), τ 2 = t t 1 t 2 t 1 [, 1]. (18) By construction, q T (τ 2 ) satisfies the interpolating conditions on position in t = t 1 (τ 2 = ) and t = t 2 (τ 2 = 1). Denote at this stage the (yet unknown) velocities at the internal knots as v 1 and v 2, respectively. By imposing q T () = v 1, q T (1) = v 2, the cubic polynomial (18) becomes fully specified as q T (τ 2 ) = q 1 + (q 2 q 1 )τ 2 + [v 1 (t 2 t 1 ) + (q 2 q 1 )] τ 2 (1 τ 2 ) 2 + [(q 2 q 1 ) v 2 (t 2 t 1 )] τ 2 2 (1 τ 2 ), (19) for τ 2 [, 1]. For later use, we compute also the expression of its acceleration: q T (τ 2 ) = 6(q 2 q 1 ) (t 2 t 1 ) 2 (1 2τ 2) + 1 t 2 t 1 (6(v 1 + v 2 ) τ 2 2(2v 1 + v 2 )). (2) (17) d. Using the expressions in (13) and (16), and imposing the equalities q L (1) = v 1, q S () = v 2, we solve for the coefficients of q L (τ 1 ) and q S (τ 3 ) as a 14 = v 1 t 1 t q 1 q 3, a 13 = 1 a 14 = 4 v 1 t 1 t q 1 q (21) and a 34 = v 2 q f q 2 3, a 33 = 1 a 34 = 4 v 2 q f q 2. (22) e. Having tailored the solution as above, we are left only with the problem of finding the correct values v 1 and v 2. For this, the conditions of continuity of the acceleration at the intermediate knots are used, namely q L (1) = q T (), q T (1) = q S (). 7
Using eqs. (14) and (17), substituting therein the coefficients given by (21) and (22), and evaluating (2) at τ 2 = and τ 2 = 1 yields q L (1) = 6v 1 t 1 t 12(q 1 q ) (t 1 t ) 2 = 2(v 2 + 2v 1 ) t 2 t 1 + 6(q 2 q 1 ) (t 2 t 1 ) 2 = q T () q T (1) = 2(v 1 + 2v 2 ) t 2 t 1 6(q 2 q 1 ) (t 2 t 1 ) 2 = 6v 2 + 12(q f q 2 ) ( ) 2 = q S (). (23) After some manipulation, the following linear system of two equations in two unknowns in matrix form is obtained: ( ) ( ) ( ) v1 6 (t2 t 1 ) + 4 (t 1 t ) 2 (t 1 t ) v1 Since M v 2 = = 2 ( ) 6 (t 2 t 1 ) + 4 ( ) 6(q 2 q 1 ) t 1 t + 12(q 1 q ) t 2 t 1 t 2 t 1 t 1 t 6 (q 2 q 1 ) t 2 t 1 + 12 (q f q 2 ) t 2 t 1 = ( n1 n 2 ) v 2 = n. det M = 12 { 3(t 2 t 1 ) 3 + 2(t 2 t 1 ) [(t 1 t ) + ( )] + (t 1 t )( ) }, (25) (24) its solution is ) ( v1 v 2 = M 1 n = 1 det M ( 6 (t2 t 1 ) + 4 ( ) 2 (t 1 t ) 2 ( ) 6 (t 2 t 1 ) + 4 (t 1 t ) ) ( ) n1, (26) n 2 where n 1 and n 2 are defined in (24), and eq. (25) is used for the determinant. The closed form solution of the problem is found by replacing the analytical expressions of v 1 and v 2 in eq. (19) for q T (τ 2 ) and in eqs. (21 22) for q L (τ 1 ) and q S (τ 3 ). f. Using the problem data, we obtain v 1 = v 2 = 22.5 [deg/s] (27) and so a 13 =.5, a 14 = 1.5, a 33 =.5, a 34 = 1.5. Indeed, the resulting trajectory is the same as the one obtained with the numerical method. However, since the symbolic expressions of the used polynomials are different, also the associated numerical values of the coefficients will be different. In Fig. 3, the computed values (27) are shown as magenta dots in the velocity plots. Note also that identical values are obtained for the coefficients of the two quartic polynomials, because of the symmetric nature of the given data. For the alternative data in (11), we obtain instead (see the plots in Fig. 4) v 1 = 21.4184, v 2 = 14.3972 [deg/s] (28) and a 13 = 1.8582, a 14 =.8582, a 33 = 1.126, a 34 =.126. To complete the analysis, we have to consider the degenerate (or singular) cases, namely when q = q 1 and/or q 2 = q f. The numerical approach is totally unaffected by any repetition of values 8
at the knots (i.e., matrix A does not lose rank). On the other hand, in the more tailored analytical approach some changes are needed. In fact, the expressions of the quartic polynomials (12) and (15) would be forced to become constant in such cases (while they should not, unless q = q 1 = q 2 = q f and no motion is needed). In a singular case, it is sufficient to replace the previous definitions by when q 1 = q, and/or by q L (τ 1 ) = q + v 1 (t 1 t ) ( τ1 4 τ1 3 ), τ1 = t t [, 1] t 1 t q S (τ 3 ) = q f + v 2 ( ) ( (1 τ 3 ) 3 (1 τ 3 ) 4), τ 3 = t t 2 [, 1] when q 2 = q f. In this way, we have in particular q L (1) = 6v 1 t 1 t, q S () = 6v 2 that coincide with the expressions in (23) of the accelerations at the intermediate knots, respectively when q 1 = q and when q 2 = q f. Therefore, v 1 and v 2 can be found using (26) as before. For instance, consider the double degenerate case t =, t 1 = 2, t 2 = 4, t 1 = 6 q = q 1 =, q 2 = q f = 9 [deg]. (29) Figure 5 shows the result obtained via the analytical method, yielding again v 1 = v 2 = 22.5 [deg/s] (the same trajectory would be obtained also with the numerical method). From the position profile, we can see that there is some motion also in the first and last time intervals, which is needed to guarantee continuity up to the acceleration in the intermediate knots. However, an under- and over-shooting is present, which makes this particular situation no longer interesting for a collision-free guarded motion of the robot, when close to environmental surfaces. Two different Matlab programs for the numerical solution and for the analytical solution (including treatment of degenerate cases) are available upon request. 9
1 polynomial 4 3 4 trajectory 8 6 [deg] 4 2 2 1 2 3 4 5 6 45 velocity with polynomial 4 3 4 4 35 3 25 [deg/s] 2 15 1 5 5 1 2 3 4 5 6 4 acceleration with polynomial 4 3 4 3 2 1 [deg/s 2 ] 1 2 3 4 1 2 3 4 5 6 Figure 3: Position (top, the red dots are the interpolated knots), velocity (center, the magenta dots are the computed v 1 and v 2 ), and acceleration (bottom) of the 4-3-4 solution trajectory 1
1 polynomial 4 3 4 trajectory 9 8 7 6 [deg] 5 4 3 2 1 1 2 3 4 5 6 3 velocity with polynomial 4 3 4 25 2 [deg/s] 15 1 5 1 2 3 4 5 6 35 acceleration with polynomial 4 3 4 3 25 2 15 [deg/s 2 ] 1 5 5 1 15 1 2 3 4 5 6 Figure 4: Position (top), velocity (center), and acceleration (bottom) of the 4-3-4 solution trajectory for the motion task in (11): Same knots, but different timing 11
1 polynomial 4 3 4 trajectory (singular case) 8 6 [deg] 4 2 2 1 2 3 4 5 6 6 velocity with polynomial 4 3 4 (singular case) 5 4 3 [deg/s] 2 1 1 1 2 3 4 5 6 8 acceleration with polynomial 4 3 4 (singular case) 6 4 2 [deg/s 2 ] 2 4 6 8 1 2 3 4 5 6 Figure 5: Position (top), velocity (center), and acceleration (bottom) of the 4-3-4 solution trajectory for the motion task in (29): Initial and first intermediate knot, as well as second intermediate and final knot have repeated position values 12
Exercise 3 The proof is based on Lyapunov stability, namely on the analysis of the time evolution of the error function V = 1 2 et e = 1 2 (r d r) T (r d r). Using eq. (1) and dropping dependencies, we have V = e T ė = e T (ṙ d ṙ) = e T ṙ d e T J q [ ] = e T ṙ d e T J J 1 ṙ d + k J T (r d r) = k e T JJ T e. (3) As long as J(q) is non-singular, we have J T e = (and so V = ) if and only if e =. Therefore, the controlled robot will be an asymptotically stable system, and the error e(t) will converge to zero from any initial condition e(). Note that, since the closed-loop system is still nonlinear, asymptotic stability (and convergence) will not be exponential in general. The two terms in the control law (1), the first with the inverse Jacobian and the second with the Jacobian transpose, need both to be computed on line (i.e., all vectors and matrices are evaluated at the current configuration q), even if the desired task trajectory r d (t), t [, T ], is completely known in advance for an arbitrary duration T. When the robot is kinematically redundant for the given task (m < n), we can just replace in (1) the inverse of the Jacobian J by its pseudoinverse J #. In fact, as long as the Jacobian is full (row) rank, it is JJ # = JJ T ( JJ T ) 1 = I. Therefore, we obtain the same cancellation of terms as in (3). 13