Need help cleaning math calculations for better cycletime

[Mr Jos]

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.

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

[Mr Jos]

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...

 

[icm]

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?

[Mr Jos]

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

 

[ord]

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

[Mr Jos]

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;

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.

[ord]

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!

[domleg]

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.

[Jonas]

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 May 9, 2021 by Jonas

[Munchkin255]

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

[Mr Jos]

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.

[Jonas]

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 June 11, 2021 by Jonas

[Mr Jos]

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 June 12, 2021 by Mr Jos
resized quote

[ord]

@Mr Jos sorry to dig up an old topic but I was reading about PID controllers on Wikipedia and the example there refers to a robotic arm. It reminded me of your fantastic robotic arm and made me curious - did you ever try changing the PID parameters on this project? Perhaps more aggressive settings could eliminate the need to re-calculate the forward kinematics at each waypoint...

[Mr Jos]

On 1/17/2022 at 2:18 AM, ord said:

@Mr Jos sorry to dig up an old topic but I was reading about PID controllers on Wikipedia and the example there refers to a robotic arm. It reminded me of your fantastic robotic arm and made me curious - did you ever try changing the PID parameters on this project? Perhaps more aggressive settings could eliminate the need to re-calculate the forward kinematics at each waypoint...

It's never to old to talk about Mindstorms here. And certainly this robot not, as I'm still using it in my current projects, and changing the code little by little to get it better.

I'm not using standard PID regulation for the motor controls, because the problem is resistance. If the bot bends forward, it will 'fall' and have no problem to maintain the wanted speed, but when coming back upwards it needs to put down a lot of power, and depending on the load it will not reach the desired speed. Because there are 6 motors, it's near impossible to program a map, and know the load on each axis at any given position. So best way (for me) is to get the current motor angles, and compare them with what they should have been/what the next set will be. The speeds are calculated by the degrees each motor needs to turn to reach the next sub-point.

I don't actually calculate the forward kinematics anymore during steps, I just use the encoder feedback.

Example (simplified; motor angles used real, not taking gearing in account):

• All motors start at 0° (M1-M6 = 0°), with forward kinematics the XYZ position is calculated lets say X=100mm, Y=0mm, Z=100mm. We now want to tip down to X=100, Y=0, Z=20
• My program measures the longest moving distance (Z100->Z20 = 80mm) it divides it by steps of 3-6mm depending on my max speed settings, let's say 5mm here. So it will make 80/5 = 16sub steps.
• Motor 1 will not run, as there is no Y movement, Motor 2 and 3 will both run to lower 5mm and keep at distance X=100. M4 will need to make a sharp 90° turn directly at the start to be able to keep the fork flat with Motor 5 whilst keeping Y=0. Motor 6 has to counter react to sharp M4 and counter turn 90°.
  • M2 and M3 will have to change maybe 2° each. M4 and M6 90° each. The program calculates the speeds for each motor by the maximum allowed speed for the fastest motor, so M4+M6 will both run at 100% speed. M2+M3 at 100%/90°*2° = 2,2% speed.
  • The program calculates by the amount of degrees M4 (largest change) needs to turn, how long it will take him. Lets say 400ms for this big movement.
  • All motors get their speed+position send at the same time and start running to it, the next step (2) starts to be calculated and takes 50-80ms, so the program waits with sending these next commands.
• Just before the motors will arrive at their position the next set of motor speeds+positions is send, so they don't stop.
  • If all is perfect M1 + M4 + M6 will now not have to turn anymore at all for the next 15steps, so their speed is set to 0 (actually I do send a minimal speed) and send to the same position every next step.
  • But now we encounter real life; If M4 had some resistance in the bearing, it did not reach max speed, so didn't turn near the 90° it should have but only 80°. If I would not measure the real motor angles I would send the speed to 0, and it would stall at 80° (turning slowly to 90°, but the fork would not be flat).
  • So I see that M4 is behind, and react by giving it again max speed as it will still be the largest movement to perform in step 2.
• The problem is, this can happen with all 6 joints that a load or internal resistance holds it from reaching the desired speed. So that's why I made my program to react to it by calculating the distance to turn by feedback motor angles.
• This line is for the people who did read this all, and want to know how to make their robot faster (I did find something some time ago), I doubled the amount of steps I can do per second by separating the motor commands from the calculating. I had it in a loop; Calculate, send motor commands, calculate, send motor commands, .... I found out that sending motor commands took half of the time in this loop, so I put the motor commands in a multithread and send them at the same time as calculating next points, making the movement smoother.

