Mr Jos

Need help cleaning math calculations for better cycletime

Recommended Posts

Hey, I tried to find the bottleneck slowing down my program and it appears to be this Forward Kinematics being calculated every re-routing to a next point between the waypoints. It takes around 45-80ms of the 65-105ms needed to get new speeds for the motors calculated.

The sub-routine is completely on screen and is running in a While True: loop with only that routine.

51165023202_f4cbf30c33_o.png

If anyone would know how to speed up this proces feel free to let me know.

Share this post


Link to post
Share on other sites

51165994213_fab6ea4052_o.png

Moved all the math.radians out to the top and seems I gained a little bit of cycletime, meanwhile cleaned it up to see how the calculation actually looks like.. perhaps there is more time to save by eliminating some of the double cos/sin calculations.. but each one of them is needed for correct XYZ positioning in millimeters...

 

Share this post


Link to post
Share on other sites

I'm not a very skilled programmer myself, but I think the first thing to do is to store the results of the cosine and sine calculations to a separate variable that you can use, and only recalculate them when needed.  You're calling those trig functions dozens of times when you only need to call them maybe twelve times, once for sine and cosine for each of the base and arm pitch, roll, and yaw angles.  Can you describe the project a bit more?

Share this post


Link to post
Share on other sites
42 minutes ago, icm said:

I'm not a very skilled programmer myself, but I think the first thing to do is to store the results of the cosine and sine calculations to a separate variable that you can use, and only recalculate them when needed.  You're calling those trig functions dozens of times when you only need to call them maybe twelve times, once for sine and cosine for each of the base and arm pitch, roll, and yaw angles.  Can you describe the project a bit more?

I'll give that a try, and the project is my 6DoF v2 arm

 

Share this post


Link to post
Share on other sites

Why do you need to calculate the fork position using forward kinematics? Surely the position is already stored somewhere? 

Share this post


Link to post
Share on other sites
2 hours ago, icm said:

first thing to do is to store the results of the cosine and sine calculations to a separate variable that you can use, and only recalculate them when needed.  You're calling those trig functions dozens of times when you only need to call them maybe twelve times, once for sine and cosine for each of the base and arm pitch, roll, and yaw angles.

And the answer to that;

51166815789_b7ea7cb02e_o.png

At least half the cycletime gone for this inverse kinematic calculation. To late now to test with the arm itself, but will do that tomorrow, and try to finetune again.

Just now, ord said:

Why do you need to calculate the fork position using forward kinematics? Surely the position is already stored somewhere? 

No, I don't hard code the angles my motors need to go to. I just send them the angle they need to try to reach, and then adjust on the fly and send them new angle and speed to reach that angle with all 6 motors at the same time. If I don't calculate with forward kinematics I don't know how far each motor got when I send new coordinates+speed. If 1 motor is 50% far and other 5 at 75%, and I send new coordinates without forward kinematics used, the motor behind would not be speeded up. With these forward kinematics calculation it "sees" that the motor is behind, sends the new coordinates and sets a higher speed then it normally would have. This makes that I can use the PS4 controller as a teaching pendant, and only use it to store a few waypoints. The program then calculates the straight line between those points and adjusts the speed based on live stress at each joint.

It would be much easyer to just hard-code a path, but that would be no fun as you can't easily make a new path, and you need to use a computer to change the waypoints. This program I wrote is some next-level thing for LEGO 6DoF with only EV3, and no computer doing the calculations. I send a set of coordinates for each motor with each it's own speed, and send the new coordinates before the old one finished, trying to make it look like a smooth movement. The thing now making it jerky was this long cycletime, but it should be lower now. If all works fine, and I have finished cleaning up more of the code, and find time to make a 3D model with instructions I will publish it.

Share this post


Link to post
Share on other sites

Interesting. I hadn't considered that some motors would be under more stress and lag behind. If only there was some way to ensure that they all moved at their desired speeds... 

It sounds like you've made some progress. Hopefully we get to see a new video!

Share this post


Link to post
Share on other sites
1 hour ago, Mr Jos said:

If all works fine, and I have finished cleaning up more of the code, and find time to make a 3D model with instructions I will publish it.

Something to look forward to. Thank you again.

