High jerk at end of trajectories

[gavanderhoorn]

Reported on ROS Answers: Industrial Robot Client sends wrong velocity on last waypoint?.

Created a plan to rotate single joint from 0 to pi/2 radians. Examined plan looks fine with smoothly tapering velocity, however, actual robot motion ends with accelerated jerk at end. Wireshark packet sniffing reveals velocity in broadcast packet for the last waypoint is wrong.

It's unclear at the moment whether this is an issue in staubli_val3_driver, industrial_robot_client or somewhere else.

[gavanderhoorn]

Related: ros-industrial/staubli_experimental#25 and ros-industrial/staubli_experimental#39.

On the industrial_robot_client side: ros-industrial/industrial_core#247.

[gavanderhoorn]

@marshallpowell97: tracking this issue here.

[bprice-parc]

Simon Schmeisser suggested that we plot the actual position of the robot against the plan. We have tried to do that in the referenced figure (we used a heuristic alignment of planned and measured position).

We note that reducing the joint velocity limit in joint_limits.yaml to 0.75 scales the planned velocity down correctly to 0.75, but the robot controller still falls behind.

In the Staubli MCP we can find a setting that limits maximum linear velocity. Currently this is set to 250mm/s. It is not clear if this value influences the maximum velocity used as the base for the percentage maximum velocity calculation. There isn't an obvious relationship to the joint velocity limits. We are going to research to Staubli docs and see if we can find out more about this.

[gavanderhoorn]

In the Staubli MCP we can find a setting that limits maximum linear velocity. Currently this is set to 250mm/s. It is not clear if this value influences the maximum velocity used as the base for the percentage maximum velocity calculation.

ISO 10218 specs 250mm/s maximum for Cartesian motions in manual modes.

If you control the robot with joint space moves having the robot in manual mode will limit performance, yes. But that would be universal across all industrial robots that adhere to 10218.

You'll have to either make sure the translational velocity of the TCP does not exceed 250 mm/s for all your trajectories, or place your robot controller in automatic (or production) mode to allow use of full robot performance.

[marshallpowell97]

@bprice-parc the maximum linear velocity set from the MCP is only applied to motions in manual mode. The percentage maximum velocity calculation does not take this into account. There is not a way to set a linear velocity limit without using a safety configuration on the DSI board (requires a license). The reasoning is that since it is not safety rated, Staubli does not want customers to use it in place of a proper safety configuration. Setting a software velocity limit in the Val3 driver will likely be the way to go. It's not as 'safe', but it could still be used alongside a safety configuration if it needs to be.

I think that this is an issue in staubli_val3_driver/val3/ros_server/pushMotion.pgx and I am currently investigating.

[bprice-parc]

After reading the Staubli ARM manual it appears there are two different velocities specified for each joint. A maximum velocity and a nominal velocity (See Figure 2.10 in Section 2.5.1 Range,Speed,Resolution,Repeatability page 27/86 in Staubli Arm TX series 60 family instruction manual) . The nominal values are somewhere around 50%-80% of the maximum values. If we set the joint_limits.yaml velocities to the nominal values and set the acceleration limits to 9999, we get a much better match between planned and executed trajectories and no jerk. So it would seem that the controller runs closer to nominal velocities than maximum when the velocity ration is 1.0.

With only velocity updated we see the controller accelerates faster than planned:

With only acceleration updated, the actual velocity on the controller is too slow and the controller falls behind.

The best match occurs when both are updated.

Our best theory right now is as follows: We are using relatively low velocities right now. When moveit generates a plan with a zero velocity, the industrial robot client substitutes what it thinks is a low velocity to finish things out (namely 0.1) but this is actually near maximum velocity in our application. Normally this wouldn't have much effect as the robot should be nearly done the trajectory - however, due to a mismatch between URDF parameters in Staubli Experimental and the actual robot, the robot was actually considerably behind by the time it got to the end. In this case the 0.1 velocity is active long enough to be seen as a significant jerk in our low speed application.

It seems like it might be worth while updating the URDF parameters in the Staubli Experimental package. I am not sure if there is a way to check with any other other ROS-I Staubli TX60 users to see if this will cause more problems than it solves.

[gavanderhoorn]

the maximum linear velocity set from the MCP is only applied to motions in manual mode. The percentage maximum velocity calculation does not take this into account. There is not a way to set a linear velocity limit without using a safety configuration on the DSI board (requires a license). The reasoning is that since it is not safety rated, Staubli does not want customers to use it in place of a proper safety configuration. Setting a software velocity limit in the Val3 driver will likely be the way to go. It's not as 'safe', but it could still be used alongside a safety configuration if it needs to be.

@marshallpowell97: are you saying that it's possible for Stäubli robots to move with a TCP speed > 250mm/s in manual mode when commanding joint space moves?

That would be contrary to what I know from 10218 and other industrial robots (and my experience with Stäubli controllers).

[gavanderhoorn]

@bprice-parc wrote:

The nominal values are somewhere around 50%-80% of the maximum values. If we set the joint_limits.yaml velocities to the nominal values and set the acceleration limits to 9999, we get a much better match between planned and executed trajectories and no jerk. So it would seem that the controller runs closer to nominal velocities than maximum when the velocity ration is 1.0.

This is a nice discovery, and not something I'd expected.

Maximum joint velocity values should be just that: maximum joint velocities attainable during a motion.

@marshallpowell97: would you have any comment about how this works?

I am not sure if there is a way to check with any other other ROS-I Staubli TX60 users to see if this will cause more problems than it solves.

@simonschmeisser: would you have opportunity to test this on the TX90 system you have deployed?

[bprice-parc]

