Homework #5 Due 10/11

1. Consider our 3DOF robot shown below, together with the model used for the camera-space kinematics of point P:

x_c = f_x = (C₁²+C₂²-C₃²-C₄²) {[cosq₂+cos(q₂-q₃)]cosq₁} +

2(C₂C₃+C₁C₄) {[cosq₂+cos (q₂-q₃)]sinq₁ - 1} + 2(C₂C₄-C₁C₃) {sinq₂+sin(q₂-q₃) - 1} + C₅

y_c = f_y = 2(C₂C₃-C₁C₄) {[cosq₂+cos(q₂-q₃)]cosq₁} +

(C₁²-C₂²+C₃²-C₄²) {[cosq₂+cos (q₂-q₃)]sinq₁ - 1} + 2(C₃C₄+C₁C₂) {sinq₂+sin(q₂-q₃) - 1} + C₆

The batch of samples is as given above. Let k denote the row number. So the camera-space sample of P of the k^th row where k=3 is (73 , 14).

(a) Letting q₁^k q₂^k q₃^k be the joint rotations in radians for the k^th row of batch data, write out the general expressions, in terms of the current guess C_1o C_2o … C_6o together with q₁^k q₂^k q₃^k, for the odd rows, i.e. h_i, i=2k-1, as defined in class.

h_i(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = h_2k-1(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = …

(b) Again, letting q₁^k q₂^k q₃^k be the joint rotations in radians for the k^th row of batch data, write out the general expressions, in terms of the current guess C_1o C_2o … C_6o together with q₁^k q₂^k q₃^k, for the even rows, i.e. h_i, i=2k. h_i(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) =

= h_2k(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = …

2. We wish to improve upon our initial guess of [C₁ C₂ … C₆]^T = [C_1o C_2o … C_6o]^T by using:

(a) Letting q₁^k q₂^k q₃^k be the joint rotations in radians for the k^th row of batch data, as before, write out the general expressions, in terms of the current guess C_1o C_2o … C_6o together with q₁^k q₂^k q₃^k, for the odd rows of the matrix A, i.e. i=2k-1. A_i,1(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = A_2k-1,1(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) ; A_i,2(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = A_2k-1,2(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) ; A_i,3(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = A_2k-1,3(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = …

(b) Again, letting q₁^k q₂^k q₃^k be the joint rotations in radians for the k^th row of batch data, write out the general expressions, in terms of the current guess C_1o C_2o … C_6o together with q₁^k q₂^k q₃^k, for the even rows, i.e. h_i, i=2k. A_i,1(q₁ⁱ q₂ⁱ q₃ⁱ C_1o C_2o … C_6o) = A_2k,1(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) ; A_i,2(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = A_2k,2(q₁ⁱ q₂ⁱ q₃ⁱ C_1o C_2o … C_6o) ; A_i,3(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = A_2k,3(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) …

3. Letting q₁^k q₂^k q₃^k be the joint rotations in radians for the k^th row of batch data, as before

(a) write out the general expressions, in terms of the k^th camera-space sample (x_c^k y_c^k), the current guess C_1o C_2o … C_6o together with q₁^k q₂^k q₃^k, for the odd elements of the vector of residuals, i.e. i=2k-1. r_i(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = r_2k-1(q₁^k q₂^k q₃^k C_1o C_2o … C_6o)

(b) Again, in terms of x_c^k y_c^k C_1o C_2o … C_6o q₁^k q₂^k q₃^k, write out the general expressions for the even elements of the vector of residuals, i.e. i=2k. r_i(q₁^k q₂^k q₃^k C_1o C_2o … C_6o) = r_2k(q₁^k q₂^k q₃^k C_1o C_2o … C_6o)

4. Write code to implement Euler-Gauss iteration and converge onto the best equal-weighting solution of C for these data. Begin with an initial guess of your choice; note that if the elements of r do not become very small you may need to retry your initial guess. List both your converged C elements as well as the corresponding final (converged) values of the 18 elements of the residuals r.