Machine arm problem, done most of it but having trouble inverting newton iteration.

Hello! I'm a student very new to matlab and I'm having some difficulty in my homework.

It concerns a machine arm composed of two segments of lengths r and l, with angle a1 between r and the x axis positive counterclockwise and angle a2 between l and a dotted line that would extend r to infinity positive clockwise.

The one LONG question reads as follows:

1. We consider the robot-arm problem discussed in
class (see picture). A robot arm consisting of two
segments of length r and l is controlled by adjusting
the angles 1 and 2 such that a desired
position (px, py) of the tip of the arm is obtained.
Write the MATLAB code that is necessary to find
the positioning angles 1, 2, given the position of
the end-point of the robot arm, (px, py), and the
lengths of the two segments of the arm, r and l.
You can do this by following these steps:

a. Write a MATLAB m-file rpos that computes the position of the robot arm given the input
parameters r, l, and a vector containing 1 and 2. As its output, the function should return
a single vector-valued variable containing the x- and y-position of the robot arm as its
components.

b. Write a MATLAB m-file drpos that computes the Jacobian matrix of the function rpos,
given the same input parameters as for rpos. As its output, this function should return a
single matrix-valued variable containing the 4 elements of the Jacobian.

c. Write a root-finding program NewtonSys that takes as its input the names of two functions
calculating the vector valued function and its Jacobian, respectively, the values of the
parameters r and l, a vector containing the components px and py of the given robot arm
position, an initial guess for the solution vector, a desired relative error, and the maximum
number of iterations the program is allowed to perform. As its output, NewtonSys should
produce a vector containing the two components a1 and a2 of the solution.

Hints:
 In order to compute the error estimate, calculate the norm of the difference between the
previous vector of the solution estimate and the current one. You can use the MATLAB
function norm for this.
 This last part is easier than it may seem. Apart from the slightly different way to
calculate the error estimate, there is only a single character that needs to be changed
in the function Newton given in Handout V.

d. To show that your program works correctly, calculate the angles 1, 2 for the case r = l =
1, (px, py) = (0.75, 0.5), to a relative error better than 10−8.

For part a, I believe I have it correct with.

function [p] = rpos(r, l, a)
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here
p= [r*cos(a(1)) + l*cos(a(1) + a(2)), r*sin(a(1)) + l*sin(a(1) + a(2))];
end

For part b I just take the derivatives of each of the components of rpos, which gives me:

function [J] = drpos(r,l, a)
%UNTITLED3 Summary of this function goes here
%   Detailed explanation goes here
J=[ -r*cos(a(1)) + l*cos(a(1)+a(2)), -l*sin(a(1)+a(2));
    r*cos(a(1)) + l * cos(a(1)+a(2)), l*cos(a(1) + a(2))]

end

Part c is giving me trouble, as I'm not sure how I'm to invert the function I already have to find a1 and a2. By hand I could do it and it would be a huge chore, but I'm to slightly modify a pre-made Newton iteration which reads as follows:

function [zero]=Newton(f,df,x,es,imax)
% Newton finds the zeroes of the function f by the Newton-Raphson 
% algorithm, given an initial guesses x; it attempts to find an estimate
% for the zero that has an estimated relative error of less than es, 
% and it will perform at most imax iterations in the attempt at finding 
% such a zero.
%
ea=1.;  % ea holds the current estimate for the relative error
iter=0; % iter is the number of iterations performed
while (ea>es & iter<imax)
   iter=iter+1;     % update the iteration counter
   fv=feval(f,x);   % evaluate the function at the current estimate for the root
   dfv=feval(df,x); % and calculate the corresponding derivative of the function
   xn=x-fv/dfv;     % calculate a new estimate for the root
   if xn~=0.        % only if xn is not zero...
      ea=abs((x-xn)/xn);    % calculate an estimate for the relative error
   end
   x=xn;            % update x
end
zero=xn;

I've used this newton to find the zeroes of linear and nonlinear functions for one variable, but never to find components of a vector. The modification is supposedly very simple, but I'm not seeing what I should be changing to make this program do something I've never done before.

Answer 1

[bym]

let's first start with part b, which should be from your definition of p

    p =

[ l*cos(a1 + a2) + r*cos(a1), l*sin(a1 + a2) + r*sin(a1)]

 jacobian(p,[a1,a2])

ans =

[ - l*sin(a1 + a2) - r*sin(a1), -l*sin(a1 + a2)]
[   l*cos(a1 + a2) + r*cos(a1),  l*cos(a1 + a2)]

your J(1,1) is incorrect

[Austin]

Thank you for telling me my derivatives were mistaken. I have changed my function in part b to

function [J] = drpos(r,l, a)

J= [ - l*sin(a(1) + a(2)) - r*sin(a(1)), -l*sin(a(1) + a(2)) ;

l*cos(a(1) + a(2)) + r*cos(a(1)), l*cos(a(1) + a(2))];

end

My version of matlab seems to dislike using the jacobian() function when I actually try to use numbers, but it will work with symbols just fine. I'm supposed to just copy the result that I get using symbols and then put them into a function where they will be treated as double precision, correct?

[bym]

You can use the function matlabFunction() to turn a symbolic equation into a Matlab function

Answer 2

[Austin]

I'm still lost on the way to use the Newton function to invert the position equation to find a1 and a2. Has anyone had experience using iterative methods to invert equations?

[bym]

can you provide some example data for r l and px py? I 'think' given the hint, you can just use the jacobian instead of dfv in the newton function. I would need data to see if this is the case

[Austin]

Sure! The sample data is a test for when r = l = 1, px = .75 and py = .5. I'll try using the jacobian as the dfv.