Automatic speed limiting in MotoROS?

[ted-miller]

Someone proposed today that we modify the MotoROS code to automatically limit the incremental data if it exceeds the maximum speed-limit for any axis.

I have a few concerns. I'm interested in hearing other people's thoughts.

1. If one axis gets limited, we need to scale the other axes proportionally. Although this could probably be implemented on a single-arm system, it becomes more complicated on a multi-arm system. I don't think we have any mechanism to know whether the other control-groups are supposed to be synchronized or independent.

2. Ignoring the multi-group scenario above, is there consequence to the robot not behaving the commanded trajectory? The truncated position would have be added on during the next time cycle... so is it a problem in the ROS environment if the robot's speed fluctuates around the commanded speed?

3. From a usability perspective, is it best to stop the robot and alarm (notify the user that they're trying to go too fast so that they can correct the process)? Or is it better to keep running at the expense of expected performance? I can think of scenarios where either one could be better than the other.

Anyone have any thoughts?

[gavanderhoorn]

Are you specifically referring to scaling down increments when they would lead to resulting motions going outside robot limits?

[ted-miller]

Yes

[gavanderhoorn]

I would not be in favour of that.

The behaviour I've observed in these sort of situations (our SIA20F on a rail) is that the safety systems of the controller will abort any motion as soon as it violates a limit, and the TP will show an error.

This seems to me the 'ideal' situation:

1. it's immediately clear something is wrong
2. it's already implemented
3. it aligns with how this would work with normal/INFORM motions (afaik, if it were even possible to command motions outside the limits)
4. it's transparent (scaling would not be)
5. it's intuitive
6. has none of the corner cases you mention
7. does not require special handling on the ROS side

And finally: it's explicit. I cannot overstate how important that is. Plodding along at scaled down speeds just so the robot will at least do something is not a good thing. Being explicit about what is and isn't possible and supported is. There are too many places in ROS (and the pipeline from planning, motion generation, execution, driver and controller) already that leave things implicit.

Could you perhaps give a bit more of the rationale for wanting to do this? I'm pretty sure I'm not seeing something scaling would make easier/better/desirable.

So is it a problem in the ROS environment if the robot's speed fluctuates around the commanded speed?

Well .. yes. At least for MoveIt's trajectory execution monitor. That part of MoveIt monitors the execution of trajectories and will step in if things take too long or are stalled - typically by aborting execution completely, but that is configurable / extendable. Note that this is not a safety-system, but a first check in the pipeline to not unnecessarily wait for completion of actions that are stalled fi.

If the entity that executes a trajectory (in this case the controller, but could be something else) starts changing the way it executes trajectories without the monitor being aware of this, it'll most likely conclude that something is wrong.

The monitor can be a) disabled and b) configured to allow for more slack, but that's all work-arounds: the robot should just execute the trajectory as ordered and the trajectory should not contain motions that the robot cannot perform. That is all part of the contract between the driver and the controller and should be respected by both sides.

Being explicit about contract breaches (ie: joint limit violations) is a good thing in such contexts.

[gavanderhoorn]

The behaviour I've observed in these sort of situations (our SIA20F on a rail) is that the safety systems of the controller will abort any motion as soon as it violates a limit, and the TP will show an error.

not remembering that ROS uses metres for everything and then asking your linear axis to move to +1000m in 1 second is an impressive way to learn about this.

The rail moved only 1 cm before the controller figured out this was impossible and aborted the entire trajectory.

[gavanderhoorn]

Note that other mfgs, such as KUKA, also don't do this (ie: scale). Typical errors seen when using RSI and commanding violating motions are Command Gear Torque An (with n <- N).

[shaun-edwards]

