Equation solving

[XDN]

Hi community

I don't know how to use prime to find solutions to a robotic problem this problem is based on matrix calculus for a serial type robot it is easy to determine the direct geometry, i.e. to express the generalized coordinates according to the joint coordinates The difficulty is to solve the inverse geometry, that is to say determine the coordinates of the joints according to the generalized coordinates usually this uses Paul's iterative method I don't know how to solve these equations with Mathcad see the attached sheet

[XDN]

[LucMeekes]

From your description it is not clear to me how q is used in producing Uo. That makes it difficult to find q from a given Uo.

Here's your file in Prime 6, for a broader audience.

Success!

Luc

[Werner_E]

[XDN]

the joint variable q is (in this example)  RZ1 and RZ2

to show the variables,  is necessary to rewrite TG with symbolic parameter

symbolic TJ1:

symbolic TJ2:

Symbolic TG

And finally

sorry it was true that it was not clear

[Werner_E]

The reason for the "solve" command failing could be because you end up with much more single equations compared to just two unknowns.

Furthermore the symbolics doesn't know "deg". You may try to define deg:=pi/180 at the top of the sheet (deg must be labelled as unit) or you use radians throughout.

And I just notice that you demand the impossible:

[XDN]

sorry for my bad explanation I will try to resume more simply with the example of a planar robot

The robot is in solid line

It is easy to determine Direct Geometrical Model

Now, i want to find Inverse Geometrical Model

q=(Fx)

it is obvious on the drawing that several solutions are possible

It is now that the difficulty appears. How to calculate joint variables based on generalized coordinates with Prime I am not necessarily looking for a symbolic solution, I have heard that it is possible to solve this with Newton's numerical method. But I don't know that. Even if this method will produce only one of the possible solutions, maybe it will be possible to refine the algorithm with bounds

This example is simple, but if we add a new segment to the robot it is much more complicated

[XDN]

For example:

we add a pivot on a spatial model.

It is clear that M is invariant for all values of q0 (if q1,q2 are fixed)

so there are many solutions for the joint variables for a single position of M It is now that we could say to the algorithm: Consider Pi/2<q0<Pi for example

[Werner_E]

The problem is that you never set up a clear system with equations where your unknowns are clearly seen.

