Numerical Methods Lab 4

Objectives

In this lab you will explore the use of non-linear optimization on a problem that occurs in robotics/animation. In particular, you will evaluate Newton's and Broyden's method for non-linear systems of equations in inverse kinematic applications.

Background

Given a kinematic chain (a set of joints that are joined by links), forward kinematics is the problem of determining the position of the joints (or only the end-effector) given the joint angles. This problem is a straighforward application of Euclidean transformations.

Inverse kinematics is the inverse problem: determine the joint orientations that satisfy a given a set of constraints (e.g., find the joint orientations that put the end-effector at a specific location). This problem is harder than forward kinematics; the analytic solution is often complicated or doesn't exist so numerical solutions are required. Furthermore, it is possible that the solution is not unique. There may be no solutions or several solutions.

Exercises

Figure 1: An example of a two link planar manipulator with $ {\bf l}=[1;1]$ and $ {\bf theta}=[\pi /4;\pi /4]$.
Image twolink

Matlab Path

presentation.pdf
Please addpath /cshome/vis/data so that you get access to the different resources required for this lab. If you are using your own computer you might have to test the last part at the labs to make sure everything is working properly.

Part 1

In the first part of this lab you will focus on a 2D planar manipulator with two links and two rotational degrees of freedom (d.o.f.) (see Fig. [*]). We will parameterize our manipulator with a vector of two link lengths ( $ {\bf l}=[l_1,l_2]^T$) and a vector of angles $ {\bf theta}=[\theta_1,\theta_2]^T$.

The non-linear equations for positioning the end effector at a point $ {\bf p}=[x, y]^T$ are

$\displaystyle f_1(\theta_1,\theta_2)$ $\displaystyle =l_1 \cos(\theta_1) + l_2 \cos(\theta_1+\theta_2) - x$ (1)
$\displaystyle f_2(\theta_1,\theta_2)$ $\displaystyle =l_1 \sin(\theta_1) + l_2 \sin(\theta_1+\theta_2) - y$ (2)

  1. Write a function [pos,J]=evalRobot2D(l,theta) that returns the position, $ {\bf pos}$ ( $ 2 \times 1$ vector), of the end-effector and the Jacobian, $ {\bf J}$, given the rotation angles theta and the fixed link lengths l.
  2. Write a function J=fdJacob2D(l,theta,alpha) that uses central differences to compute the Jacobian using step-size alpha. Evaluate several different values for alpha by comparing the finite difference Jacobian to the analytic Jacobian for a variety of values for $ {\bf theta}$.
  3. Write a function theta=invKin2D(l,theta0,pos,n,mode) that returns the rotation angles in a $ 2 \times 1$ vector $ {\bf theta}$ that put the end-effector at $ {\bf pos}$. Your method should terminate when the constraint is satisfied (use a threshold on the norm of the residual) or when the $ n$ iterations have occured. When mode=0 you should use Newton's method, and mode=1 you should use Broyden's method
  4. Use a script similar to the eval2D.m to evaluate the effect of mode. You can use our plotRobot2D.m function to plot.


Part 2

3D Arm.

The system of nonlinear equations for the position of end effector (Forward Kinematics) $ {\bf p}=[x, y, z]^T$ is given by the equations below. Unlike the planar manipulator, we also have rotations in the z direction given by $ \theta_3$.

(1)
(2)
(3)

  1. Firstly, try to plot the robot using the routine plotRobot3D(l,theta). The given routine [pos,J] = evalRobot3D(l, theta) returns the position pos of the end effector and the Jacobian J of the nonlinear system of equations given above, given the rotation angles theta and the link lengths l. Use the above method to display the Jacobian for the following configurations and explain what happens:

  2. Design a routine theta=invKin3D(l,theta0,desired,n,mode) as in Exercise 1 to obtain the joint angles for a ‘desired’ end effector position. When mode=0 you should use Newton's method, and mode=1 you should use Broyden's method. Test your algorithm on random points near the end effector and check the following conditions:

  3. Implement the pick-up and place task in the eval3D.m file using the methods discussed in Exc.2. Watch the demo video to get an idea of the required result. Move all the cans from the table to the conveyor belt.
    Note: The target points at the conveyor belt and the location of cans at the table are provided in can_positions.mat .

(Optional - LEGACY) Part 3

Image adam_1 Image adam_2 Image stick

In the second half of this lab you will extend your 2D implementation to 3D. We will be animating two legs (independently) for a human.

Assume that we can parameterize each leg with four degrees of freedom. The location of the end-effector is then the homogeneous zero transformed by the a sequence of transformations.

$\displaystyle f({\bf\theta}) = {\bf M}_{hip}{\bf R}_z(\theta_3){\bf R}_y(\theta... ...\bf R}_x(\theta_1){\bf M}_{knee}{\bf R}_{x}(\theta_4){\bf M}_{foot} [0,0,0,1]^T$ (3)

Where $ {\bf M}_*$ is a rigid transformation encoding the rest position and orientation of a joint in the space of its parent, and may be different for each leg.

As input, all your functions should take $ {\bf M}=\{{\bf M}_{hip},{\bf M}_{knee},{\bf M}_{foot}\}$ (a cell array) as its first parameter, followed by the joint angles $ {\bf theta}=[\theta_1,\theta_2,\theta_3,\theta_4]'$.

  1. Write a function [pos,J]=evalRobot3D(M,theta) that returns the position and Jacobian for a set of rotation angles. Note: using the homogeneous representation (Equation 3) your function should discard the homogeneous component of the vector, i.e., pos should be $ 3 \times 1$ The homogeneous component should always be 1 as M contain only Euclidean transformations.
  2. Write a function theta=invKin3D(M,theta,pos) that returns the inverse kinematic solution for pos.
  3. Test your function on the data sequences provided in the data directory (walk1.mat,walk2.mat,walk3.mat,...,jump1.mat). Each .mat file contains a L and R variables that are $ 3 \times t$ matrices containing a time sequence of positional constraints for the left and right legs respectively.

Appendix

Resources

Here are two videos that show the expected output of the animation of walk1 sequence: human rendering , stick figure rendering.
Add /cshome/vis/data to your path or copy to a local directory if you want to modify. The first part of the lab: The second part:

Homogeneous matrix transforms:

\begin{displaymath} {\bf R}_x(\theta)= \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 ... ...a) & 0 & cos(\theta) & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \end{displaymath}

\begin{displaymath} {\bf R}_z(\theta)= \left[ \begin{array}{cccc} cos(\theta) & ... ...0 & y \\ 0 & 0 & 1 & z \\ 0 & 0 & 0 & 1 \end{array}\right] \end{displaymath}

The regular rules of calculus apply to matrix products as long as you are aware that you cannot commute elements. The product rule for matrices:

$\displaystyle \frac{\partial }{\partial \theta} A(\theta) B(\theta) = (\frac{\... ...\theta)}{\partial \theta}) B + A (\frac{\partial B(\theta) }{\partial \theta}) $

Notice that when $ B$ is not a function of $ \theta$ you get:

$\displaystyle \frac{\partial }{\partial \theta} A(\theta) B = (\frac{\partial A(\theta)}{\partial \theta}) B $

An example of how to compute the Jacobian : Jacobian_example