Robotics I July 8 Exercise Define the orientation of a rigid body in the 3D space through three rotations by the angles α β and γ around three fixed axes in the sequence Y X and Z and determine the associated rotation matrix R YXZ α β γ. Check if the determinant of this matrix has the correct value. Provide the analytical solutions to the inverse representation problem of an orientation specified by a rotation matrix R = {R ij } using the above angles {α β γ}. Discuss singular cases. Find the mapping between the time derivative φ = α β γ of the above minimal representation and the angular velocity ω of the rigid body. Discuss the invertibility of this mapping. When the desired orientation R d and the desired angular velocity ω d are R d = ω d = [rad/s] determine all associated solutions φ d = {α d β d γ d } and φ d = α d βd γ d respectively to the inverse and the inverse differential problem. Check the correctness of the obtained results. Exercise y P P q q E - E x Figure : A planar RP robot and the generic setup of a desired motion task. Consider the planar RP robot in Fig.. Define a computational scheme that generates joint-space commands at the acceleration level realizing a cyclic trajectory such that: the robot end effector starts at rest from point P at t = and returns there at t = with zero velocity; the path traced by the end effector is a circle of suitable radius passing through the point P and having there its tangent orthogonal to the segment P P ; the timing law along the Cartesian path is a polynomial of the least possible degree. Determine first symbolically and then numerically: the Cartesian velocity v R and acceleration a R when passing through the point P ; the associated joint velocity q R and joint acceleration q R of the RP robot; the numerical values of the above four quantities when using the data.5.5 P = [m] P = [m] = 3. [s]..5 How can the scheme be made robust w.r.t. disturbances and/or initial trajectory errors? [8 minutes open books but no computer or smartphone]
Solution July 8 Exercise he elementary rotation matrices around the three coordinate axes are cos α sin α R Y α = R X β = cos β sin β sin α cos α sin β cos β R Z γ = cos γ sin γ sin γ cos γ. Being the sequence YXZ of rotations defined around fixed axes i.e. of the Roll-Pitch-Yaw type the rotation matrix representing the final orientation of the rigid body is obtained by multiplying the elementary matrices in the following order: R YXZ α β γ = R Z γr X βr Y α cos α cos γ sin α sin β sin γ cos β sin γ sin α cos γ + cos α sin β sin γ = cos α sin γ + sin α sin β cos γ cos β cos γ sin α sin γ cos α sin β cos γ. sin α cos β sin β cos α cos β It is tedious but straightforward to check that det R YXZ = +. Given a rotation matrix R = {R ij } that uniquely specifies the orientation of a rigid body the inverse representation problem is solved as follows. he angle β is given by the analytic formula { } β = AAN R 3 ± R3 + R 33. Provided that R3 + R33 = cos β this formula provides two different solution values β and β depending on the choice of the sign in the second argument. For each of these the following formulas provide an associated solution pair α i γ i for i = : α = AAN { R 3 cos β R 33 cos β } γ = AAN { R cos β R cos β }. 3 herefore in the generic case two different solution triples are found φ = {α β γ } and φ = {α β γ }. In the singular case i.e. when R 3 = R 33 = and thus cos β = the problem reduces to cos α cos γ sin α sin β sin γ sin α cos γ + cos α sin β sin γ R R 3 cos α sin γ + sin α sin β cos γ sin α sin γ cos α sin β cos γ = R R 3. 4 sin β ± When R 3 = sin β = then β = π/ and the set of equations in 4 allows only to specify the sum of the two other angles as α + γ = AAN {R 3 R }. Similarly when R 3 = sin β = then β = π/ and the set of equations in 4 allows only to specify the difference of the two other angles as α γ = AAN {R 3 R }.
he mapping between the time derivative φ = α β γ of the above minimal representation and the angular velocity ω can be obtained in different ways. he easiest is probably to reinterpret eq. as a ZXY Euler sequence with reverse order of angles {γ β α} and to compute the three contributions to ω due to the variation of each angle around its current rotation axis. We have ω = ω γ Z + ω βx + ω α Y = γ + R Z γ β + R Z γr X β = γ + = cos γ sin γ β + cos β sin γ cos γ cos β cos γ sin γ sin β cos β sin γ cos β cos γ sin β α β γ α = β γ φ. α When det = cos β = we have a singularity of the transformation. herefore when β = ±π/ the dimension of the range of drops to two and there exists a one-dimensional subspace of angular velocities ω that cannot be represented by any choice of φ. hese are all ω = ω x ω y ω z R 3 which are orthogonal to the second column of i.e. such that With the data R d = ω x cos γ + ω y sin γ =. ω d = [rad/s] we see that this is not a singular situation R d3. herefore from eqs. 3 we find two solutions to the inverse representation problem: φ d = { } { π α d β d γ d = π } φ d = { } { α d β d γ d = π π π }. Accordingly we obtain two different transformation matrices d β d γ d = d β d γ d = γ d = I and therefore two different solutions to the inverse differential problem namely α d φ d = β d = d β d γ d ω d = [rad/s] φ d = α d β d γ d = d β d γ d ω d = [rad/s]. Indeed we can immediately check that d β d γ d φ d = d β d γ d φ d = ω d. 3
Exercise A generic circle with center at P = x y and radius R is defined by the quadratic equation x x + y y = R. 5 he unknown quantities in 5 can be determined by imposing the passage through the two points P = x y and P = x y i.e. x x + y y = R x x + y y = R whose solution for x y R generates two families of circles with increasing radius placed symmetrically with respect to the line passing through P and P. he additional condition of having a desired value for the path tangent at some point along the path will specify completely one circle in each family. he situation is particularly simple when requiring that the path tangent in P should be orthogonal to the segment P P. In fact this implies directly that the center P lies on this segment and therefore it coincides with its midpoint. A single circle is obtained in this special case with P = P + P = x + x y + y = y x y R = P P x x = + y y. 6 A parametric representation of the circular path 5 is given by x coss + φ ps = + R sins + φ s [ π] 7 where the circular path is traced counterclockwise for increasing s and the angle φ characterizes the starting point chosen on the circle. Being P the starting point i.e. for s = we have x x x cos φ x cos φ + R = R = y sin φ y sin φ y y 8 and thus φ = AAN{y y x x }. 9 he timing law st is given by a cubic polynomial which has in fact the least possible degree that guarantees satisfaction of the four boundary conditions herefore s = s = π ṡ = ṡ =. st = π 3 3 t t. For t = / it is s/ = π and thus pπ = P as it can be easily checked from 6 8. Differentiating twice 7 and respectively in space and time we obtain p s = dps sins + φ = R p s = d ps ds coss + φ ds = R coss + φ sins + φ 4
and herefore ṡt = dst dt = π t ṗt = p sṡt = Rπ t st = d st dt = π t. t t sins + φ coss + φ and thus using also 8 v = ṗ = p πṡ 3Rπ sin φ = = 3π y y. cos φ x x Similarly pt = p s st + p sṡ t = Rπ 44Rπ t sins + φ coss + φ t t coss + φ sins + φ 3 and thus being s/ = and using 8 a = p = p πṡ 9Rπ cos φ = = 9π x x sin φ. 4 y y Having fully specified the Cartesian trajectory and obtained in particular the requested quantities v and a consider now the execution of this trajectory by the RP robot. At t = the robot end effector should be placed in P. Solving this inverse kinematics problem we have q AAN{y x } q = = 5 q x + y where for simplicity we considered only the robot configuration facing point P i.e. with q >. he robot should start at rest and so q =. From this initial robot state which is matched with the Cartesian trajectory at the initial time t = we generate the desired cyclic trajectory using the acceleration command obtained by solving the second-order inverse differential kinematics or qt = J qt pt Jqt qt t [ ] 6 where pt is given by eq. 3 and we assumed that kinematic singularities are not encountered. he analytic Jacobian J and its time derivative J needed within 6 are found by differentiating the direct kinematics of the RP robot q cos q p = fq = q sin q yielding q sin q cos q ṗ = Jq q with Jq = q cos q sin q 5
and p = Jq q + Jq q with Jq = q sin q q q cos q q sin q q cos q q q sin q q cos q. When passing through the point P the robot configuration is q q AAN{y x } = q =. 7 x + y hus the joint velocity at t = / when the robot end effector is in P will be q = J q ṗ = J q v = 3π sin q q q cos q cos q q sin q y y x x 8 where the values of q / and q / are given by eq. 7 and we used v as defined in. Similarly the requested joint acceleration at t = / is q = J q p Jq q = J q a Jq q [ = q sin q cos q q cos q q sin q q sin q q q cos q q cos q q q sin q 9π x x y y q sin q q cos q q ] where the values of q / and q / are given by eq. 7 q/ comes from eq. 8 and we used a as defined in 4. Substituting the numerical values.5 P = [m] P =.5 [m].5 = 3. [s] we obtain from 6 and 9 x.5 P = = [m] R = =.559 [m] φ = 6.565 =.4636 [rad].5 y and thus from 7 ps = We evaluate then and 4 obtaining v = coss.4636 +.559 s [ π]..5 sins.4636.7363 4.337 [m/s] a = [m/s ]..476.686 9 6
he robot configurations at the initial time t = and at the halftime t = / =.6 s are computed from eqs. 5 and 7 q = while 8 and 9 yield q.6 =.588 [rad].88 [m].473 [rad/s].699 [m/s] q.6 = q.6 =.49 [rad].58 [m].735 [rad/s ].655 [m/s ] he planned Cartesian task is sketched in Fig. while the evolution of the joint positions and velocities associated to the computational scheme 6 are shown in Figs. 3 and 4.. y P v a P R P q q x Figure : he planned Cartesian motion with the RP robot in the correct initial position at t =.. robot joint variables.8 joint/blue [rad] joint/red [m].6.4..8.6.4.5.5.5 3 time s Figure 3: Joint positions of the RP robot while executing the task in Fig.. 7
.5 robot joint velocities joint/blue [rad/s] joint/red [m/s].5 -.5 - -.5 -.5.5.5 3 time s Figure 4: Joint velocities of the RP robot while executing the task in Fig.. Finally if there is an initial mismatch of the robot state with respect to the desired Cartesian trajectory p d t at t = or if an external disturbance will bring the robot end effector out of its nominal trajectory a feedback modification of the scheme 6 is needed. In order to recover the tracking errors the following control law should be used q = J q p d + K D ṗ d Jq q + K P p d fq Jq q with diagonal gain matrices K P > and K D > and where the desired motion in position velocity and acceleration is specified by 7 and 3 respectively. he control law robustifies the robot behavior also with respect to numerical approximations due to a discretetime implementation of the scheme. 8