I think it's fair for the robot to expect that the incoming trajectory is achievable and if it's not, the user is responsible for taking corrective action. The proposed change to alter the trajectory at the lowest level would be detrimental to any application where trajectory where timing was important. Painting and welding come to mind. Synchronizing with other equipment would also be dependent on trajectory timing (although I'm not sure I would recommend this practice).

Traditional robot programs will sometimes alter their trajectories based on path constraints. However, in these cases, the robot program is simply executing a pre-programmed path. Once the path is "approved' by the operator, the execution will be consistent, even if not adhering exactly to the desired trajectory constraints.

I would challenge your user to create achievable trajectories. What within ROS or their software is specifying a bad trajectory? It's much better to check the trajectory at a higher level, given that trajectory smoothing across multiple points would be needed. Checking and correcting at the point to point level is not ideal.

[gavanderhoorn]

@shaun-edwards: one use-case I could think of where the proposed automatic scaling does make some sense is in material handling (ie: pick-and-place). You don't necessarily care how fast the robot is going, as long as it is as fast as it can go (as you want to minimise cycle times). There is no way to communicate that currently to MotoROS with motoman_driver.

However, setting too high velocities and then requiring MotoROS to scale back as needed does not seem like the correct way to achieve this (it's almost the opposite): it's like asking for something by not specifying it.

We could try to think of a nice way to achieve go-as-fast-as-possible without violating semantics of JointTrajectory or JOINT_TRAJ_PT_FULL and / or ROS_MSG_MOTO_JOINT_TRAJ_PT_FULL_EX. One thing that could make this problematic is it would need to be something that cannot be the result of misuse of the current messages (ie: forgetting to populate a field, or setting incorrect values for a field).

[alemme]

If a trajectory is generated by moveit or similar motion planning pipelines and the execution causes exceeding the maximum speed-limit for one or more axis, doesn't that mean that the planning simply was working with the wrong speed/velocity limits?

I think it's fair for the robot to expect that the incoming trajectory is achievable and if it's not, the user is responsible for taking corrective action.

In moveit the joint limits are typically available for each robotic system, specified in the corresponding conifg files. My suggestion would be to expose the currently active limits in either the rosparam server or by providing a rosservice. That way the joint_trajectoy_action can use it for checking the trajectory requests and reject them if the trajectory is not within limits before actualy sending it to the hardware. Also users could then easily correct their parameters.

In the sda10 setup I use, the torso joint limit was set to 40% of the maximum value, so it happened quite frequently that the limit was exceeded, because I used the standard limits.

[gavanderhoorn]

@alemme wrote:

My suggestion would be to expose the currently active limits in either the rosparam server or by providing a rosservice. That way the joint_trajectoy_action can use it for checking the trajectory requests and reject them if the trajectory is not within limits before actualy sending it to the hardware.

this should already be supported in industrial_robot_client. The method is_valid(..) checks (among other things) whether a trajectory contains points that specify velocities outside joint limits (here). Validation should fail if that is the case.

motoman_driver extends industrial_robot_client and should make use of that functionality as well (here).

That method requires a urdf (and only works with urdf at the moment) to be present in robot_description, and I've noticed that the parsing and extraction of joint velocity limits (here) can sometimes be brittle.

Besides making it a little more robust, we could perhaps extend this method with what @alemme suggests and allow for a non-urdf way to set (or override) those limits.

[andreaskoepf]

My opinion regarding trajectory-resampling or clipping inside the controller (in the MotoPlus part) is: Do not do this. The driver should reject any trajectory that violates a limit (pos, vel, acc), e.g. by reporting an error.

What I could imagine is to be able to generally set a time-scaling factor (which could be set <1). This could be simply a factor on the controller clock step-increments used during interpolation, e.g. with a factor of 0.1 and a interpolation period of 4ms the interpolation time would only be incremented by 4ms after 10 interpolation cycles. While it could be easily integrated such an execution slow-down can be very problematic for other ROS components, e.g. when the trajectory execution monitoring of MoveIt! is active and the duration check is on it would report a timeout error because the trajectory was not finished within its planned time + x.

Beside pos and vel limits a very critical parameter that can lead to invalid trajectories (e.g. excessive segment or overspeed errors) is also the maximum acceleration. We ran our own experiments with our SDA since we are currently not aware of any publicly available specs by Yaskawa regarding acceleration of Motoman robots. It seems currently you either have to use low conservative vaules to be on the safe side or to run your own higher-speed rexperiments. On the MotoPlus side there is the max-pulse-increment value but it is not exposed to the ROS world. I personally would highle appreciate it if the hardware limits could be directly read out from the controller and exposed to the ROS side (e.g. via service or ROS params). For Rosvita we ended up to define not only pos,vel,acc limits but also cartesian task-space limits for safe 6D cartesian setpoint-jogging.

@ted-miller (maybe should be a different issue): A very big problem when trying to execute multiple concurrent high-speed moves (at least observed on our SDA) is to determine real final convergence / end-of-trajectory to safely read out the start-point of the following trajectory. The biggest problem here is the very tight check for the trajectory start position in the MotoPlus driver which allows only for minimal pulse-differences or it will general an error. You can see in this video early test we did - as you can see the actual robot moves DURING the move are at 'industrial level' but it takes a realitve long time for the SDA to get to a stand-still that allows to read out a joint-value that can be used for consecutive motion-planning. If you read out the value to early the next trajectory will be rejected by the motoros driver since it detected a too high deviation of the pulse-position. We ended up to increase the pulse-deviation tolerance and could thereby improve the performance of consecutive motion execution but in general I think this is still a big problem. Maybe the simplest workaround here would be not to use the join-feedback data but the last command position for testing trajectory startpoint deviation?

[gavanderhoorn]

@andreaskoepf: please post your last comment as a separate issue. This seems orthogonal to what is being discussed in this thread.

[andreaskoepf]

(@gavanderhoorn I have to do a further analysis, I realized that on the MotoPlus side the Command-Position is already used (see MotionServer.c#L1152-L1161), e.g. there is probably the problem I mentioned in #217 (comment), e.g. MoveIt reads joint feedback as start-positon of next trajectory segment but the motor-controllers have not converged at that point -> command pos != feedback pos. If I have a clear understanding I will open a separate issue, sry).

[gavanderhoorn]

You could perhaps already open an issue. The fact that it was / is unclear to you what is going on means it's worth documenting.

It could be an 'investigative' issue and also one where we can centralise the discussion around this topic.

[gavanderhoorn]

I believe we reached a consensus about this @ted-miller (as in: don't auto-scale trajectories, but error out).

If you agree, could you please close this issue?

If there are any action items resulting from this discussion, could you open new issues for those?