Share this post


Link to post
Share on other sites
Posted (edited)

Hi Jos. I am glad that you go on with the project.

I tried to look at your last piece of code and I still see some multiplication terms that occur in all the three equation and that could be pre-computed. It might save some milliseconds.

Edited by Jonas

Share this post


Link to post
Share on other sites

Hi Jos

Have you tried to run the loop without printing output?

I am a little surprised by how much it slows down program execution - especially if the output is sent via Bluetooth.

 

Cheers

Carsten

Share this post


Link to post
Share on other sites
4 hours ago, Munchkin255 said:

Hi Jos

Have you tried to run the loop without printing output?

I am a little surprised by how much it slows down program execution - especially if the output is sent via Bluetooth.

 

Cheers

Carsten

The output is not send with Bluetooth to print it on screen of the laptop, it only prints it when running wired with USB running a debug. When directly run from EV3 and nothing connected with USB it just executes the program. It's already better and looking better, but I still want to try improving the whole program.

Soon maybe others might be able to help when I've finished my instructions for the robot arm, and test themself and try to see what I can't figure out that slows the movements a little.

Share this post


Link to post
Share on other sites
Posted (edited)

Such calculations are my weak point. I've always needed help with this. I have always solved any mathematical problems using https://plainmath.net where you can always find the answer to your question. I hope you solved this problem successfully!

Edited by Tia1234

Share this post


Link to post
Share on other sites
Posted (edited)

I played with MrJos' code a bit and made some optimizations that are based on a) precomputing some variables, b) applying goniometric formulae for a difference between two angles, and c) rearranging some terms in the equations. The new code is more compact and about 2 times faster. (Now, the main speed bottleneck is the physical reading of the 6 motor angles.)

Here is my code:

    yawbasecos = math.cos(yaw_base_angle)
    yawbasesin = math.sin(yaw_base_angle)
    pitbasecos = math.cos(pitch_base_angle)
    pitbasesin = math.sin(pitch_base_angle)
    pitarmcos  = math.cos(pitch_arm_angle)
    pitarmsin  = math.sin(pitch_arm_angle)
    rolarmcos  = math.cos(roll_arm_angle)
    rolarmsin  = math.sin(roll_arm_angle)
    yawarmcos  = math.cos(yaw_arm_angle)
    yawarmsin  = math.sin(yaw_arm_angle)
    
    #new angle introduced and its cosine and sine computed
    difpitcos  = math.cos(pitch_arm_angle - pitch_base_angle)
    difpitsin  = math.sin(pitch_arm_angle - pitch_base_angle)

    #new variables added
    tempA = rolarmcos * yawarmsin         
    tempB = a67 * rolarmsin * yawarmsin
    tempC = pitbasesin * pitarmcos * yawarmcos - tempA * difpitcos
    tempD = a67 * tempC - a45 * difpitsin + a3 * difpitcos - pitbasecos * (a67 * pitarmsin * yawarmcos - a2)

    #significantly simplified equations
    x_pos_fork = round(yawbasecos * tempD - yawbasesin * tempB)    
    y_pos_fork = round(yawbasesin * tempD + yawbasecos * tempB)
    z_pos_fork = round(a67 * (tempA * difpitsin - yawarmcos * difpitcos)- a45 * difpitcos - a3 * difpitsin +  a2 * pitbasesin + a1)
   

 

Edited by Jonas

Share this post


Link to post
Share on other sites
Posted (edited)
19 hours ago, Jonas said:

I played with MrJos' code a bit and made some optimizations that are based on a) precomputing some variables, b) applying goniometric formulae for a difference between two angles, and c) rearranging some terms in the equations. The new code is more compact and about 2 times faster. (Now, the main speed bottleneck is the physical reading of the 6 motor angles.)

Here is my code:


~   

 

Thanks for helping together to find a better solution! I will have a look at at soon when I clean up my workbench as my next project (that is failing for few weeks already, is taking up my whole table [Technic pin sorter]). I'll implement your new code and see what it does and if it can smooth out the movement some more. It might take some days for me to answer back how it went!

Edited by Mr Jos
resized quote

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

  • Recently Browsing   0 members

    No registered users viewing this page.