Planner plans trajectories that exceed acceleration limit.

SansoneG commented 4 years ago

Commit

cb782c272ee6

Steps to reproduce

How can the issue/bug be reproduced?

Switch into AUTO mode
Use RViz to plan a movement with velocity- and acceleration scaling 1.0.
Execute the movement.

Expected behavior

Planning and Executing works without error.

Observed behavior

Planning works, but executing fails. The robot doesn't even start moving.

Logfiles

[ INFO] [1592834842.386682498]: Recovering successful
[ INFO] [1592834848.094568331]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1592834848.094682587]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1592834848.094722985]: Generating PTP trajectory...
[ INFO] [1592834849.925210143]: Execution request received
[ERROR] [1592834849.972380926]: Acceleration limit violated by joint prbt_joint_6. Desired acceleration: 4.43134rad/s^2, limit:  4rad/s^2.
[ INFO] [1592834849.972646862]: Controller prbt/manipulator_joint_trajectory_controller/ successfully finished
[ WARN] [1592834849.972706115]: Controller handle prbt/manipulator_joint_trajectory_controller/ reports status PREEMPTED
[ INFO] [1592834849.972743617]: Completed trajectory execution with status PREEMPTED ...
[ INFO] [1592834849.972813891]: Execution completed: PREEMPTED
[ INFO] [1592834850.003672288]: PREEMPTED: Preempted

Reference to test

https://github.com/PilzDE/pilz_robots/blob/melodic-devel/pilz_control/test/acceptance_test_acceleration_limit.md

martiniil commented 4 years ago

@jschleicher @hslusarek I would like to discuss if we could skip the stop trajectory completely and let the firmware do the stopping. Then we would not need the acceleration limit, either.

jschleicher commented 4 years ago

I'm not sure if, the firmware offers a stopping command in the meantime. My latest information is, that "halt" does close the brakes and thus wears out the mechanics, which has only limited numbers of "STOP-0" cycles.

martiniil commented 4 years ago

But if we abort the trajectory and stay on the latest position, the firmware will stop the movement asap without closing the brakes?

hslusarek commented 4 years ago

But if we abort the trajectory and stay on the latest position, the firmware will stop the movement asap without closing the brakes?

I'm not sure if I understand your suggestion correctly, because your suggestion will cause a massive jerk if the robot moved with full speed beforehand. For me, a strong/massive jerk does not sound good, especially with regard to the mechanical components of the robot. But maybe I misunderstand your suggestion?

martiniil commented 4 years ago

But if we abort the trajectory and stay on the latest position, the firmware will stop the movement asap without closing the brakes?

I'm not sure if I understand your suggestion correctly, because your suggestion will cause a massive jerk if the robot moved with full speed beforehand. For me, a strong/massive jerk does not sound good, especially with regard to the mechanical components of the robot. But maybe I misunderstand your suggestion?

Yes, you understood correctly. But:

As far as I remember, the firmware is responsible for avoiding damage to the mechanics. Maybe I'm wrong? Do we have to avoid some kind of "jerk"? Then we lack clear requirements on the controller. Even now, it is very easy to command motions, which result in such jerks.
In our case, the robot would not be allowed to move with full speed, only reduced.

jschleicher commented 4 years ago

The joint values sent by the controller should obey the joint limits for both velocity and acceleration. That's what the configuration values are meant for.

But this issue is about planned trajectories that violate the acceleration limit in the controller - is there any connection to the stop trajectory, that I'm missing? Or is your suggestion independent from our current issue?

martiniil commented 4 years ago

The joint values sent by the controller should obey the joint limits for both velocity and acceleration. That's what the configuration values are meant for.

But this issue is about planned trajectories that violate the acceleration limit in the controller - is there any connection to the stop trajectory, that I'm missing? Or is your suggestion independent from our current issue?

My suggestion is connected to the current issue, because I questioned the acceleration limit which was only introduced due to a poor stop trajectory. I know, I question a lot... But it is answered now. Thank you.

hslusarek commented 4 years ago

I know, I question a lot...

I think this is a good quality for a developer. :+1:

martiniil commented 4 years ago

This is the desired velocity the controller produces from a PTP trajectory. You can see that the acceleration varies at the beginning:

ptp_acceleration_limit_violation

martiniil commented 4 years ago

I think the trajectories which are generated by the planner do not violate the limit. But through its interpolation routine the controller can slightly deviate from the intended trajectory.

I see possible solutions:

Increase the limit for the controller/Decrease limit for planners (how much?)
Tweak the planners. Some aspects:
- Is it really necessary/helpful to specify velocities/accelerations instead of just positions? What is the effect of each option?
- Velocity profile: Can it be tweaked?
Fix stop trajectory such that the acceleration limit is no longer needed.

jschleicher commented 4 years ago

