Robotics I June 6, 217 Exercise 1 Consider the planar PRPR manipulator in Fig. 1. The joint variables defined therein are those used by the manufacturer and do not correspond necessarily to a Denavit-Hartenberg (DH) convention. The blue arrow indicates in this case the positive increments of the joint variable. y p L q m4 q m2 q m3 B q m1 x Figure 1: A planar PRPR manipulator with the joint variables q m = (q m1, q m2, q m3, q m4 ) defined by the manufacturer. Determine the direct kinematics f m (q m ) for the planar position p R 2 of the end-effector w.r.t. the given (x, y) reference frame, using the joint variables q m provided by the manufacturer. Assign a set of frames according to the DH convention, draw them on the manipulator, and compile the associated table of DH parameters. Choose the origin of frame coincident with that of the manufacturer. Determine again the direct kinematics f(q) for the planar position p R 2 of the end-effector w.r.t. the DH frame, using now the joint variables q of your DH notation. Draw the manipulator in the two zero configurations: the one associated with q m = and the one with q =. Establish the transformation φ( ) needed to map one set of variables into the other, say q m q, so that for any given q m the transformed q = φ(q m ) describes the same physical configuration of the manipulator (in particular, f m (q m ) = f(φ(q m )). Suppose that a joint velocity command q has been determined using routines and control software that operates with the DH variables q. What velocity command q m should be transferred to the low-level controller of the robot manufacturer in order to obtain the expected motion behavior? 1
Exercise 2 With reference to Fig. 2, a laser sensor placed at the base of a 3R robot is monitoring an angular sector of the workspace, scanning it on a horizontal plane at a given height, with minimum and maximum radial sensing distance ρ min and ρ max, respectively. When a human presence is detected by the laser sensor and localized by vector r human, the robot end-effector should be controlled for safety, so as to: either i) retract with a velocity v [i] e along the horizontal and centripetal direction of the workspace, with a scalar speed which is inversely proportional to the distance r human ; or ii) be repulsed with a velocity v [ii] e along an horizontal direction and away from the detected human, with a scalar speed which is inversely proportional to the relative distance between the human and the robot end-effector. The two alternative reactions occur only when r human [ρ min, ρ max ]. Beyond ρ max the robot continues its original task, as specified by a joint velocity q task. Finally, when the human is closer than ρ min to the robot base, the robot should simply stop. Provide the symbolic expression of the commanded joint velocity q that realizes these human-robot coexistence control rules in all the considered cases. y e z pe x e y! min x! max r human Figure 2: Human-robot coexistence, with a laser scanner measuring the position of a human w.r.t. the base of a spatial 3R robot. Exercise 3 A generic revolute joint of a robot should move through the sequence of four knots specified by the angular values q 1 = 7, q 2 = 45, q 3 = 1, q 4 = 1. Define an interpolating path in the form of a cubic spline q = q(s) C 2, for s [, 1], such that at s 1 =, s 2 =.25, s 3 =.5, and s 4 = 1, we have q() = q 1, q(.25) = q 2, q(.5) = q 3, q(1) = q 4. The path tangent q (s) at the initial and final knots is given by (q 2 q 1 )/( ) and (q 4 q 3 )/( ), respectively. Provide the parametric expression and the associated numerical values of the coefficients for the three cubic polynomials in the spline solution. Assuming that the path is executed at a constant speed ṡ =.4/s: what is the total traveling time T from q 1 to q 4? which is the value of the joint velocity at the initial and final time, q() and q(t )? what is the maximum absolute value of the acceleration q(t) reached during the time interval [, T ], and which is the time instant t = t a at which this maximum occurs? [24 minutes, open books but no computer or smartphone] 2
Solution June 6, 217 Exercise 1 A frame assignment for the PRPR manipulator which is consistent with the Denavit-Hartenberg (DH) convention is shown in Fig. 3, and the associated parameters are given in Tab. 1. x 4 p x 3 q 4 x y 4 x 2 x 1 q 3 y 3 q 2 y 1 z 2 z q 1 Figure 3: The DH frames assigned to the planar PRPR manipulator, with associated joint variables. i α i d i a i θ i 1 π/2 q 1 2 π/2 q 2 3 π/2 q 3 4 L q 4 Table 1: Table of DH parameters of the planar PRPR manipulator. Using the joint variables q m = (q m1, q m2, q m3, q m4 ) of the manufacturer, by geometric inspection of Fig. 1 the direct kinematics of the PRPR manipulator is ( ) ( ) px qm1 + (B + q m3 ) sin q m2 + L cos q m4 p = = = f p y (B + q m3 ) cos q m2 + L sin q m (q m ). (1) m4 Similarly, using the D-H variables q = (q 1, q 2, q 3, q 4 ), ( by geometric ( inspection (3 of Fig. 3 (or, computing the sequence of matrix/vector products A 1 (q 1 ) 1 A 2 (q 2 ) 2 A 3 (q 3 ) A 4 (q 4 ) ( 1 ) ))) T from Tab. 1, and then extracting the z and x components) we have ( ) ( ) p z q1 + q 3 cos q 2 L sin(q 2 + q 4 ) p = = = f(q). (2) q 3 sin q 2 + L cos(q 2 + q 4 ) p x The zero configurations of the robot arm when q m = and when q = are obviously different 3
y " p = f m () = $ L% ' # B& x " p = f () = $ % ' # L& x z Figure 4: Zero configurations of the robot arm: when q m = (left) and when q = (right). and are shown in Fig. 4. By comparing eqs. (1) and (2), we find the direct transformation q = q 1 q 2 q 3 q 4 q m1 = π/2 q m2 B + q m3 = φ(q m). (3) q m2 + q m4 π From the inverse transformation, we obtain also the linear mapping from the DH velocities q to the robot joint velocities q m used by the manufacturer: q 1 1 q m = φ 1 π/2 q 2 1 (q) = q q 3 B m = q = J q. (4) 1 q 2 + q 4 + π/2 1 1 Exercise 2 Define the projections along the z axis (on an arbitrary plane, say z = ) for the two vectors and r human = p e = r human,x r human,y h p e,x p e,y p e,z q = J 1 (q)v e, with v e = p e = Π z (p e ) = p e,x p e,y r human = Π z (r human ) = r human,x r human,y where h is the (arbitrary) height of the laser scanning plane. The joint velocity command for the requested robot motion is then given by the following cases:, if r human < ρ min v [i] e = p e p e 1 r human v [ii] e = p e r human p e r human 2,, if r human [ρ min, ρ max ] q task, if r human > ρ max. (5) 4
Exercise 3 Using path parametrization, the three cubic tracts of the interpolating spline are conveniently defined as q A (σ A ) = q 1 + a 1 σ A + a 2 σ 2 A + a 3 σ 3 A, σ A = s s 1 [, 1], s [s 1, s 2 ] (6) q B (σ B ) = q 2 + b 1 σ B + b 2 σ 2 B + b 3 σ 3 B, σ B = s s 2 [, 1], s [s 2, s 3 ] (7) q C (σ C ) = q 3 + c 1 σ C + c 2 σ 2 C + c 3 σ 3 C, σ C = s s 3 [, 1], s [s 3, s 4 ], (8) with the nine coefficients a 1,..., c 3 determined by satisfying the nine boundary conditions q A (1) = q 2, q B (1) = q 3, q C (1) = q 4, q A () = q 2 q 1, q C (1) = q 4 q 3, q A (1) = q B () [ = v 2 ], q B (1) = q C () [ = v 3 ], q A (1) = q B (), q B (1) = q C (). (9) However, the divide et impera approach is more convenient if we assume, for the time being, that we know the value of the first derivatives v 2 and v 3 at the two intermediate knots (at s = s 2 and s = s 3, respectively). The coefficients of each of the three cubic polynomials would then be completely defined by the four local boundary conditions at the two extremes of their interval of definition. Performing computations for the cubic A yields the coefficients a 1 = q 2 q 1, a 2 = (q 2 q 1 ) v 2 ( ), a 3 = v 2 ( ) (q 2 q 1 ), (1) and thus Similarly, for the cubic B q A(1) = 2a 2 + 6a 3 ( ) 2 = 4v 2 4(q 2 q 1 ) ( ) 2. (11) b 1 = v 2 (s 3 s 2 ), b 2 = 3(q 3 q 2 ) (2v 2 +v 3 )(s 3 s 2 ), b 3 = 2(q 3 q 2 )+(v 2 +v 3 )(s 3 s 2 ), (12) and thus and Finally, for the cubic C q B() = 2b 2 ( ) 2 = 6(q 3 q 2 ) ( ) 2 4v 2 + 2v 3 (13) q B(1) = 2b 2 + 6b 3 ( ) 2 = 2v 2 + 4v 3 6(q 3 q 2 ) ( ) 2. (14) c 1 = v 3 ( ), c 2 = 2(q 4 q 3 ) 2v 3 ( ), c 3 = v 3 ( ) (q 4 q 3 ), (15) and thus q C() = 2c 2 ( ) 2 = 4(q 4 q 3 ) ( ) 2 4v 3. (16) Imposing the continuity of the second derivative at the internal knots q A(1) = q B() q B(1) = q C(), 5
and using eqs. (11), (13 14) and (16), leads to the linear system of equations ( ) v2 A = b with ( ) 4(s3 s 1 ) 2( ) A =, b = 2( ) 4(s 4 s 2 ) Replacing the numerical data, the system is solved as ( ) ( v2 = A 1 147.2727 b = v 3 329.99 v 3 6(q 3 q 2 ) + 4(q 2 q 1 ) 4(q 4 q 3 ) + 6(q 3 q 2 ). and the coefficients (1), (12), and (15) of the three cubic polynomials take then the numerical values a = q 1 = 7, a 1 = 115, a 2 = 78.1818, a 3 = 78.1818, b = q 2 = 45, b 1 = 36.8182, b 2 = 156.3636, b 3 = 64.5455, c = q 3 = 1, c 1 = 164.5455, c 2 = 149.99, c 3 = 74.5455. The plots of the interpolating geometric spline q(s), for s [, 1], and of its first and second derivatives q (s) and q (s) are shown in Fig. 5. If the motion on the geometric path q(s) is at constant speed ṡ = k =.4 s 1 ( s = ), then the total time for tracing the interval between s = and s = 1 is simply T = 1/k = 1/.4 = 2.5 s. Moreover, velocity and acceleration of the joint moving at constant speed are q = q ṡ = q k and q = q s + q ṡ 2 = q k 2, respectively. Therefore, the initial and final velocity will be q() = q A() ṡ() = q 2 q 1 k = 184 /s, q(t ) = q C(1) ṡ(t ) = q 4 q 3 k = 72 /s. On the other hand, since the second derivative of a geometric cubic spline is made by linear segments between the knots, its maximum necessarily occurs in one of the knots. Being the joint acceleration simply a scaled version of the second spatial derivative, the maximum (absolute) value of the acceleration is found as max q(t) = t [,T ] k2 max s [,1] q (s) = k 2 max { q A(), q B(), q C(), q C(1) } ), =.16 max { 251.8, 53.6, 1192.7, 596.3 } = max { 4.299, 8.5818, 19.8364, 95.4182 } = 8.5818 /s 2, occurring at the second knot. The plots of the interpolating trajectory q(t), for t [, T ], and of its velocity q(t) and acceleration q(t) are shown in Fig. 6. 6
1 geometric cubic spline 5 [deg] 5.1.2.3.4.5.6.7.8.9 1 parameter s [non dimensional] 4 1st derivative 3 2 1 [deg] 1 2 3 4 5 6.1.2.3.4.5.6.7.8.9 1 parameter s 6 2nd derivative 5 4 3 2 [deg] 1 1 2 3.1.2.3.4.5.6.7.8.9 1 parameter s Figure 5: Geometric spline q(s), with first and second derivative. 7
1 cubic spline position 5 [deg] 5.5 1 1.5 2 2.5 time t [sec] 15 velocity 1 5 [deg/sec] 5 1 15 2 25.5 1 1.5 2 2.5 time t [sec] 1 acceleration 5 [deg/sec 2 ] 5.5 1 1.5 2 2.5 time t [sec] Figure 6: Spline trajectory q(t), with velocity and acceleration. 8