Maximum joint velocity values should be just that: maximum joint velocities attainable during a motion.

One of the complexities here is that there are multiple levels of controllers involved. We are not running on the robot itself, but on top of the Staubli motion controller which may be running far away from physical maximums.

[gavanderhoorn]

True, but that manual is all we have when it comes to information about what the robot should be able to do.

It seems like a normal assumption to make as a user that if the manual states that N rad/s is achievable by the robot, the controller would be configured to allow that as well.

At least, that's what my experience is with other robot manufacturers.

[bprice-parc]

Reviewing the table, there is a note (2) which states the "Maximum" values are for "reduced ... load and inertia". So maybe this capability has to be specifically enabled if you know the robot is not actuating a large load in addition to its own mass.

[gavanderhoorn]

Yes, I was just checking as well.

This could simply have been a misunderstanding on the part of the robot support package author.

We should verify for all models and rectify.

For the TX-60 specifically, we should reduce the limits to what the table you included show.

[marshallpowell97]

are you saying that it's possible for Stäubli robots to move with a TCP speed > 250mm/s in manual mode when commanding joint space moves?

No, sorry, I was referring to auto and remote modes, which are able to run a full speed.

would you have any comment about how this works?

I'm not super familiar with the nominal/maximum joint speeds since we typically only work with cartesian speeds. From what I understand, the maximum is only possible when using a low payload and with VEL in the motion descriptor set to >100% (there is not an upper limit, so I am not sure what the actual % would be). I think that the nominal speeds are meant to be a more "realistic" expectation under normal conditions. Although I would take this with a grain of salt as I am not 100% sure.

[simonschmeisser]

@bprice-parc good plots, did you mean to include four of them in the last comparison? By "updated" you mean reduced from "Maximum" to "Nominal"? Does the manual actually give values for acceleration limits? @gavanderhoorn Sorry but our customer just had the tx2-90 lend to them for a demonstration and they currently don't have one. There should be another demonstration (with the tx2-90xl) upcoming and hopefully eventually an actual deployment. I hope to get some more time for tuning and observing then.

[bprice-parc]

did you mean to include four of them in the last comparison?

The behavior using the parameters from the original Staubli Experimental TX 60 config are in the plot contained in the post from 2 days earlier. You can see that the robot gets ahead of plan early on due to acceleration being higher than expected, but falls far behind as the target velocities were above what the controller actually achieved resulting in a large catchup at the end.

"updated" you mean reduced from "Maximum" to "Nominal"?

Yes. The values in Staubli Experimental / TX60 config match the "maximums" in the manual and we updated them to the "nominal" values.

Does the manual actually give values for acceleration limits?

I have not found a reference to accelerations yet. Instead, Staubli gives torque limits for joints (Section 2.6.2 page 30/86).

[gavanderhoorn]

My suggestion would be to replace all joint limits (in all packags) with the ones listed in the Nominal rows.

[gavanderhoorn]

@marshallpowell97: could you perhaps provide some advice on whether by default the Nominal or Maximum joint velocity limits should be used?

Would be good to have some conclusion here.

[marshallpowell97]

@gavanderhoorn From what I've been able to gather, the maximum values are a "best case" scenario, and cannot be expected from multiple joints at the same time. The motion descriptor only has a singular "vel" parameter which is applied to the joint that is moving the furthest and all of the other joints will move at a percentage of that. So using the nominal velocities would be my recommendation.

[marshallpowell97]

Also, side note, I was able to replicate the trajectory above at one point, but now something has changed and my new trajectory looks super messy: Any ideas what I could have done? I am using the nominal velocities and accel limits of 9999. (This is not using any of the changes from #8 )

[gavanderhoorn]

Unfortunately, without looking into this I wouldn't know.

I would recommend to take a look at the ROS side of things though (ie: plot juggler and rostopic echo of the goal topic).

Looks like lots of acceleration/deceleration?

[gavanderhoorn]

@marshallpowell97: have you had any opportunity to look into the trajectory execution performance? The plots you show indeed don't look very nice.

[gavanderhoorn]

Even though #8 was merged, I've not closed this as I believe we've not actually resolved it.

The velocity interpolator added in #8 seems to not work correctly (tracking in #18), so I expect the same problem to exist.

Summarising: the last segment appears to be given a higher value for the velocity than the immediately preceding point in some cases, which leads to a sudden jump in TCP velocity and acceleration.

See https://github.com/ros-industrial/industrial_core/issues/247 for my reasoning.

@marshallpowell97: would it perhaps be an idea to use duration, instead of velocity? Just to see whether this improves the behaviour of the robot?

[Oguked]

We have the same issue with our TX2-60L... still no solution?

[gavanderhoorn]

According to the analysis performed in the earlier comments, the problem is not caused by anything here in staubli_val3_driver, but in the trajectory relay in industrial_robot_client.

As no one has spend time fixing the way in which the trajectory relay converts JointTrajectory timing-information into the Simple Message representation, there has been no change to this behaviour.

Another factor could be what was reported in https://github.com/ros-industrial/staubli_val3_driver/issues/2#issuecomment-563364169.

[leo002]

Summarising: the last segment appears to be given a higher value for the velocity than the immediately preceding point in some cases, which leads to a sudden jump in TCP velocity and acceleration.

For what ever reason, the moveit planner seems to send a default speed for the last segment of the motion, to the joint_trajectory_action/goal topic, which directly controls the robot.

By downloading the industrial_core package and fixing the following line solved the problem for me:

file: industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_interface.h Line 72: JointTrajectoryInterface() : default_jointpos(0.0), default_vel_ratio(0.1), defaultduration(10.0){} change default_velratio(0.1) to default_velratio(0.01)