RSI Smooth Motion

  • Hi there,


    I'm working with Robot Sensor Interface (RSI) and a KR60 and having trouble producing smooth motion.


    For reference, I've attached a directory called "shared.zip" containing a SRC file and two RSI files, one of which connects inputs received via an Ethernet object directly to an AxisCorr object and the other incorporates a PID object. I have tried using P, PD, I, D, and PID with varying parameters in place of that PID object without success, though it's quite likely my implementation of RSI simply isn't correct. Otherwise, I'm using #IPO_FAST (4ms cycle time) and sending corrections in the range 0.001 to 0.01 degrees to any or all of my robot's axes, though I would like to send corrections as large as 0.1 degree. I have a custom C# application (connected upstream to a game controller) on an external PC which transmits datagrams to the Ethernet object.


    The robot appears to move abruptly and rapidly, its motors making a terrible noise as it does; corrections are not reached smoothly. I've read a couple of posts here that refer to jagged motion and robots appearing to tear themselves apart using RSI, though I've not seen any actionable solutions (except for modifying the system variable $FILTER at the expense of the robot's warranty). What I'd like to know is how, specifically, do I (a) implement and (b) tune the parameters of RSI such that I can achieve smooth -- think slow or surfy -- corrections of larger size? I'm a programmer and, if possible, I'd really like to see example code. Let me know if I can provide more information.


    Thanks a ton for your answers,.
    FlexoWasTaken


    (gratuitous smiley: :fish:)

  • Place your Ad here!
  • [size=small]RSI can be very dangerous and careful consideration of every step is needed.[/size]
    [size=small]this is not a video game, laws of physics apply... [/size][size=small]robot have specs - and limits.[/size][size=1em]for example each axis will have it's own maximum speed, typically some 60-200 deg/sec.[/size][size=1em]lets say there is an axis that can do maximum speed of 180 deg/sec.[/size][size=1em]if you are using 4ms update, it means that in one second, you can do 250 corrections (1sec = 1000ms). [/size][size=1em]for this particular axis no correction ever should exceed 180deg/250= 0.72 deg .... EVER![/size][size=1em]default RSI correction limit is rather generous 5 deg. [/size][size=1em]do you see how outrageous your 180deg correction limits are? this allows remote system to try driving such axis at speeds of 180*250= 45000 deg/sec. still wonder why robot screams and acts the way you describe?[/size][size=small]same goes with acceleration, load etc. (i don't see your program specifying loads at all - bad idea...).[/size]

    1) read pinned topic: READ FIRST...

    2) if you have an issue with robot, post question in the correct forum section... do NOT contact me directly

    3) read 1 and 2

  • Might have time to look at your code later, but the first thing to do is add an RSI Monitor block to your RSI program, and monitor the inputs to your Move-Corr object. Also add a Position Monitor object and add those outputs to your Monitor. This is the critical first step to understanding what your RSI program is ordering the robot to do, and how quickly. It could be something as simple as a mistake between relative/absolute positioning, could be noisy commands, could be antirestart windup... getting the data first is critical.

  • OK awesome, thanks all for the rapid response! Changes been made reflecting concerns expressed by panic mode, some of the parameters identified by SkyeFire have also been tuned, and the RSI project has been very much redesigned; the updated version is attached for your reference.


    Know that we have adequate safeguarding in place and that this is not a video game for us, either. To clarify, we don't plan to have an end-effector or a workpiece so, for our tool (here, the mounting flange), the default maximum load has been applied; I don't see a way of governing loads using RSI. We are also using #ABSOLUTE corrections. Your concerns withstanding, I think that my movements would necessarily exceed those limits. The BCO move puts our robot at [0, -90, 90, 0, 90, 0], so if a series of corrections equal to 0.01 degrees per axis should attempt to bring the robot to [10, -80, 100, 10, 100, 10], then those limits would constrain my corrections not beyond [5, -85, 95, 5, 95, 5]. In the case of #RELATIVE corrections, this makes sense to me and I agree with you. However, I think those limits conflict with the usage of #ABSOLUTE correction. To me, it sounds like an implementation of the Limit object earlier in the program would be better in my case. Please see my changes and correct me if I'm totally wrong and if some part of that logic would still cause unsafe behavior.


    For each axis, the new RSI implementation works as follows:
    (1) First, the correction is calculated by differencing a target joint-position (via Ethernet) and the current joint-position (via AxisAct).
    (2) Next, the correction is multiplied by a gain factor (via P) and limited the result to a certain range (via Limit).
    (3) Next, the correction is summed with the adjusted correction and the total absolute correction (via AxisCorrMon)
    (4) Finally, the correction is passed to the motion block (via AxisCorr).


    This implementation is working fairly well. I still see some abrupt motion here and there. SkyeFire, I will post images from the RSI Monitor later. Also, an undesirable result of the solution is that the robot will never actually reach or exceed my targets, getting infinitely close but never achieving them (the magnitude of the displacement decreases exponentially). Does anyone have ideas on how to implement RSI in order to fix this, such as a method of using the PID object instead of the P object?


    Thanks,
    FlexoWasTaken



    Edit: The file you will care about in the attachment is called RSI_Ethernet.rsi; open using RSIVisualShell.

  • No RSI-monitor traces? BTW, you can export the traces to a CSV file, which would be more effective to post than screenshots.


    As far as the "Zeno's Paradox" problem -- normal practice is to establish a tolerance band around your target, and (for example) generate an RSI Termination condition when the robot's position has been inside that tolerance band on all axes for X milliseconds. Generally, a properly-tuned PID function will prevent the robot from "hunting" significantly around the target. Also, even "below" the RSI layer, the robot has its own tolerance band for each commanded axis value, and essentially quits moving an axis once its deviation from the commanded value falls within that tolerance band.

  • Hello! My name is Alexander, I'm a student from Vladivostok. Now I'm working with the robot KUKA KRC4 ARGULUS and trying to control it using RSI 3.2. I calculated its kinematics and I manage its generalized coordinates using Simulink real-time. I have a problem. It consists in the fact that during movement the robot vibrates and its movement is not smooth. I use absolute correction. I used both IPO and IPOFAST modes. In IPO mode, the robot vibrates less. As I understand it, the reason is that when the robot comes to the desired coordinate it stops, and only then it continues to move. I searched for information about this on the Internet and found your topic on the robot-forum, where such a problem was discussed. The timing is very tight and therefore I would like to ask you for advice in this matter. If you have a little time, please tell me if you managed to make a smooth move using RSI. And if so, how? Thanks for the answer.

  • Hey,


    RSI has a sort of steep learning curve and there are few useful, working implementations available online for it; I've attached mine. My lab used this material at Autodesk University (2017) and it works nicely when used with live-input from a virtual reality controller. It's a generalizable implementation, but just note that the parameters I used to tune it are for that application only (we had to account for poor input, such as from shaky-hands). It'll have to be tuned for your application so I highly recommend using it as a starting point only. It should be enough to get started, though.


    The code I've provided is fairly well organized and documented. You'll be interested in the files _datagram_to_send.xml, RSI_Ethernet.rsi, and RSI_Ethernet.src. Note that you'll have to modify the contents of RSI_EthernetConfig.xml (or, really, generate your own) to match your setup.


    The key to RSI is that you have to build the motion planner itself. In the example I've provided is implemented a fairly standard trapezoidal velocity algorithm which, in effect, chases after the most-recently-received set-point per axis. The constant acceleration parameter is added (a*1) to a velocity parameter; this effect is cumulative, and the robot will, in effect, start speeding up. Acceleration is no longer applied (a*0) once the velocity reaches a set limit (such as an axis or work cell limit); the robot will, in effect, move at a constant rate. Acceleration is negated (a*-1) once the axis is within a certain range of the set point; the robot will, in effect, slow down. The following diagram shows this behavior.



    When you open RSI_Ethernet.rsi in the RSI editor, note that I implement at custom PD function for each axis to control their displacements. I don't use many of the internal PID functions provided by RSI; the documentation and support for this material is shockingly poor. The function is the same for each axis, however, you will need to verify and configure a few of the parameters inside -- such as velocity limit and starting position -- yourself. Note that this implementation doesn't care about "how long" it takes to reach a set point by any axis, so each axis will move towards its set point independent of the whole; the algorithm we implemented can, however, be rearranged such that all axes converge synchronously. The bulk of the work -- the motion planner, that is -- is done by this file. I won't copy all the code here, but it is pretty well documented and included in the attachment.


    I've also included a Processing sketch called sim.pde which we used to test the motion planner and parameters before running them on the real robot; it's a short program and I recommend running it (using the Processing IDE) to visualize what's going on. It will follow the position of your mouse, the Y position of which corresponding with a displacement that might be to an axis. Again, this is only used to prototype/visualize in low-resolution a few aspects of the motion planner, but was useful nonetheless.


    The robot runs the following program. Note that the parameters in the fold RSISYS are store the velocity of each axis during the previous cycle; if any of these would exceed the physical limit of an axis (which may be reduced further by the user parameter $SEN_PREA[8]), acceleration to that axis is, again, multiplied by 0. The parameter $SEN_PREA[9] corresponds with the convergence tolerance, a range around the set point within which the robot can be said to have "reached the position". This code follows what you'll find in the RSI documentation.



    What I send to the robot, then, is a datagram containing the degree of rotation per axis and a timestamp. Note that I overwrite the value 000.000 with the appropriate degree of rotation and 0000000000 with the timestamp. I was able to send this reliably using both the 4ms and 12ms cycle times. This follows what you'll find in the RSI documentation.


    Code
    <Sen Type="ADSK">
      <AKorr A1="000.000" A2="000.000" A3="000.000" A4="000.000" A5="000.000" A6="000.000" />
      <IPOC>0000000000</IPOC>
    </Sen>


    I have to get to work, but I hope this helps!

  • Just adding on a direct answer your question about smoothing...


    Smoothing is achieved via the PD (Proportional Derivative) controller algorithm. In simple terms, the algorithm allows each axis to take little steps towards its target, always respecting its current velocity. So, when axis is directed to move from a start point, 0, to a set point, 90, the algorithm determines whether an acceleration of constant value, 0.001 deg/<cycle time>s, needs to be added to or subtracted from to its current velocity, 0 deg/<cycle time>s. On the next iteration, the robot is at 0.001, still needs to get to 90, but is now moving at a rate of 0.001 deg/<cycle time>s; the acceleration is added again to the velocity and the robot increases in speed towards the set-point. Because the acceleration value is small, an axis never appears to jerk, regardless of which direction the robot is told to go; hence smooth motion.


    In previous posts, I had said I was looking for washy or surfy motion, wherein the robot is allowed to overshoot set-points (and then try to cycle back) in the interest of keeping motion really fluid and smooth; this is possible. It's also possible -- especially if you play with the D parameter -- to get fine motion that never overshoots and also slows-down nicely as it approaches a set point. To experiment with different motion characteristics, try playing around with the parameters in the USER fold of the SRC file; keep your changes minimal, though, until you understand how their effects.


    Good luck, RSI rocks.

  • No problem! On that note, and to anyone downloads this implementation, I'm really interested to see your modifications or improvements to it; if you wouldn't mind, post them here.

  • Hello, FlexoWasTaken. I read your posts, but unfortunately I couldn't open your files with RSI Visual. I faced the same problem making the smooth motion.

    I want to move robot to the target point smoothly without using $FILTER.

    As I couldn't open your RSI Visual examples, itried to understand: Did you make all computations of motor positions at the robot side (in RSI Visual context) or at the external PC?


    As I see, I have to make next points to reach my goal:

    1. We have actual positions for every motor.
    2. We know the Cartesian position of Target point.
    3. Make the inverse kinematic transformation. We will know axis angles in the Target point.
    4. Calculate the whole Travel for every axis.
    5. Calculate: axis travel increment for every update cycle (4ms or 12ms). We know speed limits, acceleration limits for every axis. I have to calculate the correct travel and DO NOT exceed motor speed and acceleration
    6. Make STOP condition. Compare actual position and target positions


    I wonder if it necessary to make all calculations at the extarnal PC side, or it is possible to make it at the robot side?


    Regards

  • I wonder if it necessary to make all calculations at the extarnal PC side, or it is possible to make it at the robot side?

    Either one works. The mathematical burden remains largely the same, it's just a matter of which controller you do the math in.


    Doing all the calculations on the PC will require the PC to send a constantly-changing set of values to RSI. Then the PID loops would have to be implemented either on the PC or in RSI.


    My personal preference would be to have the PC send over a target destination, and do all the control and PID in RSI. But that's because I have more realtime programming experience in KRL/RSI than I do in PC-side languages. Other people are more comfortable in C++ or MatLab, and like to keep the robot as "dumb" as possible.


    Either approach works, but you need to architect your solution ahead of time, and decide which controller has "custody" over which parts of the computation. Dueling PID algorithms can be fun, but unpredictable.

    I want to move robot to the target point smoothly without using $FILTER.

    $FILTER is something you should not tamper with unless it's very necessary, and you really know what you're doing. $FILTER is factory-tuned for the model of the robot, and is one of the very few "safety nets" that reign in RSI at all. Increasing $FILTER acts much like increasing the Derivative gain on a PID, and reducing it has effects similar to reducing the D gain. Reducing $FILTER substantially can allow the robot to jerk itself to pieces.

  • Hi, SkyFire! Thank you for your reply.

    So, it is possible to make all transformations for every axis in the RSI context only.

    Now I'm looking for more detailed documentation about context objects like D, PD, I , PID. Want understand how to make a signal flow olny at the RSI context side. Do you have any except 'help datasheet'?

    Unfortunately I can't open examples from FlexoWasTaken.

    I have KR16 KSS 8.6.3 and RSI v4.1.1

  • Some of the RSI details, like the gain variables for the PD objects, is oddly buried in a .CHM help file. If you look in the documentation that came with RSI (a copy is located on the robot in D:\KUKA_OPT\RSI\DOC), it should be there.

  • For each axis, the new RSI implementation works as follows:
    (1) First, the correction is calculated by differencing a target joint-position (via Ethernet) and the current joint-position (via AxisAct).

    As FlexoWasTaken had implemented at his example, robot gets a target position as values of every axis. Next steps are clear.

    Another case if robot gets a Cartesian target position. Is it possible to convert the Cartesian position to values of every axis inside the RSI context (like inverse() in KRL)?


    Regards.

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

Advertising from our partners