============================================================

CS474 -- Sample solution to assignment 1

Zhanyong Wan

2/1/2001

============================================================


1.(a)

The ICC (if any) is the intersection of the roll axes of both
wheels.

Let the length of the frame (i.e. distance between the axles of the
two wheels) be d. Recall this theorem (sine):

  In any triangle ABC,

    BC      AC      AB
  ----- = ----- = -----,
  sin A   sin B   sin C

  where A is the angle at vertex A, BC is the distance between
  vertices B and C, and so on.

Let l_r be the distance between the ICC and the rear wheel. Using this
theorem, we get

             cos alpha_f
  l_r = ------------------------ d
         sin (alpha_f - alpha_r)

Hence the ICC

  (x0, y0) = (x - l_r * cos beta, y - l_r * sin beta),

where beta = theta + alpha_r - (pi/2).


1.(b)

The ICC doesn't exist iff

  alpha_f = alpha_r != pi/2 mod pi

Note that when

  alpha_f = alpha_r = pi/2 mod pi

the ICC can be anywhere along the line connecting the axles of both wheels.


1.(c)

Let l_f/l_r be the distance between the ICC and the front/rear wheel.

As both wheels observe the same angular speed wrt the ICC, we have

  v_f/l_f = omega = v_r/l_r.

Therefore

  v_r = (l_r/l_f)*v_f

By the sine theorem,

   l_r                sin (pi/2 - alpha_f)
  ----- = -------------------------------------------------
   l_f    sin (pi - (alpha_f - alpha_r) - (pi/2 - alpha_f))

           cos alpha_f
        = -------------
           cos alpha_r

Thus

           cos alpha_f
    v_r = ------------- v_f
           cos alpha_r


2.(a)

(In what follows,

    t1   = theta1,
    t2   = theta2,
    ...
    t5   = theta5,
    t23  = theta2 + theta3,
    t234 = theta2 + theta3 + theta4,
    t15  = theta1 + theta5.)

  x 
[ y ] 
  z 

     sin t1           sin t1 * cos t2           sin t1 * cos t23
= [     0   ]*l1 + [           sin t2 ]*l2 + [           sin t23 ]*l3
   -(cos t1)        -(cos t1)* cos t2         -(cos t1)* cos t23

     sin t1 * cos t234
+ [           sin t234 ]*l4
   -(cos t1)* cos t234

    cos t1 * sin t5 + sin t1 * cos t5 * cos t234
+ [                            cos t5 * sin t234 ]*l5
    sin t1 * sin t5 - cos t1 * cos t5 * cos t234

3.

as1RobotModel :: SimbotModel
as1RobotModel d v_max a_max aa_max v_l0 v_r0 =
  let
      v_0    = (v_l0 + v_r0)/2
      v_rd   = limitSymB v_max desiredVRightB
      v_ld   = limitSymB v_max desiredVLeftB
      a      = lift1 signum (v_rd + v_ld - 2 * v) * lift0 a_max
      v      = lift0 v_0 + integralB a
      v_r    = v + v_diff
      v_l    = v - v_diff
      -- the above lines are the same as in Henrik's model

      w_0    = (v_r0 - v_l0)/d        -- initial angular speed (static)
      w_d    = (v_rd - v_ld)/lift0 d  -- desired angular speed (behavior)
      aa     = lift1 signum (w_d - w) * lift0 aa_max
                                      -- angular acceleration (behavior)
      w      = lift0 w_0 + integralB aa
                                      -- angular speed (behavior)
      v_diff = w * lift0 (d/2)        -- half difference between the
                                      --   two wheel speeds (behavior)
  in
      withVelocities v_l v_r