Module 2: Robotics Interlude - Planning

A challenge with discretizing:

• The arm moves "continuously"
       ⇒ motors that move joints move in very small steps.

• We could discretize, but at what granularity?

Let's first take a naive approach and fully discretize the space:

• Two options:
  • One way to do this is to impose a grid on the space.
  • Another way: define discrete "neighbors" for each state.

• We will use the second approach. For each state and each movable joint:
  • Define eight neighboring positions

    

  • If the coordinates of a joint are (x, y), then the new position is potentially (x+d_x, y+d_y) .
  • Here, d_x is either 0, δ or -δ.
  • Similarly, d_y is either 0, δ or -δ.

Exercise 20: Compare BFS, CBP and A* on the arm problem. Initially, use a simple target (a short distance up the y-axis). Gradually make the target harder.

Exercise 21: Identify the part of the code that computes the neighbors of a state in ArmProblem.java. Change the 8-neighborhood to a 4-neighborhood (N,S,E,W) and compare. In the CBP code, you can un-comment the "draw" line to see what the screen looks like when the algorithm is in action.

Exercise 22: How do we know we have visited a state before? How and where is equality-testing performed in the code? What makes this equality-test different from the test used in the maze and puzzle problems?

Next, let's consider discretizing another way:

• We'll break the arm-movement problem into two levels:
  • A high level that's discrete.
  • A low level that's continuous.

• Key ideas:
  • Use a discrete approach to move the arm from one major configuration to another:
         ⇒ Example: from "straightened" to "folded"
  • Use a continuous "inverse-kinematics" approach for finer-grained movement.

Inverse kinematics:

• Let's look at an example with a two-link arm:
  • Suppose we have an initial position:

    

  • And that we wish to move the gripper horizontally along a line, as shown above.

• Let's now translate this problem into "angle commands"
       ⇒ small changes in angle to enable the desired movement.

• To start, let
  • $\theta_0 \eql$ the angle of the first link from the horizontal.
  • $\theta_1 \eql$ the angle of the second link from the first.

• Now consider a small movement along the desired direction, and ask: what should the new angles be?

  

  That is, what should $\theta_0^\prime$ and $\theta_1^\prime$ be?

• This way, we "feed" the differences to the motors:
  • The first motor (at the origin) is given the "change" $\Delta\theta_0 \eql \theta_0^\prime - \theta_0$
  • The second is given $\Delta\theta_1 \eql \theta_1^\prime - \theta_1$

• Note: some motors are set up to receive the desired target angles $(\theta_0^\prime, \theta_1^\prime)$ instead of the change.

• Let's first write the $(x,y)$ coordinates of the end effector: $$\eqb{ x & \eql & L_0 \cos(\theta_0) + L_1 \cos(\theta_0 + \theta_1)\\ y & \eql & L_0 \sin(\theta_0) + L_1 \sin(\theta_0 + \theta_1) }$$

  Exercise 23: Why is this true?

• In our example, we want the y value to remain constant
       ⇒ horizontal movement of the gripper

• Clearly, there are multiple combinations of $\theta_0,\theta_1$ that will allow for small horizontal movement.

• Let's fix the change in the first angle and then calculate what the second change needs to be.

• Suppose we just pick a (small) value for $\Delta\theta_0$.

• Then, $$\theta_0^\prime \eql \theta_0 + \Delta\theta_0$$

• Next, since the y-value is fixed (and known) we'll use the second equation above: $$\theta_1^\prime \eql \sin^{-1} \left( \frac{y - L_0 \sin\theta_0^\prime}{L_1} \right) \;- \; \theta_0^\prime$$

• Which will result in the change $$\Delta\theta_1 \eql \theta_1^\prime - \theta_1$$

• The two numbers $\Delta\theta_0, \Delta\theta_1$ are the controls applied to the arm.

Now let's look at a slighly more complex task that combines high-level discrete states with low-level small changes to angles:

• Suppose a two-link arm is initially folded so that the end is on the y-axis.

• The final destination: $x=30, y=10$.

• Suppose, further, that high-level planning (such as A*) creates the following high-level sequence of actions:

  

  where:

  • The arm is first nearly straightened as a precursor to folding "the other way".
  • The second link is brought down to $y=10$.
  • The end is now moved along $y=10$ until $x=30$.

  Exercise 24: Within each of the above phases, identify the signs (positive or negative) of $\Delta\theta_0, \Delta\theta_1$. For example, when straightening, what are the signs of each likely to be?

• We'll then set up our control loop to follow the phases:

      // Initially, state=0
      while not over
          if state = 0
              if (straightened) 
                  state = 1       // Go to state 1
              else
                  apply Δθ0,Δθ1 to straighten
              endif
          else if state = 1
              if y = 10
                  state = 2       // Go to state 1
              else
                  apply Δθ0,Δθ1 to bring 2nd link down
              endif
          else if state = 2
              if x = 30
                  stop
              else
                  apply Δθ0,Δθ1 to move horizontally
              endif
          endif
      endwhile
      
      

Exercise 25: Download and unpack twolinkexample.zip.

• Examine the code in TwoLinkController.java to confirm that the above controls are implemented.
• Add code for the last part, to compute $\theta_1^\prime$, or newTheta1 in the code so that the end effector reaches $(30,10)$.
• To execute, run ArmSimulator, type in the controller name "TwoLinkController", set the number of links (to 2), then click on "tracing", then "reset", then "go".

In practice:

• It is not at all straightforward to combine high-level planning and inverse-kinematics.

• Inverse-kinematics calculations are highly dependent on the particular arm or robot.

• Most inverse-kinematics calculations must also take into account other complexities, for example:
  • Forces and torques for smoother movement, for handling loads.
  • Safety: to prevent the arm from crashing into itself.
  • Corrections from drift, when model and reality are out of sync.

• Both planning and inverse-kinematics continue to be active areas of research.

2.7   Other algorithms