Edited January 18, 2022 by Mr Jos
Typo

[Toastie]

2 hours ago, Mr Jos said:

This line is for the people who did read this all

I did - and this is a very nice write up.

However - the PID thing.

@ord mentioned more aggressive settings - I'd say individually adapted PID settings?

When you have direct access to the PID algorithm, you can adjust the parameters on the fly. I do not believe that this is necessary, but each motor should have an individually optimized PID response parameter set. This is really tricky (as far as I am concerned) but worth the try. A PID algorithm simply does not care whether the actuator reaches final speed; it will just rank out full power. The thing is the response behavior to changes. When these become more or less erratic, then I'd go with rather aggressive settings, as @ord suggested. This may lead to serious overshooting, but if this happens you can relax the set a bit. 

All the best,
Thorsten 

[ord]

Wow, thanks for the detailed write-up and it's good to hear that you're still working on it :). I ask because I am in the early stages of making my own 6DOF robot - I will definitely take your learnings into account and hopefully too can achieve the holy grail of a robot capable of linear movements ;).

On 1/19/2022 at 6:24 AM, Mr Jos said:

and depending on the load it will not reach the desired speed.

If it is the case that a motor cannot physically reach its desired speed, is this not a design problem rather than a programming problem? i.e. fixed by gearing down/slowing down/spring balancers/counterweights?

If it is the case that a motor could theoretically reach its desired speed but doesn't, then maybe this is something that can be fixed with PID tuning? (I don't know - it is all new to me)

The way I look at it is that each motor should track its desired speed/position as closely as possible at all times, no matter the load. If this is achieved, then the robot will be as accurate as possible (IMO).

The reason I think about this comes from my plotter project, where any deviation from the programmed path is very visible on the plot. It took me some time to figure out that the majority of the deviation I was seeing stemmed from the period during which the motors were accelerating (even with equal loads/acceleration rates, one axis will take longer to accelerate if its accelerating to a higher speed [see below]). The performance of the plotter significantly improved when I removed the acceleration limits of the motors, and they tracked their desired positions as quickly as possible. Have you done this for you robot? (The next problem, and what I am working on now is making it stiff enough to handle such high accelerations).

Here you can see the problem visualised, before I removed acceleration limits. At ~14750 the program is ready for its next move but the x-axis is only at ~92mm instead of 103.3mm (I think similar to your example only being at 80°). After removing acceleration limits, the actual positions (solid lines) very closely track the desired positions (dotted lines).

Anyway, this is all said with the goal of having the most accurate movements possible, and maybe this is not your goal :).

[Toastie]

40 minutes ago, ord said:

If it is the case that a motor cannot physically reach its desired speed, is this not a design problem rather than a programming problem?

Sorry to straddle into your discussion.

I believe this is really important: Before a program can deal with "things", the hardware needs to allow that. Otherwise any PID control may not lead to any acceptable result - timing wise that is. I now understand that this is a major challenge.

44 minutes ago, ord said:

The way I look at it is that each motor should track its desired speed/position as closely as possible at all times, no matter the load.

Yes, with sufficient reserve - power wise. The moment you reach limits (level off), timing will go bad.

I believe there are two issues here: 1) accurate positioning and 2) timing (synchronization). 2) seems to be critical for 1) in your project, as the position depends on more than one coordinate - to be reached within a defined temporal profile.

I may be totally off here, but I believe the two dimensions time and position need to be critically evaluated for the operation of such a nice and complex built.

Best,
Thorsten

[ord]

8 hours ago, Toastie said:

Sorry to straddle into your discussion.

No need to be sorry - the more the merrier! 

Indeed it is a challenge, when building a robot arm that has to potentially reach to a far distance and lift a load. 

8 hours ago, Toastie said:

I may be totally off here, but I believe the two dimensions time and position need to be critically evaluated for the operation of such a nice and complex built.

I agree. There is a lot to ponder here. It's probably time I read up about how industrial 6-axis robots operate...

[Mr Jos]

13 hours ago, ord said:

If it is the case that a motor cannot physically reach its desired speed, is this not a design problem rather than a programming problem? i.e. fixed by gearing down/slowing down/spring balancers/counterweights?

If it is the case that a motor could theoretically reach its desired speed but doesn't, then maybe this is something that can be fixed with PID tuning? (I don't know - it is all new to me)

The way I look at it is that each motor should track its desired speed/position as closely as possible at all times, no matter the load. If this is achieved, then the robot will be as accurate as possible (IMO).

