hold position behavior in face of steady state error

[dcconner]

We're seeing an issue on robot where a joint is physically unable to reach desired position due to actuator force limitation. This results in a steady state error.

Thus we get a constant control command= k_p * error.

When the trajectory enters the setHoldPosition it uses getPosition (actual position) on https://github.com/ros-controls/ros_controllers/blob/indigo-devel/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h#L689 This takes error to zero, and reduces the servo command value causing a perturbation in the system.

For cases with potential steady state error (e.g. holding a heavy load), should we not hold to the previous desired position to provide constant control.

Only on controller (re)start should we initialize desired position equal to the actual position.

[adolfo-rt]

The setHoldPosition method is called when:

• The controller starts
• An action goal is canceled
• An empty trajectory is sent through the topic interface (the topic way of canceling)

I would argue that if you cancel execution, you expect joints to stop, and remain stopped. If we hold desired position and the error is large, the source of error might disappear at some point and cause the robot to jump to a far-away reference. This might happen a long time after we entered hold position mode, and could be very surprising and undesired. Example error sources that can disappear/change are a moving obstacle or the robot reorienting itself differently wrt gravity.

I'm still open to discussing the proposal, but it seems to me that this should be solved at a lower level.

[adolfo-rt]

An alternative that comes to mind would be to reimplement setHoldPosition such that when the stop_time parameter is zero (the default), the current behavior is preserved, but when greater than zero it computes a stopping trajectory from the desired position and velocity to the actual position and zero velocity.

This might be an acceptable compromise.

[dcconner]

Not sure the suggested compromise addresses the issue. Anytime the output "desired" value tracks actual, the system with not hold if the code is depending on proportional error gain to hold the required effort.

I need to do some more digging, but that must wait a bit.

[adolfo-rt]

My understanding of your problem is that you get a perturbation because the desired state discretely jumps to track the current state. The proposed alternative would create a trajectory that goes from the desired state to the current one in a specified time, making the transition continuous.

[sachinchitta]

@dcconner - what is your underlying actuator interface? Are you specifying torques to hardware level controllers or position-velocity setpoints? You might need a feed-forward term to compensate for the situation you are seeing.

[dcconner]

@sachinchitta
We're using the JointTrajectoryFollower and PID from control_toolbox to generate a "controller command", this gets added to some gravity compensation and feedforward terms before being sent to a hydraulic servo valve command. Model errors and friction/stiction prevent us from getting non-zero steady state errors, and we haven't tuned the PID values aggressively due to issues with trajectory quality from https://github.com/ros-planning/moveit_core/issues/167.

All this, coupled with a joint torque limit, led to a steady state error and a significant contribution to the servo command from the k_p gain. When setHoldPosition is called (I thought this was called at the end of trajectory) the tracking error goes away, and with it the k_p*error component of the servo command. This causes the arm to jerk.

Reducing the steady state error reduces this effect, but that is not always the possible (e.g. joint torque limit). If a portion of the command is required just to maintain position, and that disappears as the tracking error is reset by setHoldPosition, then the joint will sag/jerk.

In my opinion, if a controller is active and the trajectory ends or a new trajectory is activated, then blending should be from the prior "desired" state and not the actual. This will allow for "bumpless" transfer between trajectories as the instantaneous control command will be consistent.

Admittedly this will cause issues if the underlying system dynamics change (e.g. if the system drops a heavy object or a constraint is removed as @adolfo-rt suggested above), but I see that an an issue for high-level decision making. If you detect the goal is not acheived, then the system can re-issue a new trajectory or change controllers. The issue I'm describing here seems to be lower level.

Regarding @adolfo-rt last comment, if you require the k_p*error component of the servo command to hold position (e.g. if you do not have significant integral), then tracking the actual as desired will cause the arm to sag as the system is essentially chasing it's tail. (Integral terms have their own nastiness in cases of actuator saturation, so we tend to clamp those if we use at all.)

[dcconner]

I'm going to test the following change in our fork:

@@ -593,8 +593,12 @@ cancelCB(GoalHandle gh)
     // Controller uptime
     const ros::Time uptime = time_data_.readFromRT()->uptime;

-    // Enter hold current position mode
-    setHoldPosition(uptime);
+    // Retain current command for bumpless stitching
+    if (0 == gh.getGoal()->trajectory.points.size()) // if new trajectory keep current command, otherwise hold current
+    {
+        // Enter hold current position mode
+        setHoldPosition(uptime);
+    }

The intent is that if we have an active trajectory, then retain and stitch using the current trajectory. If Cancellation is due to an empty command, then call the setHold.

After digging, I realized I mis-interpreted the behavior if the trajectory completes; the upper bound in sample will return the last segment, so setHoldPosition is not called. The fix above should address my issue, while still allowing you to cancel an active trajectory if you so desire. I'll test this tomorrow with @skohlbr, and check back in.

[adolfo-rt]

In my opinion, if a controller is active and the trajectory ends or a new trajectory is activated, then blending should be from the prior "desired" state and not the actual. This will allow for "bumpless" transfer between trajectories as the instantaneous control command will be consistent.

This is what I'm proposing: On cancel, go from the desired to the actual state in stop_time seconds. If stop_time > 0, you should see an improvement.

[sachinchitta]

Had forgotten about this when I did #177. I like adolfo's last idea - go from desired to actual state - was going to suggest it as the next step for #177. The method that's currently implemented in setHold (looking for a sample at stop_time for a segment that's 2x stop_time and hoping it has zero velocity) will not work though. I think we need a better solution overall - the choice of stop_time is currently user-driven so kind of arbitrary, setHold can try to choose a target that is outside joint limits. Ultimately, we are looking for something that will bring the robot to a stop as fast as possible when you abort a trajectory (maybe along the trajectory itself).

[dcconner]

We just implemented a custom approach in a fork.

There are issues with either holding desired as Adolfo points out (e.g. what happens if disturbance disappears), or with going to actual even over some time as Adolfo suggested for "fix". In this case, if you have steady state error, you will just end up letting the arms sag instead of "holding."

As the "best" solution will be application/system dependent, having an easy way to select this desired behavior might be the best long term solution.

[adolfo-rt]

I agree that the stop_time parameter is arbitrary, and is not the best solution. A much better solution would be to perform some form of limits enforcing inside the controller, as we not only know the current state, but also the desired state all the way up to acceleration (and higher derivatives, if needed). It has been mentioned in a few other threads to have a variant of joint_trajectory_controller with an alternate interpolator that is joint-limits-aware. There has been discussion on basing it on the Reflexxes library, but so far no development time has gone down this road.

I still think that having some form of limits enforcing at the robot driver level is a desired feature, even if the controller is doing some enforcing, so one strategy does not invalidate the other.