next up previous
Next: Assessment Up: An Inverse Problem Previous: Newton's Method for

Implementation in Matlab

The implementation in Matlab is straight forward. Very few changes are required to the newton function presented in the previous lesson. This function is named newtonsys for Newton's method for systems.

function [xnew iter]=newtonsys(f, Jacobian, x, xtol, ftol)
%NEWTON Algorithm for approximating the root of a nonlinear system of
%       n-equations.
%       newtonsys(f, Jacobian, x) returns the zero of a function f.
%       f is a string containing the name of a function of n-variables.
%       Jacobian is a string containing the name of the function 
%       that calculate the Jacobian matrix for the function f.
%       x is an n-vector, the starting guess
%       Optional third and fourth arguments set the tolerances
%       for the convergence test
%       
%       For example, newtonsys('sin','cos',3) is  3.1416 (pi). (n=1)
if margin < 4, xtol = 100*eps; ftol = 100*eps; end
error = 2*xtol;
funval = feval(f,x);
fderiv = feval(Jacobian,x);
xnew = x;
iter =0;
while (error > xtol) & (norm(funval) > ftol)
   iter = iter + 1;
   xnew = x - fderiv \ funval; % solves the system fderiv * delx = funval
   error= norm(xnew - x);
   x = xnew;
   funval = feval(f,x);
   fderiv = feval(Jacobian,x);
end

There are a few things to note in the syntax. The definition of the function has two output parameters: [xnew iter]. If one invokes the function as y = newtonsys('sin','cos',3) the result is the value of y = 3.1416. However, [y iter] =newtonsys('sin','cos',3) would yield the correct value of y but also the number of iterations taken to solve the problem.

[y, iter] = newtonsys('sin','cos',3)
 
y =
 
    3.1416
 
 
iter =
 
     4

In our case, we want to compute the function for the inverse problem of the robotic arm. We also need to compute the Jacobian matrix .

function functval=robotarm(xshldr,yshldr,arml,forearml,alpha,beta,xwrist,ywrist)
%ROBOTARM
%        This function returns a vector that contains the differences
%        between the position of the desired position of the wrist
%        and the one computed using arml,forearml,alpha,beta.
xcomput = xshldr + arml * sin(alpha) + forearml * sin(beta-alpha);
ycomput = yshldr - arml * cos(alpha) + forearml * cos(beta-alpha);
functval = [xcomput - xwrist; ycomput - ywrist];

The Jacobian function is also given below:

function matrix = Jacobians(arml,forearml,alpha,beta)
%JACOBIAN
%        This function returns the 2 X 2 Jacobian matrix for the function
%        robotarm.
matrix(1,1)=arml* cos(alpha)- forearml * cos(beta-alpha);
matrix(1,2)= forearml * cos(beta-alpha);
matrix(2,1)= arml * sin(alpha) + forearml * sin(beta-alpha);
matrix(2,2)=- forearml * sin(beta-alpha);

As in the previous chapter, wrapper functions for robotarm and Jacobian are needed. The wrapping function for robotarm is straight forward:

function functval=robotarm2(x)
%ROBOTARM2
%     Wrapper for robotarm.
global xshldr yshldr arml forearml xwrist ywrist;
alpha = x(1);
beta  = x(2);
functval=robotarm(xshldr,yshldr,arml,forearml,alpha,beta,xwrist,ywrist);
The wrapping function for Jacobian is straight forward:
function matrix = Jacobian2(x)
%JACOBIAN2
%        Wrapper of Jacobian.
global xshldr yshldr arml forearml xwrist ywrist;
alpha = x(1);
beta  = x(2);
matrix = Jacobians(arml,forearml,alpha,beta);

A script file may contain those items that define the workspace environment. This file is named invrobotarm.m.

%INVROBOTARM--
%Determine angles for desired X-Y location of wrist.
global xshldr yshldr arml forearml xwrist ywrist;   
yshldr= 1.45; xshldr=0; arml= .343; forearml= .350; ywrist= 1.45; xwrist=.693;
x=[ pi*.8/2;pi*.8]; 
alpha = x(1); beta = x(2);
[y, iter] = newtonsys('robotarm2','Jacobian2',x)           
alphad = alpha *180/pi
betad = beta *180/pi
Invoking invrobotarm.m produces the following output.
y =
    1.5708
    3.1416
iter =
    27
alphad =
   90.0000
betad =
  180.0000


next up previous
Next: Assessment Up: An Inverse Problem Previous: Newton's Method for


J. C. Diaz