unable to run pinv (Moore Penrose) with accuracy limit

I am trying to use pinv to allow a robot system to converge.I tried but get errors. This is my script stateent :

dq=double(pinv(X)*(rGoal-r0),1e-4) 

where X is the M.Penrose Jacobian; I am trying to minimize (rGoal-r0) to a tolerance of 1e-4

Answer 1

[John D'Errico]

What do you think the 1e-4 applies to?

dq=double(pinv(X)*(rGoal-r0),1e-4)

Do you think this is a tolerance for pinv? If so, then where did you put it, with respect to the parens? Yes, pinv takes a tolerance, used to define the cutoff on the SVD. But to use that, you need to pass the tolerance into pinv. As it is, you are passing this as a second argument to the function double. And double does not take a second argument.

So you MIGHT have done this instead:

dq=double(pinv(X,1e-4)*(rGoal-r0))

Does that do what you want? I'm not sure, since this is not really how pinv works. It does not do any explicit minimization of your objective, so it will not give you a result that is accurate to your desired tolerance. As such, you may be misusing the tolerance on pinv. But that is your choice to make, not mine.

[Ken]

Thanks. I do want to use that tolerance, so if you have any ideas on where to put it, kindly mention. As mentionedthe idea is to iteratively minimize (rGoal-r0)

[John D'Errico]

I already told you how to pose a tolerance on PINV. I explicitly show how the line would be written in my answer, where that tolerance is passed to PINV. Look at the line of code I wrote.

[Ken]

Thanks. Tried it i.e. dq=double(pinv(X,1e-4)*(rGoal-r0));

get error: Too many input arguments.

[Walter Roberson]

That would happen if your X is symbolic.

Answer 2

[Walter Roberson]

double() does not have any way of applying a tolerance. The tolerance has to be applied at the pinv() level. And whatever tolerance you use will be multiplied by (rGoal-r0) .

Suppose your tolerance for pinv() was 1e-7, but your (rGoal-r0) is 10000, then the uncertainty in the pinv() [because of the tolerance] times 10000 would be 1e-7 * 1e4 --> 1e-3 --- and you would have exceeded your tolerance budget for the overall system.

So you need to calculate

b = (rGoal-r0);
tolerance = 1e-4/max(abs(b),1);
dq = pinv(X, tolerance) * b;

[Ken]

Thanks. I tried and inserted your 3 lines in a while loop

while dq >= 1e-4
    b = (rGoal-r0 );
    tolerance = 1e-4/max(abs(b),1 );
    dq = pinv(X, tolerance) * b ;
    q=q+dq;
    dq=pinv(J_BF_inB)*(rGoal-r0);
end

but got error:

Conversion to logical from sym is not possible.

[Walter Roberson]

Some of your variables are symbolic, but we cannot really tell which at the moment.

I suspect that your J_BF_inB might be symbolic.

Your loop is not updating rGoal or r0 or X or J_BF_inB, so it only has three possibilities:

1. That dq starts out < 1e-4 so the body of the loop never gets executed; OR
2. The first time through the loop, the computed dq is >= 1e-4 so the loop exits after exactly one time; OR
3. The loop is infinite and never exits.

[Ken]

Thanks. Not sure about 2. in your reply. It is not likely to exit the first time as the initial error corresponding to q0 and qGoal would be high enough that it would need a number of iterations till the error goes below 1e-4

I was trying to follow the Newton-Raphson method. I think I should also add a step size, otherwise it will never converge. Not sure if there is an algorithm for this in Matlab.

[Walter Roberson]

Let us look again at your code

while dq >= 1e-4
    b = (rGoal-r0 );
    tolerance = 1e-4/max(abs(b),1 );
    dq = pinv(X, tolerance) * b ;
    q=q+dq;
    dq=pinv(J_BF_inB)*(rGoal-r0);
end

Do you update rGoal or r0 anywhere in the loop? No. So you might as well calcualte b=rGoal-r0 before the loop, leading to

b = (rGoal-r0 );

while dq >= 1e-4
    tolerance = 1e-4/max(abs(b),1 );
    dq = pinv(X, tolerance) * b ;
    q=q+dq;
    dq=pinv(J_BF_inB)*b;
end

Then, because b is not being updated within the loop, that can become

b = (rGoal-r0 );
tolerance = 1e-4/max(abs(b),1 );

while dq >= 1e-4
    dq = pinv(X, tolerance) * b ;
    q=q+dq;
    dq=pinv(J_BF_inB)*b;
end

Is X being updated within the loop? No. So you can do

b = (rGoal-r0 );
tolerance = 1e-4/max(abs(b),1 );
DQ = pinv(X, tolerance) * b ;

while dq >= 1e-4
    dq = DQ ;
    q=q+dq;
    dq=pinv(J_BF_inB)*b;
end

is J_BF_inB being updated within the loop? No. So you can do

b = (rGoal-r0 );
tolerance = 1e-4/max(abs(b),1 );
DQ = pinv(X, tolerance) * b ;
newDQ = pinv(J_BF_inB)*b;

while dq >= 1e-4
    dq = DQ ;
    q=q+dq;
    dq = newDQ;
end

Now, we can "unroll" the loop once.

b = (rGoal-r0 );
tolerance = 1e-4/max(abs(b),1 );
DQ = pinv(X, tolerance) * b ;
newDQ = pinv(J_BF_inB)*b;

if dq >= 1e-4
    dq = DQ ;
    q=q+dq;
    dq = newDQ;
    while dq >= 1e-4
        dq = DQ ;
        q=q+dq;
        dq = newDQ;
    end
end

And then we can optimize...

b = (rGoal-r0 );
tolerance = 1e-4/max(abs(b),1 );
DQ = pinv(X, tolerance) * b ;
newDQ = pinv(J_BF_inB)*b;

if dq >= 1e-4
    q = q + DQ;
    dq = newDQ;
    while newDQ >= 1e-4
         q = q + newDQ;
    end
end

and then we can optimize further

b = (rGoal-r0 );
tolerance = 1e-4/max(abs(b),1 );
DQ = pinv(X, tolerance) * b ;
newDQ = pinv(J_BF_inB)*b;

if dq >= 1e-4
    q = q + DQ;
    dq = newDQ;
    if newDQ >= 1e-4
       q = sign(newDQ) * inf;
       pause(inf)
    end
end

Why? Because

    while newDQ >= 1e-4
         q = q + newDQ;
    end

does not change newDQ and so if it executes at all, executes forever, adding infinite number of newDQ copies, and taking an infinite amount of time to do so.

[Walter Roberson]

https://en.wikipedia.org/wiki/Newton%27s_method

and then gets assigned and the cycle repeats.

Notice that you need to have some parameter (or set of parameters) that you have an initial guess for, and which are to be updates as you go. Notice that you need to evaluate the function at the guess. Notice that you also need to evaluate the derivative of the function at the guess. And then you use the ratio of those values as the correction adjustment to your guess, and iterate.

The code you have been developing has some initial guess, q, but evaluates a constant (J_BF_inB) without using the guess. The code you have been developing never evaluates the derivative of the function at any value. And although you update your guess with something, it does not matter, because your value is unrelated to what your evaluation of the constant J_BF_inB . And is J_BF_inB your function or is it the derivative of your function ???

You will also find that Newton's Method fails if is singular, so the distinction between using pinv() and \ is not relevant here. If the determinant of the denominator is 0 then you should be detecting that and giving up, rather than masking it with pinv(). The cases where pinv(J)*value differs J\value are cases where Newton's Method would fail or diverge, so test and error instead of using pinv()

[Ken]

J_BF_inB is my function. Just wondering if there is a matlab implementation of N.Raphson

[Walter Roberson]

Is J_BF_inB a function with no arguments? Because as I already demonstrated, you cannot take pinv() of a function handle. If it is a function with no arguments, then how is the function written that it is able to find updated values of variables it needs but which are not passed into the function ?

If your J_BF_inB is a symbolic expression, then you still have problems: symbolic expressions will not look in the MATLAB workspace to determine if there are updated values for any variables mentioned in the expression. Not unless you subs(), which you do not do.

... And where is the derivative of your function?

[Ken]

Yes it has i.e. J_BF_inB(alpha,beta,gamma)

"Not unless you subs(), which you do not do." Yes, you are right. Hence thinking of changing to: dq=double(subs(dq,[alphabeta,gamma]))

Derivative of function: Thinking of doing that using Matlab implementation.

[Walter Roberson]

Just wondering if there is a matlab implementation of N.Raphson

Possibly, but it might be a bit difficult to find. MATLAB routines tend to use a modified Newton Raphson, rather than a pure Newton Raphson. The pure Newton Raphson requires more function evaluations for every iteration.

You can find modified Newton Raphson implementations in the File Exchange. Or you could use a function that is designed for this kind of work, such as fsolve()

[Walter Roberson]

Reminder: if you have a symbolic expression, then you can make it into an anonymous function using matlabFunction. For example, let q be a row vector of numeric values, then

JBFun = matlabFunction(J_BF_inB, 'vars', {[alpha, beta, gamma]});

then you can evaluate

JBFun(q) \ b

but... you still need your derivative function to get the f'(x_0), and for that purpose you need to know "derivative with respect to what?"

[Ken]

Derivatives wrt q (3X1 matrix) which is the angular displacement of the robot arms wrt to each other. q0 =pi/180*(0,-30,60)'

[Walter Roberson]

You have a 3 x 3 matrix. The derivative of a 3 x 3 matrix will respect to three variables would give you something that was 3 x 3 x 3.

If J_BF_inB is already the derivative, then you need the original function as well in the computation.

Answer 3

[Ken]

I think the NRaphson is overkill for this, so will go with your code posted at 5PM yesterday:

b = (rGoal-r0 );
tolerance = 1e-4/max(abs(b),1 );
DQ = pinv(X, tolerance) * b ;
newDQ = pinv(J_BF_inB)*b;
if dq >= 1e-4
    q = q + DQ;
    dq = newDQ;
    if newDQ >= 1e-4
        q = sign(newDQ) * inf;
        pause(inf)
    end
end

Why? Because

ThemeCopy

while newDQ >= 1e-4

q = q + newDQ;

end

It gives me error: "Error using svd

First input must be single or double.

Error in solution (line 25)

dq=pinv(J_BF_inB)*b;"

[John D'Errico]

This is NOT an answer to your question. Merely an extension to your question, where you finally show some code. Post it as a comment.

Anyway, you are still completely misunderstanding the point of using a tolerance for pinv.

For example, consider the matrix A:

A = rand(100,1).^(0:25);

A will probably be a numerically rank deficient matrix. Since the matrix has 26 columns, it has a theoretical rank of 26, at least if we were working in super-high precision. But in double precision arithmetic, the computed numerical rank is only 21. Double precision craps out beyond that point.

rank(A)

ans = 21

I'm actually surprised it had as large a rank of 21, I would have expected a rank of around 15. Regardless...

Now I can use pinv, to compute a pseudo-inverse for A. But the tolerance for pinv is not a tolerance that applies for a Newton-Raphson code. The tolererance does nothign more than reduce the rank of the pseudo-inverse. For example

rank(pinv(A))

ans = 21

rank(pinv(A,1e-5))

ans = 12

Would it be a good idea to use that reduced rank pseudo-inverse in a Newton Raphson estimation? NO! ABSOLUTELY NOT! By your choice of a relatively large tolerance for pinv, you are decreasing the ability of the Newton-Raphson iteration to adequately explore the search space. You are NOT imposing a tolerance on the iterations itself. The difference could be significant.

[Walter Roberson]

That's my code... written only to show in detail why I said that there were only three possibilities in the earlier code that the user had posted: that the tolerance was met before any looping, or that the tolerance was met after exactly one loop, or that the loop was infinite. It was never intended to be a solution: it is intended to sin detail what the previous code was equivalent to, and why the earlier code was wrong.

[Ken]

Thanks. Still, the question now is with this code:

b=rGoal-r;

dq=pinv(J_BF_inB)*b;

Why do I get error ?:

Error using svd First input must be single or double.

Error in pinv (line 18) [U,s,V] = svd(A,'econ');

Error in solution (line 25) dq=pinv(J_BF_inB)*b;

[Walter Roberson]

You get that error because you cannot use pinv() on a function handle. As we have told you multiple times.

[Ken]

But you have not suggested how to avoid this.

[Walter Roberson]

Let your J_BF_inB be a function handle that accepts a vector and returns a matrix. Then instead of

 dq=pinv(J_BF_inB)*b;

You would use

dq=pinv(J_BF_inB(q))*b;

This would invoke J_BF_inB on q, returning a numeric matrix, and then pinv() of the numeric matrix would be calculated.

[Ken]

Not clear. Both look same to me.

"Then instead of

dq=pinv(J_BF_inB(q))*b;

You would use

dq=pinv(J_BF_inB(q))*b;"

[Walter Roberson]

Corrected.

[Ken]

Thanks. Tried but get error:

Not enough input arguments.

Error in solution>@(alpha,beta,gamma)[0,-cos(beta+gamma)-cos(beta),-cos(beta+gamma);cos(alpha)*(cos(beta+gamma)+cos(beta)+1),-sin(alpha)*(sin(beta+gamma)+sin(beta)),-sin(beta+gamma)*sin(alpha);sin(alpha)*(cos(beta+gamma)+cos(beta)+1),cos(alpha)*(sin(beta+gamma)+sin(beta)),sin(beta+gamma)*cos(alpha)] (line 12)

0, - cos(beta + gamma) - cos(beta), -cos(beta + gamma);...

Error in solution (line 27)

dq=pinv(J_BF_inB(q))*b;

[Walter Roberson]

You had told us that J_BF_inB took a vector q as its parameter, but it does not.

dq=pinv(J_BF_inB(q(1),q(2),q(3)))*b;

[Ken]

Thanks, this seems to work better as no compile error. Now I get error with the loop i.e.:

qGoal is wrong, your foot position has a position offset to the goal location of: 0.90879, which is larger than 1E-3

This is the program I used:

r=[0,0,0]';
b=(rGoal-r);
tolerance = 1e-4/max(abs(b),1);
%dq = pinv(X, tolerance) * b;
dq=pinv(J_BF_inB(q(1),q(2),q(3)))*b;
while dq >= tolerance
    dq=pinv(J_BF_inB(q(1),q(2),q(3)))*b;
    q=q+dq;
    b=rGoal-r; 
end

[Walter Roberson]

Your dq is going to be a vector. Do you want to stop when one element is within tolerance, or when they all are? What if a negative value is returned?

[Ken]

Stop when all are within tolerance. Carry on if -ve value.

[Walter Roberson]

while any(dq >= tolerance)

[Ken]

Tried, got error:

Undefined function 'mtimes' for input arguments of type 'function_handle'.

Error in solution (line 36)

dr=J_BF_inB*dq;

[Walter Roberson]

I have no idea what code you are looking at now. None of your previous code versions of mentioned dr .

Everywhere you have J_BF_in, except where you created it, you should be using either

J_BF_in(alpha,beta,gamma)

or else

J_BF_in(q(1),q(2),q(3))

depending on what you are doing.

[Ken]

There was one place i.e.dr=J_BF_inB(q(1),q(2),q(3))*dq; where I had not used q(1),2,3 etc.

I changed that but still get same error yOUR FOOT POSITION HAS AN OFFSET OF 0.90879----.

No matter what , this .90879 does not change so may be the loop is not being entered?

while dq >= tolerance
    q(1)=q(1)+1e-4;
    q(2)=q(2)+1e-4;
    q(3)=q(3)+1e-4;
    dq=pinv(J_BF_inB(q(1),q(2),q(3)))*b;
    q=q+dq;
    dr=J_BF_inB(q(1),q(2),q(3))*dq;
    r=r+dr;
    b=rGoal-r; 
end  

[Walter Roberson]

any()

[Ken]

Thanks. Still error but now reduced to .38372.

while any(dq >= tolerance)
    q(1)=q(1)+1e-4;
    q(2)=q(2)+1e-4;
    q(3)=q(3)+1e-4;
    dq=pinv(J_BF_inB(q(1),q(2),q(3)))*b;
    q=q+dq;
    dr=J_BF_inB(q(1),q(2),q(3))*dq;
    r=r+dr;
    b=rGoal-r; 
end  

[Ken]

Made some slight changes but the error b diverges instead of converging:

while n<5
    dq=pinv(J_BF_inB(q(1),q(2),q(3)))*b;
    q=q+dq;
    dr=J_BF_inB(q(1),q(2),q(3))*dq;
    r=r+dr;
    b=rGoal-r; 
    n=n+1;
end  

Answer 4

[Walter Roberson]

I refer you back to https://www.mathworks.com/matlabcentral/answers/1648515-unable-to-run-pinv-moore-penrose-with-accuracy-limit#comment_1982890

At each step:

1. arrange an input vector of data (this might have already been set up in previous steps, but might for example involve putting a series of indivdual values together into a vector)
2. evaluate the function itself itself at the input vector
3. evaluate the hessian of the function at the input vector
4. use \ or pinv() to calculate the ratio -- to calculate getting out a vector of values the same length as the vector of inputs
5. subtract the result of the above step from the current guess to arrive at the updated guess
6. calculate the function at the updated guess
7. if the absolute value of the function at the updated guess is less than tolerance, leave the loop, and otherwise go back to step 1
8. If appropriate, unpack the current guess into the destination variables

If you do a small bit of extra setup before step 1, then you can merge step 2 and step 6.

For the purpose of the above, it does not matter whether you evaluate the function or the hessian by using subs() into a symbolic expression, or invoking a previously-defined function handle with the appropriate inputs, as long as you do the right thing for your representation of the function or hessian.

[Ken]

Trying it with:

q0 = pi/180*([0,-30,60])';//initial value of link angles
rGoal = [0.2,0.5,-2]';
r=[0,0,0];//initial value of link positions
b=rGoal-r;
dq=0;

Not sure how to so step 4. Help appreciated. Thx

[Ken]

After a few weeks and back / forth on this Q & A site, I am not much closer to understanding/solving this problem!

[Walter Roberson]

Okay, so it seems to me that pinv(J_BF_inB(alpha,beta,gamma))*(rGoal-r0) is the function that you want to find the zero of. So use the Symbolic Toolbox (or hand calculations) and multiply that out, to get a vector of three values, each of which is a sum of trig terms. For the moment I will call that f . Now take the derivatives of JB with respect to alpha, beta, gamma, creating the Hessian; call that . Now follow the steps described above https://www.mathworks.com/matlabcentral/answers/1648515-unable-to-run-pinv-moore-penrose-with-accuracy-limit#answer_895320

Answer 5

[Ken]

Still not able to get this to converge. Problem is not sure whether to state a tolerance limit i.e. the foot should be in the goal position with a tolerance or should I iteratively determine if the angle change with each iteration is so small that it has effectively converged (chicken and egg?)

[Ken]

It compiles OK. Just the tolerance is out of whack.

while  sqrt(dr(1)*dr(1)+dr(2)*dr(2)+dr(3)*dr(3)) >= tolerance
dq=pinv(J_BF_inB(q(1),q(2),q(3)))*b;    
    q(1)=q(1)+1e-6;
    q(2)=q(2)+1e-6;
    q(3)=q(3)+1e-6;
    b=rGoal-r; 
    if b<0
        q=q-dq;
    else
        q=q+dq;
        q=q+dq;
    dr=J_BF_inB(q(1),q(2),q(3))*dq;
    if dr<0
        r=r-dr;
    else
        r=r+dr;
    end
    end
end
end
qGoal=q;