Approximating acceleration profiles for PTP motions

  • Hello everyone,

    I am currently trying to approximate a custom acceleration profile for PTP motions. The robot is basically only used to perform rotations of the tool around the TCP, which is why I had some prior issues using LIN motions and decided to stick to PTP motions for now. My intention is to e.g. create a motion that is somewhat similar to an S-curve acceleration profile.


    S-curve acceleration profile

    (Please excuse the small size, I simply couldn't manage to upload the image directly)


    From what I gathered so far, such an acceleration profile could be either accomplished by chopping a single motion into several smaller motions and change $ACC and $VEL in between (orange curve) or by adjusting the program override $OV_PRO either at certain time intervals or in between motions which would result in a smoother acceleration profile but also reduce the programmed velocity by the same factor. I hope I am correct in my understanding so far.


    I tried to create example programs to implement this but unfortunately, I only have basic knowledge of KRL programming and limited robot access at the moment. Can anyone tell me if any of my approaches should work as intended or if there are major things I’ve overlooked?


  • SkyeFire

    Approved the thread.
  • The S-curve you describe is what the robot normally does naturally. We tend to draw it as a simple trapezoid, and the "curves" in the S tend to be pretty short, but the motion planner's PID algorithm is definitely generating an S-curve accel and decel profile.


    So, my first question is, what's wrong with the robot's inherent accel/decel profile that you want to improve?


    All three of your examples look valid, insofar as they should execute without causing any KRL errors. Your $VEL and $ACC values may be a bit high (depends heavily on robot payload and pose). I think Approach #1 is the best one. Approach #2 is likely to give you issues with the Advance Run Pointer, but Approach #3 would resolve that problem.


    What you have to keep in mind is that the KRC has a lot of different motion control algorithms going on constantly that cooperate and compete with each other at all times, and you're now trying to add another layer atop that. For example, if you command the robot to accelerate at a rate it physically can't, it will simply apply the maximum acceleration it can. And the physical limit changes, not only with robot payload, but also robot pose and inertia -- the robot can accel harder under conditions where the servos have to fight gravity less, or from a standing stop compared to doing a sudden reversal.

  • hang on.. isnt ptp motions only limited by axis speed $VEL_AXIS[] and axis acceleration $ACC_AXIS[]? Atleast that has been my understanding. $VEL or $ACC would do nothing for a ptp motion. But as SkyeFire wondered what is wrong with the original path and motion planner?

  • btw getting lin and circ motion speeds correctly set up requires first of all $TOOL_DIRECTION to be correctly set up (watch out theres two variables that needs to be set, believe the other is $TOOL_DIRECTION_LIN_CIRC). Then relevant structures $ACC, $VEL AND $APO needs to be set up correctly. Check this forum for "swivel and rotation" to get a good grip on which is which *.ORI1 and *.ORI2. Suitable approximation strategy must also be used as eg. If you are turning around tcp using c_dis will not make for any smooth motion. So that also must be taken care of c_ori, c_vel, c_dis, c_ptp or c_spl.


    With that set up correctly and should be fairly easy to get a smooth motion profile.

  • hang on.. isnt ptp motions only limited by axis speed $VEL_AXIS[] and axis acceleration $ACC_AXIS[]? Atleast that has been my understanding. $VEL or $ACC would do nothing for a ptp motion.

    ...you are absolutely correct. I got hung up enough on seeing all the $VEL and $ACC changes that I overlooked the fact that the motion commands were PTP.

  • First of all, I would like to say a big thanks to both of you. The info you've provided was already a huge help (from what I can tell). I'll try to address everything one by one.


    So, my first question is, what's wrong with the robot's inherent accel/decel profile that you want to improve?

    There isn't anything 'wrong' per se. We move a relatively large tool, which is basically a 50kg heavy, 2.5m long aluminum construction, that exhibits quite a lot of oscillations at the endpoint of a motion. Due to the robot being used for x-ray imaging, we only tolerate small oscillations during image capture so as to not impede the image quality.

    Hence, part of my master thesis is to implement a way to reduce these oscillations to allow for faster cycle times. My core idea is to test a couple of different acceleration profiles for their ability to induce less oscillations in the first place. That's why I need the ability to change not only the ramping time but the accel/decel profile as a whole.

    All three of your examples look valid, insofar as they should execute without causing any KRL errors. Your $VEL and $ACC values may be a bit high (depends heavily on robot payload and pose).

    That's a big relief. I'll probably testing Approach #3 first, as it would be more flexible for my purposes. But if I run into problems I'll likely fall back on Approach #1 and use that instead.

    hang on.. isnt ptp motions only limited by axis speed $VEL_AXIS[] and axis acceleration $ACC_AXIS[]? Atleast that has been my understanding. $VEL or $ACC would do nothing for a ptp motion.

    Now that I read over the documentation again I have definitely confused ORI1 and ORI2 for the axis acceleration. Thanks for the heads up, I will definitely have to use $ACC_AXIS[] and $VEL_AXIS[] instead.

    For example, if you command the robot to accelerate at a rate it physically can't, it will simply apply the maximum acceleration it can. And the physical limit changes, not only with robot payload, but also robot pose and inertia.

    I'm well aware of that but for the most part it doesn't matter too much for me right now. The main concept behind the oscillation reduction is based on limiting acceleration/velocity at the right times. I don't mind as much if the motion is slower by a second or even two as long as I don't get more oscillations than intended. The oscillations I encounter usually take 3-10 seconds to die down which takes a lot more time overall when I move to 12 different positions.



    I do have two new questions you guys might be able to help me with.


    The data sheet for the KR150 2700 extra we use states the maximum speed at 150kg is:

    A1: 123 °/s, A2: 115 °/s, A3: 120 °/s, A4: 179 °/s, A5: 172 °/s, A6: 219 °/s
    Am I correct in assuming that I can safely set $VEL_AXIS[] to any value that is equal or smaller to these? Of course if the robot ever reaches these velocities during a motion is a completely different question.


    What joint acceleration values does the robot realistically use? In the KUKA docs I haven't found any information at all so far and I don't know if the range is something like 250-500 °/s² or more like 5000-10000 °/s² or anything in between. I only need a rough estimate to know the range I am working with. :/

  • What joint acceleration values does the robot realistically use? In the KUKA docs I haven't found any information at all so far and I don't know if the range is something like 250-500 °/s² or more like 5000-10000 °/s² or anything in between. I only need a rough estimate to know the range I am working with. :/

    You can check $ACC_AXIS_MA[] there might be a typo in the variable name but it should point you in the right direction

    • New
    • Helpful

    and if you have oscillation problems easiest way is to stiffen up your tool, decrease acceleration or both. Setting correct loads and inertias are also essential to make motions smooth. There are also parameters for jerk if memory serves (derivative of acceleration). You may want to keep a high speed but decrease acceleration and jerk alot to keep it from oscillating if tool is flimsy. On abb's you get one more parameter so you get vel, acc, jerk and snap. But i've never felt the need for the last one atleast. If your tool is flimsier in some direction you can quite comfortably set $ACC_AXIS[6] to something decently low if it is sloppu around flange z axis. If it is sloppy in both swiveling and rotation, i would suggest setting $ACC.ORI1 and ORI2 to a decent value and keeping critical moves as LINs. Also approximation strategy could be worth checking eg. Diffrence between c_dis and c_vel. Could also be worth checking out newer S type motions. There are some experts on pancake chromodynamics and kuka motion related stuff on this forum. I think Fubini would have valuable input on this and surely others too. But stiffening up your tool and lowering accelerations and setting speeds, approximation etc up perfectly would be my suggestion. Many times you can also orientate the tool in such a way that it does not flap around that much when you do big moves.

  • What joint acceleration values does the robot realistically use? In the KUKA docs I haven't found any information at all so far and I don't know if the range is something like 250-500 °/s² or more like 5000-10000 °/s² or anything in between. I only need a rough estimate to know the range I am working with.

    There is no explicit bound for this. Since we are talking PTP moves here the limitations come from the robots dynamic model. The dynamic model restricts gear and motor torques. In this case $ACC_AXIS[] does not limit axis acceleration directly but the torque available to accelerate/decelerate each joint (the maximal values for torques are stored in robcor.dat inside the dyn_dat[] array. But the documentation of the dyn_dat values are a company secret of KUKA and not publicly available). How much torque there is available depends on the robots state (position, velocities, accelerations, loads, ...). Lets assume a motor can generate a torque of 40 Nm. Holding the robot and its current position (assuming for simplification positions, accelerations are zero) requires a motor torque of 27 Nm than a value of 100 in $ACC_AXIS[] means the robot will try to use 13 Nm for acceleration. Basically everything that can be used up to the maximal torque value. But since the robot is a coupled system this might not be the resulting acceleration you will see in the end. Since the calculated trajectories of the robot are time and phase synchronized over all axes it might be that another axis is the actual limiter for the overall torques. In this case all other axes are "slowed" to fit the limiting axes in such a way that all axes start moving and finish moving at the same time (this is an important feature that no matter what the user programs for VEL_AXIS and ACC_AXIS the resulting movement of the TCP in Cartesian space is always the same).


    To use accelerations on joint level in rad/s^2 you would need to deactivate the dynamic model. But this is not recommended because it is than a lot easier to destroy your robot and in your use case you would also see even more unwanted oscillations.


    If you want full control over how the robot accelerates and decelerates yourself the only option I can think of is using the RSI package.


    Another possibility to get closer to commanding direct rad/s^2 values would be to use spline motions in combination with adding additional restrictions by means of $SPL_VEL_RESTR. If I remember correctly you can add joint specific restrictions for axis acceleration in there. In that case $ACC_AXIS is applied as percentage value to both (torque limit and joint acc limits). Joint limit acc then is defined by the $RAISE_TIME[] the time it takes to acc from stand still to maximal velocity for each axis. In KRC2 $RAISE_TIME was directly inside R1/$machine.dat but for KRC4 this changed and the info was moved with tag RampUpTimeUnderLoad into the motor configuration xml-File of the specific axis.


    Maximum axis acceleration - General Discussion of Industrial Robots Only - Robotforum - Support and discussion community for industrial robots and cobots (robot-forum.com)


    Of course for getting a better performance with less or minimal oscillations the would be other options using available features of the robot controller but as I take that is not what would be a feasible solution justifying a masters thesis.


    Last but not least what you also could do is just "measure" the resulting accelertions by using the built-in oscilloscope function (aka trace) inside the KRC. You can record the resulting axis acc by configuring the correct channel you want to record and than plot the result using either WorkVisual or the Tracetool found in any KRC installation in the C:\KRC\UTIL folder. So use $ACC_AXIS just to "scale" the input but what the resulting axis acc you get is only defined after recording the values and looking at the trace plots.

    The data sheet for the KR150 2700 extra we use states the maximum speed at 150kg is:

    A1: 123 °/s, A2: 115 °/s, A3: 120 °/s, A4: 179 °/s, A5: 172 °/s, A6: 219 °/s
    Am I correct in assuming that I can safely set $VEL_AXIS[] to any value that is equal or smaller to these?

    The value entered for $VEL_AXIS[] is a percentage of the maximal value. So 100 means maximal velocity. The actual maximal value is given by $VEL_AXIS_MA in motor revolutions per minute inside R1/$machine.dat. If you want to calculate maximal axis velocities you need to also consider gear ratios RAT_MOT_AX[] and axis couplings $AXIS_COUP[,] inside the same file.

    External Axis (on Rotary Table) - Rotation Speed Control - KUKA Robot Forum - Robotforum - Support and discussion community for industrial robots and cobots (robot-forum.com)

    (its basically the same for robot axes).

  • Thanks so, so much for the huge amount of info and feedback from all of you, it is truly invaluable to me right now! :thumbup: :thumbup: :thumbup:

    You can check $ACC_AXIS_MA[] there might be a typo in the variable name but it should point you in the right direction

    I haven't found a mention of $ACC_AXIS_MA[] in the systems variables handbook but I assume I will find it either in R1/$machine.dat or together with $RAISE_TIME[] in the motor configuration xml-File of the specific axis that Fubini mentioned. But maybe I will just write a program and do an acceleration test. We have acceleration sensors for the tool and if I simply test the individual accelerations of each axis I will get a rough idea of what to expect.

    If you want full control over how the robot accelerates and decelerates yourself the only option I can think of is using the RSI package.


    Last but not least what you also could do is just "measure" the resulting accelertions by using the built-in oscilloscope function (aka trace) inside the KRC.

    The company actually has bought the RSI package for the robot but due to a PLC defect we're using the X11 interface instead and from what I've read without a PLC it is impossible to use the RSI. ;(

    Thanks a lot for the tip with the built-in oscilloscope but as I mentioned earlier we have acceleration sensors on hand which I prefer to use, since they are already set up anyway.

    If your tool is flimsier in some direction you can quite comfortably set $ACC_AXIS[6] to something decently low if it is sloppu around flange z axis.


    But stiffening up your tool and lowering accelerations and setting speeds, approximation etc up perfectly would be my suggestion.

    I will certainly keep in mind to use lower accelerations for axis 6 since our tool is especially flimsy around the flange Z axis. Actually we have planned for creating a stiffer version of the tool in the near future. Unfortunately 'near' is most likely next year which won't make it in time for my thesis deadline. :rolleyes:




    I've tried to integrate all the feedback for another iteration of the example programs. Especially feedback on Approach 4 would be greatly appreciated as the option to limit jerk would be wonderful for me.


    Approach 1 and Approach 3 would stay mostly the same but instead with $ACC_AXIS[] and $VEL_AXIS[].

    Approach 2 got canned.

    If I want to limit jerk I'd need to use spline motions, as they enable me to either set $GEAR_JERK or take the more customizable route via $SPL_VEL_RESTR with $ACC_AXIS[] and $RAISE_TIME[]. I've written Approach 4 that would do this with SPTP motions and a subprogram for changing the kinematic parameters (jerk, acceleration, velocity). If I only want to use SPTP for the sake of a single PTP motion I shouldn't need to program any additional points in between my start and goal as all the spline supports are computed by the robot controller, is that correct?


  • If I only want to use SPTP for the sake of a single PTP motion I shouldn't need to program any additional points in between my start and goal as all the spline supports are computed by the robot controller, is that correct?

    That is correct, you can replace a single ptp with a sptp to the same point. I do not know if it holds true but sptp's seem smoother than traditional ptp moves. Im speculating this is because they are bound by diffrent limits

  • The company actually has bought the RSI package for the robot but due to a PLC defect we're using the X11 interface instead and from what I've read without a PLC it is impossible to use the RSI. ;(

    Eh? That's not true at all. Plenty of RSI applications just use local sensor I/O.


    I've never tried it, but it should be possible to write an RSI app (probably of the "superposed motion" type) that exercises no control over motion, but simply streams data to the RSI Monitor. Which would require exporting the CSV-style log file after the test and running it through Excel or some other tool to generate columnar data that can be plotted, but long ago I wrote an Excel VBA script that did most of that for me with a single hotkey, so I know it's doable.

  • I do not know if it holds true but sptp's seem smoother than traditional ptp moves. Im speculating this is because they are bound by diffrent limits

    This one I can clear up. The spline motions of Kuka are based on piecewise polynomials of order 5 afaik, which in turn means the position is four times continuously differentiable. If we were to differentiate three times we see the jerk is a piecewise quadratic function (and the snap being linear). This creates a motion that induces significantly less oscillations at higher frequencies (something like >5 Hz). Of course the oscillations occuring in practice are highly dependent on the system.

    I only found a website plotting this for S-curves based on third order polynomials. It should look similar for fifth order ones but with a stronger shift of the energies towards f=0.

    Mathematics of Motion Control Profiles
    To tune motion profiles for maximum performance, understand the mathematics of motion profiles and which profiles are best for your step motor application.
    www.pmdcorp.com

  • I only found a website plotting this for S-curves based on third order polynomials. It should look similar for fifth order ones but with a stronger shift of the energies towards f=0.

    That link was broken -- it connected to the top of this thread. I took the liberty of fixing it, since the label of the link was a valid URL.

  • UPDATE:

    After finding out what was missing in the code I managed to run into a more fundamental problem. Apparently $GEAR_JERK is write-protected when the subprogram is called (as can be seen in image below in German). I have no idea why it should be write-protected, since I specifically didn't access $GEAR_JERK_C which I know is write-protected. Does anyone know how to circumvent this or is the only option to use a constant jerk limit for the whole SPTP motion?


    In other news, if anyone ever wants to do something similar, I needed to fix a couple of mistakes in the code and now it works with acceleration & velocity constraints at least. Turns out spline motions are a bit finnicky in what parameters they expect.

    • TRIGGER WHEN always needs either the DISTANCE or PATH argument. Keep in mind that DISTANCE doesn't work with SPTP. The DELAY is applied relative to the position determined by either of these (see documentation).
    • If the trigger calls a subprogram you also need to insert PRIO, where a value of -1 lets the operating software schedule the priority of the subprogram call.
    • Both $ACC_AXIS as well as $VEL_AXIS understandably expect values of at least 0.1 or greater but my python script that created the motion profile starts with zero acceleration. Not a big deal but I suggest to set a reasonable minimum value in your scripts from the get-go.

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