My model does not use springs as they need very good bracing to much straigth, if they bend a little they will work against you and make it even worse. Also the small travel is limiting, but maybe the new shocks from the BMW motor could be off some help? But I don't have them to test.

So I did design my robotic arm to have the correct gearing to be able to operate in all positions without load, this allows for highest speeds possible. The thing then is the load, I see it more as a forklift. Depending how far forward you place the load, and how high you want to go, the max allowed load goes down. So if you reach out to the max point the arm can go, a load WILL prevent an axis to go to full speed, but my program sees this in the next step, and that slowed motor gets the highest power, the rest is slowed down, to keep the fork flat. This way I don't limit myselve to a low load that allows reaching max speed at any time. Or i would need to gear down and reduce the speed.

Initially I did try with counterweight (Using 1 or 2 EV3 bricks), but it was not so easy to make this move in the opposite direction as the arm and still have enough room to bend backwards enough.

 

About the motor settings, I do use the control class to change the "limits" parameters. There I changed the acceleration for every movement. I have done a lot of testing, and there is a 'to much'. If the acceleration is set to high it will get bad, it will overshoot the desired angle and run backwards, this makes a very bumpy movement as many endpoints are send. I did find the best acceleration for my robot that makes it not overshoot but still an aggresive acceleration.

 

Offcourse in the ideal world it should always reach it's received speed, this will be possible by lowering the max speed for the motors, and keeping power at 100%. It will then track better (and slower) if instead of 800degrees/second you set 400 as the max speed.

 

Maybe I will start over again and redesign the robot arm, and try to get longer arms. But then I certainly will need a counterbalance system, and/or a damping cylinder or something like in the real ones. Preferably I would make one with Robot Inventor as it's better available now, as I can't get my hands on new/2nd hand EV3's at the moment, but the lack of any sensors would make it hard to have a full 360° axis 1+4+6 like I have now with 2xEV3. (But I don't have any RI/Spike, and to be able to do so, need to sell a few more instructions on Rebrickable ( https://rebrickable.com/users/Mr_Jos/mocs/ ) to explain the cost as I will need extra motors as I have 0 :D

Edited January 20, 2022 by Mr Jos

[Mr Jos]

Thanks Ord, I'm curious what you will be able to make with it, if you have any questions feel free to ask me. When I initially made the instructions (it were my first ever) I thought no one would need/want them and maybe have 1 sale in 5 years, but now already need my 2hands and 2 feet to be able to count them and only 8months far. Glad I've been able to help more people to make these great arms, if only I would see some more feedback of them. So far found a video who modded my arm with pneumatics, and one I helped changing the program to work with 1 Robot Inventor set (and succeeded).

But they are very fun to play with, and everytime to try get it better working (and offcourse fail many times, and go back and try again).

[ord]

You're welcome, and thank you. I've spent the best part of my day reading through your code - It's a fine achievement getting to where you did. You and Akiyuki are the only two I know of who have achieved inverse kinematics with Lego. So congrats!

I plan to make an arm with the Robot Inventor set, and have just now tested sending signals from the Mindstorms LED display to the technic hub through the Mindstorms colour sensor. Hooray for a hackish 7th motor for an end effector!

I now better understand your example from earlier, and I believe the starting position of your robot is a singularity. You've managed to overcome it, but for what it's worth: when I used to run a Yaskawa Motoman, if the robot's position came anywhere near a singularity it would stop and throw an error, because the joint speed approaches infinity here. It's starting position looked more like a limp arm to avoid this, with no 90° angles, and the user just had to know to avoid them when programming.

20 hours ago, Mr Jos said:

So I did design my robotic arm to have the correct gearing to be able to operate in all positions without load, this allows for highest speeds possible. The thing then is the load, I see it more as a forklift. Depending how far forward you place the load, and how high you want to go, the max allowed load goes down. So if you reach out to the max point the arm can go, a load WILL prevent an axis to go to full speed, but my program sees this in the next step, and that slowed motor gets the highest power, the rest is slowed down, to keep the fork flat. This way I don't limit myselve to a low load that allows reaching max speed at any time. Or i would need to gear down and reduce the speed. 

That's a good analogy with the forklift and makes sense to have the robot capable of both heavy loads and fast speeds. I think when I make my robot, I'll have it throw an error if any of the motors can't reach their programmed speed, and just say to the operator: "You'll have to send this move command at a slower speed if you want it to happen." I don't know for sure, but I would guess that this is what happens on a real 6-axis robot.

Anyway, now I'm more motivated than ever to finish off my plotter and free up parts for the robot arm.

Thanks again for your help @Mr Jos