In the example you just posted I assume that the last line, the 2 x 1 matrix M is given, you know that is must be created by a matrix of the type we see in last but one line (with l1 and l2 as known constants and q1 and q2 as variables to calculate.

Here the math is clear as you have two equations and two variables to solve for.

But in your original example, where I , as I understood it, noted that you demand the impossible (1 is never equal to sqrt 2 /2), it looks like you have 16 equations but still only 2 unknowns.

[XDN]

In the simple example we seek the coordinates in X and Y of the point M according to the geometry l1 and l2 (the constants) and the joint variables q1 , q2

Yes the original model is wrong, I wrote it on paper and I must have made a mistake in the transport, and in the application of the method. That's why I'm starting from a simpler example to correct my model step by step

[XDN]

In the simple example, i find X,Y with blocksolve

here it is clear that for  the generalized coordinates of M(X,Y) BlockSolve gives the joint variables q1,q2

But can Blocksolve provide all the solutions?

here the value in the first calculus

q1=45°

q2=-5°

Are there other ways than going through blocksolve?

[Werner_E]

I also tried and I was surprised that the symbolics is not able to find a solution, not even when provided with the exact symbolic values.

And yes, I also noticed that a solve block is able to provide a solution

And, no, a numeric solve block will only return one specific numeric solution. If the system has more than one solution it depends on the guess values which one you get.

Apart from choosing different guess values you can also add additional constraints to control the output

Sometimes with unexpected results 😉

According different guess value - you can also turn the solve block into a funciton with the guess values as its arguments.

That way you can quickly experiment with different guess values

[XDN]

Hello everyone I'm coming back to an old message, no need to make a new one

It's the same robotics problem as before.

of course the given solutions work, but from an automatic point of view I would like to use an iterative method

I can apply this method by hand, but I have difficulty coding it in a program

Can anyone help me

[Werner_E]

I am not sure about the iteration algorithm  you would like to program, but I guess it sure would help to turn all the calculations into function of the variables you would like to vary.

But why not rely on the algorithms built into Prime?

You may turn a solve block into a function of the target coordinates X.d and maybe also of the arm lengths l.1, l.2.

And for the second solution you may calculate it based on the first one as shown in the picture:

You then could use this function multiple times in your sheet for different targets and maybe also different robot arm lengths and you may also use units

[XDN]

I try it:

I know there are errors in this program but it's a start

@Werner_E 

there are 3 reasons

1/ to download this into a microcontroller I must not use a Prime function

2/this algorithm makes it possible to determine the inverse model with a unique solution for each displacement

3/the Jacobian calculation makes it possible to find the static forces directly (for calculate joint motor)

[XDN]

I therefore calculate step by step the path to the arrival position of the arm

[XDN]

I'm trying to understand how "while" it works

while i>3 is for example

in reality, while must run until convergence...

But already at this stage it is not working. I must be able to display all the Jacobian matrices for each calculation step

[Werner_E]

I don't see what you are actually trying to do, so just a few remarks.

Why do you define X and J as nested matrices? (a 2x1 vector as sole element of a 1x1 matrix).

Why is dX a 1x1 matrix? Isn't it supposed to be a simple scalar (matrix determinant)?

Your while loop runs as long as i is greater than 3. And you increase i in that loop. So once you enter the loop it will run endless!
Fortunately(!?) when you come to that loop, i has the value 1 and so the loop is never entered. So dq remains undefined and I guess your program therefore returns the simple value 0.

What was the problem with the first program you posted? There you used "while (1) " as an endless loop on purpose and breaked out of it using the "break" command.

The condition for break looks a bit strange to me. Should it stop when dq.c[i is near zero?
So that would be something like "if |dq.c[i| < 10^-8 ...".

P.S.: It may be easier to help with the next release of your program if you also attach your worksheet.

[Werner_E]

From what I understood so far you might be looking for something like this

(or do you really need all the intermediate J matrices and dX, dq and q vectors from all iteration steps?).

Prime 9 file attached

[Werner_E · Accepted solution]

Prime does not have a "do ... while" or "REPEAT ... UNTIL" kind of loop which would be more appropriate for this task.

Nonetheless here a shorter version which avoids doubling the calculation of X.

It makes one additional iteration step at the end, though!

[XDN]

Thanks Werner

I will look into this in detail

But is necessary to calculate intermediate matrices

For example, i want to need check the determining of J, because if  this matrix is not invertible i found a singularity position for example when the segments are aligned

And i use J for calculate static forces for all position of the path .

[XDN]

another interest is to make a txt file result:

for example:

q1.txt :with all value of q1 angle

q2.txt : with all value of q2 angle

it's easy if we obtain a matrix result (With this file is possible to drive a virtual servo motor in PTC CREO motion , and is possible to simulate the mechanical arm)

In fact my objective is to obtain a results matrix for each iteration (

this will create the basis for coding on a python microcontroller for example

Werner, thank you again for your superb example program

[Werner_E]

If J is not invertible the whole program would fail at the calculation of dq and you won't get any result from it at all.

One option could be that we check if J is invertible and if its not the program stops and returns the values calculated so far.

Which values would you need as an output. All matrices J, all angles theta 1 and theta2 - anything else?

Actually it would suffice to return only the vectors of the two angles as you could calculate J, X, etc. from them anyway.

Here is my last program, modified to return the vectors of angles and J matrices.
The superfluous extra iteration step of the previous function is now suppressed (the if-statement at the end) and in case the calculation of the next set of angles fails (because of an not invertible matrix J) the program stops, returns the values calculated so far and adds a "NaN" as the last value in each of the returned vectors. So the angles and the corresponded not invertible matrix J are the last but one values in the returned vectors.

You may create the text files with the angle values either using the WRITECSV() function or using WRITETEXT().

Prime 9 sheet attached

[XDN]

Wow 🤗

I don't understand completely your code ...

But , Yes! if det(J)=0  the movemet is not possible

I'm thinking 2 ways

• locally modify the precision to pass through this singular point
• modify the trajectory vector to shift by an arbitrary value to avoid this point

But what you are proposing is already wonderful.

In fact, for output  is what is interesting:

• q1 angle
• q2 angle
•  first joint position (x,y)
• second joint position (x,y)
• speed of joint (wq1, wq2) with [Vx,Vx]=J*[wq1,wq2]  Vx,Vy are fixed (the speed of end arm)
• Torque of joint(Tq1,Tq2) with tau=Jtranspose*F  F are the forces at the end point of arm
• Arm stiffness for position Kext=(Jinv)Transpose*K*Jinv

Great !

You have my complete gratitude

[Werner_E]

Maybe this quick hack can solve the problem with |J|=0.

In case this happens we go back one step and use epsilon/2 for the next step.
After that the given value of epsilon is used again and so I guess this should pass through that point. If thats true, the try...on error at the end would no longer be necessary .

[XDN]

OK

perhaps it is by increasing epsilon that we have a greater chance of passing through the singularity ?

for the rest, I added the necessary data, it's great

there are still unit problems

in this results matrix

m/s is wrong , rpm is correct (this is angular speed)

N is Wrong, Nm is correct (this is a Torque)

I think I'm going to rewrite by removing all the units 🤔

[Werner_E]

---

@XDN wrote:

OK

perhaps it is by increasing epsilon that we have a greater chance of passing through the singularity ?

You have to give it a try and use something like 1.1*epsilon instead of the epsilon/2 I used.
Don't have any test data where the inverse would fail. Just thought that making half a step backwards and then a full step forward again may do the trick, but could not test it.

---

there are still unit problems

in this results matrix

m/s is wrong , rpm is correct (this is angular speed)

N is Wrong, Nm is correct (this is a Torque)

I think I'm going to rewrite by removing all the units 🤔

---

You did not attach your worksheet and your second screenshot is cut off and does not show if you provided a correct length unit for the first function argument (arm lengths) when you called the function. Providing unit-less values would explain the wrong units you experience.

[XDN]

Sorry

i forget to attached worksheet in my previous post

here for unit problem

Unit are not compatible,

I think it's because of the Jacobian ??

maybe it's simpler to remove them and freeze the input units in usual mechanics: MMNKS

[Werner_E]

The parameter "prec" is a threshold for the difference between the current and the desired position as must be a length.

As an alternative, if you want "prec" to stay unitless, you have to change the program accordingly and divide by the desired length unit

[XDN]

of course ! sorry I didn't see it, I'm stupid

thanks ! it's good

next week a make a test to see the behavior on a singularity, for this ,  for this I am trying to solve J=0