The first option sounds like a quick fix. So I'd prefer to increase the limit inside the controller by 5-10% for now. I'll add a PBI to determine the actual limits to achieve 250mm/s incl. the delay.

ct2034 commented 4 years ago

@SansoneG can you please elaborate on the original issue: You are in Auto mode, right? Then we should not care about the limits.

SansoneG commented 4 years ago

@SansoneG can you please elaborate on the original issue: You are in Auto mode, right? Then we should not care about the limits.

Yes, the behaviour was observed in AUTO mode. The acceleration and velocity limits imposed by the robot hardware should not be exceeded even in AUTO mode. I think this is what was meant here. But by planning a movement with seemingly accepted acceleration values, the controller stops the movement. That is the issue.

ct2034 commented 4 years ago

Yes, the planner planning a trajectory that not violates the limits is one thing. We use the speed override for this: https://github.com/PilzDE/pilz_robots/blob/81b2dff62e9142139850a58e718d13d5355cfb97/prbt_hardware_support/src/operation_mode_setup_executor.cpp#L43

But I think we do not evaluate the operation mode when checking the acceleration limit: https://github.com/PilzDE/pilz_robots/blob/81b2dff62e9142139850a58e718d13d5355cfb97/pilz_control/include/pilz_control/pilz_joint_trajectory_controller_impl.h#L326 We check it when evaluating the speed limit: https://github.com/PilzDE/pilz_robots/blob/81b2dff62e9142139850a58e718d13d5355cfb97/pilz_control/src/cartesian_speed_monitor.cpp#L91

SansoneG commented 4 years ago

I understand where you are coming from now. If I understand correctly we cannot simply ignore the acceleration limit in AUTO mode, because the acceleration limit also serves the purpose of not allowing crazy stop trajectories.

martiniil commented 4 years ago

Yes, the planner planning a trajectory that not violates the limits is one thing. We use the speed override for this:

https://github.com/PilzDE/pilz_robots/blob/81b2dff62e9142139850a58e718d13d5355cfb97/prbt_hardware_support/src/operation_mode_setup_executor.cpp#L43

The purpose of setting the speed override to 0.1 is limiting the cartesian speed in T1. The acceleration limit is another matter. It should never be violated such that the hardware is not damaged.

martiniil commented 4 years ago

The first option sounds like a quick fix. So I'd prefer to increase the limit inside the controller by 5-10% for now. I'll add a PBI to determine the actual limits to achieve 250mm/s incl. the delay.

The quick fix should work for now. Imho the time and effort we would spend in determining "good" limits (I think it is not straightforward) would be better invested in a proper solution.

ct2034 commented 4 years ago

The acceleration limit is another matter. It should never be violated such that the hardware is not damaged.

Okay, I got it. Thanks

martiniil commented 4 years ago

For better understanding this picture shows the desired joint velocity of a single joint at the beginning of a PTP trajectory (Acceleration limits disabled).

The acceleration with which the trajectory is planned is exceeded in the first segment.

ptp_acceleration_limit_violation_detailed

martiniil commented 4 years ago

@jschleicher For the demo program we need more than 100% increase of the limit inside the controller. See https://github.com/PilzDE/pilz_robots/pull/442

martiniil commented 4 years ago

LIN trajectory with acceleration scale 0.1: lin_acceleration_limit_violation

martiniil commented 4 years ago

The oscillations shown above happen for LIN and CIRC.

I guess the crucial point is the computation of the velocities in the trajectory points. For PTP the velocities are obtained directly from the trapezoidal profile. Opposed to that, LIN and CIRC are planned in cartesian space, the positions are transformed via the inverse kinematic and the velocities are computed as follows: https://github.com/PilzDE/pilz_industrial_motion/blob/dd9b30aaaab9c3cdb4b524d4970dfeb91e48e62f/pilz_trajectory_generation/src/trajectory_functions.cpp#L290 Using these approximated values, the positions and velocities do not "fit together" very well which leads to the oscillations.

fmros commented 3 years ago

d values, the positions and velocities do not "fit together" very well which leads to the oscillations.

I am seeing similar oscillations of the velocity profile from a LIN planned trajectory. But evaluating the output-trajectories with the rqt_joint_trajectory_plot tool shows me, that the trajectories are clean before entering the joint_trajectory_controller.

After some investigation, i found this parameter. in the trajectory_generator.h. I increased the frequency by a order of 10, so sampling_time = 0.01 and the resulting velocity/acceleration trajectories become free of oscillations. I assume that the sampling_time of 10Hz is to low for a smooth interpolation of the joint-trajectory controller?

Unfortunately I did not find yet, where this parameter could be passed to the trajectorygenerator*, so it remains hard-coded for me at the moment.

Would be happy to get a feedback to this thoughts

PilzDE / pilz_industrial_motion