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/piInvoking

y = 1.5708 3.1416 iter = 27 alphad = 90.0000 betad = 180.0000