Controller performs trajectory sent by MoveIt! , but it aborts with status: goal tolerance violated

[waldez junior]

Hi Everyone,

I have a custom robot arm simulated inside gazebo that uses ros_control in order to control the robot from ROS. 

Apparently everything is working well, but I keep getting some warnings from MoveIt and from the controller that are really bugging me.

Whether I send an end-effector pose to the robot with the C++ API or with the the RVIZ gui , the controller always returns GOAL TOLERANCE VIOLATED as soon as the arm reaches the goal (it has a lil bit of overshooting), even though I set goal tolerances as high as 0.5.

And then I noticed two things: 

-Although it says its gonna abort the action, it finishes to complete the action and succesfully achieves the desired goal.

-The second is that in the controllerś terminal screen the controller always gets me the following warning:

[ WARN] [1398541940.787273604, 37.613000000]: Dropping first 1 trajectory point(s) out of 10, as they occur before the current time.

First valid point will be reached in 0.591s.

I got the impression that it have something to do with the fact that the controllers might have a little too much overshooting when setting the arm's joint position, but even when the overshooting is really low, the same errors occur.

I've tried several thing like setting goal tolerances in the controllers .yaml  file but it didnt do much , I suspect moveit overwrites these tolerances later.

I also checked on the /arm_controller/state topic and I got messages like this one, that shows a very small position error:

 joint_names: ['shoulder_base', 'shoulder_pitch', 'shoulder_yaw', 'elbow_pitch', 'wrist_roll', 'wrist_yaw', 'wrist_pitch']

desired: 

  positions: [1.5407998256742936, -0.021604311166606778, 0.0731369922469057, 0.05369559340623227, 0.04452054172957319, 0.0028327113593034476, -0.020179279295668835]

  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

  accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

  effort: []

  time_from_start: 

    secs: 0

    nsecs: 0

actual: 

  positions: [1.5407994175728081, -0.02160636513611447, 0.07313716963359607, 0.053704695384432455, 0.044519216801671924, 0.0028136686987876303, -0.02018912514018467]

  velocities: [-0.0008687290709627884, -0.004263925852244968, 0.00020412806365846796, 0.018052722323870264, 0.0030874503701598575, -0.03118019501356243, -0.02106780469845654]

  accelerations: []

  effort: []

  time_from_start: 

    secs: 0

    nsecs: 0

error: 

  positions: [4.081014854939724e-07, 2.0539695076912667e-06, -1.7738669036426735e-07, -9.10197820018499e-06, 1.3249279012667037e-06, 1.904266051581732e-05, 9.845844515837116e-06]

  velocities: [0.0008687290709627884, 0.004263925852244968, -0.00020412806365846796, -0.018052722323870264, -0.0030874503701598575, 0.03118019501356243, 0.02106780469845654]

  accelerations: []

  effort: []

  time_from_start: 

    secs: 0

    nsecs: 0

I know those are all just warnings that could easily be ignored, but I just wanna make sure I'm not overlooking anything that might turn out to be really important.

Thanks in advance for any help with it,

Waldez Jr

[Adolfo Rodríguez Tsouroukdissian]

On Sat, Apr 26, 2014 at 10:19 PM, waldez junior <walde...@gmail.com> wrote:

Hi Everyone,

Hi Waldez,
 

I have a custom robot arm simulated inside gazebo that uses ros_control in order to control the robot from ROS. 

Apparently everything is working well, but I keep getting some warnings from MoveIt and from the controller that are really bugging me.

Whether I send an end-effector pose to the robot with the C++ API or with the the RVIZ gui , the controller always returns GOAL TOLERANCE VIOLATED as soon as the arm reaches the goal (it has a lil bit of overshooting), even though I set goal tolerances as high as 0.5.

Which goal tolerance are you setting to 0.5, constraints/goal_time or constraints/<joint>/goal?. It might be useful to set the time tolerance to a value greater than zero (if you're not already doing so). Doc of what these parameters mean, and their defaults can be found here. You could also copy your controller config here for inspection.

In its current implementation, when the controller aborts it does not specify more details on the nature of the violated goal tolerance. My hope is to remedy this in the indigo branch of ros-controllers. A first step has been taken to add an error string to the action response in ros-controls/control_msgs#4. What is pending is to make the controller populate this string with useful and descriptive information (and do so in a realtime-safe way).

And then I noticed two things: 

-Although it says its gonna abort the action, it finishes to complete the action and succesfully achieves the desired goal.

This issue has been raised in ros-controls/ros_controllers#48. Since the proposal implies a change in controller behavior (stopping on abort, instead of continuing), I decided to not push it into the already released hydro code. It will likely make it into indigo. If you support this change, feel free to +1 the issue to put some pressure for its prompt resolution.

-The second is that in the controllerś terminal screen the controller always gets me the following warning:

[ WARN] [1398541940.787273604, 37.613000000]: Dropping first 1 trajectory point(s) out of 10, as they occur before the current time.

First valid point will be reached in 0.591s.

You have an eye for detail. This warning won't cause any execution issues, and is unrelated to controller overshooting. It's MoveIt! and ros_control mildly not getting along ;-). MoveIt! generates motion plans where the trajectory start time is zero (meaning start now) and the first waypoint has zero time from start (meaning be there now). The joint_trajectory_controller, OTOH rejects all waypoints whose timestamp is not in the future, because there is no way to reach a configuration in zero time. Of course, if the first waypoint is the current configuration it can be reached (it already has). The details on how the joint_trajectory_controller handles trajectory replacement are documented here.

This point deserves some discussion in its own right. Is it really necessary to add the current configuration to the plan?, and if so, could it be timestamped in the future?. On the controller side, would it make sense to change the policy and accept waypoints happening 'now'?.

I don't know at which moment this data was taken. Since the controller goal currently continues execution even after getting aborted, it might very well be the case that the position and velocity tolerances are satisfied _after_ the goal time tolerance has elapsed.

I know those are all just warnings that could easily be ignored, but I just wanna make sure I'm not overlooking anything that might turn out to be really important.

Thanks in advance for any help with it,

...and keep on picking on the things that don't make sense to you :)

Adolfo.
 

Waldez Jr

--
Adolfo Rodríguez Tsouroukdissian
Senior robotics engineer
adolfo.r...@pal-robotics.com
http://www.pal-robotics.com

PAL ROBOTICS S.L
c/ Pujades 77-79, 4º4ª
08005 Barcelona, Spain.
Tel. +34.93.414.53.47
Fax.+34.93.209.11.09
Skype: adolfo.pal-robotics
Facebook - Twitter - PAL Robotics YouTube Channel

AVISO DE CONFIDENCIALIDAD: Este mensaje y sus documentos adjuntos, pueden contener información privilegiada y/o confidencial que está dirigida exclusivamente a su destinatario. Si usted recibe este mensaje y no es el destinatario indicado, o el empleado encargado de su entrega a dicha persona, por favor, notifíquelo inmediatamente y remita el mensaje original a la dirección de correo electrónico indicada. Cualquier copia, uso o distribución no autorizados de esta comunicación queda estrictamente prohibida.

CONFIDENTIALITY NOTICE: This e-mail and the accompanying document(s) may contain confidential information which is privileged and intended only for the individual or entity to whom they are addressed.  If you are not the intended recipient, you are hereby notified that any disclosure, copying, distribution or use of this e-mail and/or accompanying document(s) is strictly prohibited.  If you have received this e-mail in error, please immediately notify the sender at the above e-mail address.

[Martin Günther]

Hi Adolfo,

On Mon, 28 Apr 2014 12:37:36 +0200
Adolfo Rodríguez Tsouroukdissian <adolfo.r...@pal-robotics.com>
wrote:

> You have an eye for detail. This warning won't cause any execution
> issues, and is unrelated to controller overshooting. It's MoveIt! and
> ros_control mildly not getting along ;-). MoveIt! generates motion
> plans where the trajectory start time is zero (meaning start now) and
> the first waypoint has zero time from start (meaning be there now).
> The joint_trajectory_controller, OTOH rejects all waypoints whose
> timestamp is not in the future, because there is no way to reach a
> configuration in zero time. Of course, if the first waypoint is the
> current configuration it can be reached (it already has). The details
> on how the joint_trajectory_controller handles trajectory replacement
> are documented

> here<http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement> .

>
> This point deserves some discussion in its own right. Is it really
> necessary to add the current configuration to the plan?

It definitely makes sense to add the current configuration to the plan.
I wrote a joint trajectory action server for the Katana arm; I have to
use the (dumb) controller boards on the arm, so I can't do clever
trajectory replacement. All I can do is check whether the current state
is identical to the first trajectory point; if yes, fine, otherwise
reject. Also, the first trajectory point contains essential information
about the velocities and speeds. Perhaps the ros_control implementation
always recalculates the first trajectory point, but that's a special
case.

> , and if so,
> could it be timestamped in the future?. On the controller side, would
> it make sense to change the policy and accept waypoints happening
> 'now'?.

All very good questions. There should be a clear specification of the
JointTrajectory action that clarifies these points.

As a short-term workaround it may make sense if ros_control would check
if the unreachable trajectory point is reasonably close in time and
positions to the current state, and only output the warning if not.
Alternatively, it could print the time and joint angle differences in
the error message so the user could check that it's a false alarm.

Cheers,
Martin

[Adolfo Rodríguez Tsouroukdissian]

On Mon, Apr 28, 2014 at 4:20 PM, Martin Günther <martin.gu...@gmail.com> wrote:

Hi Adolfo,

On Mon, 28 Apr 2014 12:37:36 +0200
Adolfo Rodríguez Tsouroukdissian <adolfo.r...@pal-robotics.com>
wrote:

> You have an eye for detail. This warning won't cause any execution
> issues, and is unrelated to controller overshooting. It's MoveIt! and
> ros_control mildly not getting along ;-). MoveIt! generates motion
> plans where the trajectory start time is zero (meaning start now) and
> the first waypoint has zero time from start (meaning be there now).
> The joint_trajectory_controller, OTOH rejects all waypoints whose
> timestamp is not in the future, because there is no way to reach a
> configuration in zero time. Of course, if the first waypoint is the
> current configuration it can be reached (it already has). The details
> on how the joint_trajectory_controller handles trajectory replacement
> are documented

> here<http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement> .

>
> This point deserves some discussion in its own right. Is it really
> necessary to add the current configuration to the plan?

Hey Martin,
 

It definitely makes sense to add the current configuration to the plan.
I wrote a joint trajectory action server for the Katana arm; I have to
use the (dumb) controller boards on the arm, so I can't do clever
trajectory replacement. All I can do is check whether the current state
is identical to the first trajectory point; if yes, fine, otherwise
reject.

I understand your usecase. Still, I'm not very fond of this option, as it involves specifying additional ad-hoc tolerances. A trajectory_msgs::JointTrajectory waypoint can be specified up to the acceleration level, so that would mean having to potentially tolerance-check positions, velocities and accelerations. Furthermore, it's not customary to have joint acceleration measurements readily available. One could circumvent this issue by not using actual joint measurements but sampling the active trajectory (which yields accelerations), but I'd not trust that the robot will in general be where it should. AFAIK MoveIt! does not specify funky initial trajectory points with nonzero velocities and accelerations, but the joint_trajectory_controller should be general enough to support well these scenarios.

In fact, if supporting zero-time waypoints is really a desired feature, I'd prefer to just accept the waypoint and let the robot's low-level joint limit enforcing mitigate the jump, if any. I'm open to this one.

Two simple workarounds (not fixes) to this issue that are straightforward to implement would be to either lower the severity of the warning that notifies about discarded waypoints to DEBUG, or offsetting motion plans a bit to the future, so the first waypoint does not start at exactly zero (IIRC, arm_navigation did just this). I don't like either, but I put them here for the record.

Also, the first trajectory point contains essential information
about the velocities and speeds. Perhaps the ros_control implementation
always recalculates the first trajectory point, but that's a special
case.

If the first waypoint of a new command has a non-zero time_from_start, a bridge spline segment is created between the current trajectory and the new one. Since both the current and new trajectories are known, they can be sampled at any time.

Cheers,

Adolfo.

> , and if so,
> could it be timestamped in the future?. On the controller side, would
> it make sense to change the policy and accept waypoints happening
> 'now'?.

All very good questions. There should be a clear specification of the
JointTrajectory action that clarifies these points.

As a short-term workaround it may make sense if ros_control would check
if the unreachable trajectory point is reasonably close in time and
positions to the current state, and only output the warning if not.
Alternatively, it could print the time and joint angle differences in
the error message so the user could check that it's a false alarm.

Cheers,
Martin

[Adolfo Rodríguez Tsouroukdissian]

Hi Martin,

You only replied to me. Putting the list back in the loop...

On Tue, Apr 29, 2014 at 3:08 PM, Martin Günther <martin.gu...@gmail.com> wrote:

Hi Adolfo,

On Mon, 28 Apr 2014 18:54:52 +0200

Adolfo Rodríguez Tsouroukdissian <adolfo.r...@pal-robotics.com>
wrote:

> Hey Martin,
>
>
> > It definitely makes sense to add the current configuration to the
> > plan. I wrote a joint trajectory action server for the Katana arm;
> > I have to use the (dumb) controller boards on the arm, so I can't
> > do clever trajectory replacement. All I can do is check whether the
> > current state is identical to the first trajectory point; if yes,
> > fine, otherwise reject.
>
>
> I understand your usecase. Still, I'm not very fond of this option,
> as it involves specifying additional ad-hoc tolerances. A
> trajectory_msgs::JointTrajectory waypoint can be specified up to the
> acceleration level, so that would mean having to potentially
> tolerance-check positions, velocities and accelerations. Furthermore,
> it's not customary to have joint acceleration measurements readily
> available. One could circumvent this issue by not using actual joint
> measurements but sampling the active trajectory (which yields
> accelerations), but I'd not trust that the robot will in general be
> where it should. AFAIK MoveIt! does not specify funky initial
> trajectory points with nonzero velocities and accelerations, but the
> joint_trajectory_controller should be general enough to support well
> these scenarios.

Yes, I think what the joint_trajectory_controller does is correct:
Print out a warning when it's asked to execute an impossible trajectory
and had to drop trajectory points. The question is what trajectories
should MoveIt generate so that they can be properly executed (see
below).

All I was saying is that MoveIt shouldn't drop the initial point from
the trajectory. Sure, we can always recover it from the current joint
states and recompute velocities and accelerations, but that's going to
make dumber implementations of joint trajectory action servers a bit
more complex, and also doesn't make the joint trajectory message easier
to understand.

Ack. I understand the first waypoint can provide useful information for some controller implementations, and in fact already is.
 

> In fact, if supporting zero-time waypoints is really a desired
> feature, I'd prefer to just accept the waypoint and let the robot's
> low-level joint limit enforcing mitigate the jump, if any. I'm open
> to this one.

Unless time_from_start = 0.0 has some special semantics (like "as soon
as possible"), which I don't think it has,

It does not.

trajectories with zero-time
waypoints are not strictly speaking executable, so I don't think that's
a desired feature.

I agree with you. Thus far this has been my opinion, and it's how the joint_trajectory_controller is currently implemented.

> Two simple workarounds (not fixes) to this issue that are
> straightforward to implement would be to either lower the severity of
> the warning that notifies about discarded waypoints to DEBUG, or
> offsetting motion plans a bit to the future, so the first waypoint
> does not start at exactly zero (IIRC, arm_navigation did just this).
> I don't like either, but I put them here for the record.

I'm not fond either about lowering the severity to DEBUG, since dropped
trajectory points *could* point to a severe problem. So we should
instead make sure that MoveIt sends trajectories that ros_control is
happy with, so that ros_control only prints this warning when there
actually is a problem.

To me, it sounds like offsetting motion plans a bit to the future is
exactly what we should do. Why don't you like that method?

What I don't like(d) is that it implies adding a new ad-hoc value to MoveIt! (but read on)

The problem is that a zero-time initial waypoint is impossible to
reach, except when it coincides exactly with the current state, right?

Yes.
 

And they basically never coincide exactly, due to slight fluctuations
in the joint states when the controller is trying to "hold" the arm.

So, in the most common use case, the arm is still stopped in *almost*
the same position as when we started planning. So if we shifted the
time_from_start of the first trajectory point from 0.0 to 0.01, it
should be reachable. So, if MoveIt would generate plans like this,
ros_control would be happy, right? Sounds like it could easily be done
with a PlanningRequestAdapter.

The PlanningRequestAdapter idea sounds really good. It would be a clean way to introduce this new ad-hoc value. IMO it's the best compromise so far.

Adolfo.

> Also, the first trajectory point contains essential information
> > about the velocities and speeds. Perhaps the ros_control
> > implementation always recalculates the first trajectory point, but
> > that's a special case.
> >
>
> If the first waypoint of a new command has a non-zero
> time_from_start, a bridge spline segment is created between the
> current trajectory and the new one. Since both the current and new
> trajectories are known, they can be sampled at any time.

I know. So the solution seems to be to make MoveIt issue non-zero
time_from_start.

Cheers,
Martin

[waldez junior]

Hi Adolfo,

I didnt have time to check on your answer until now, but now that I read it thank you very much for the clarifications, things make a WHOLE lot more sense now =) .

So I used set my goal tolerances in the .yaml file like this:

  arm_controller:

    type: effort_controllers/JointTrajectoryController

    joints:

     - shoulder_base

     - shoulder_pitch

     - shoulder_yaw

     - elbow_pitch

     - wrist_roll

     - wrist_yaw

     - wrist_pitch

    constraints:

      shoulder_base:

        goal: 0.1

      shoulder_pitch:

        goal: 0.1

      shoulder_yaw:

        goal: 0.1

      elbow_pitch:

        goal: 0.1

      wrist_roll:

        goal: 0.1

      wrist_yaw:

        goal: 0.1

      wrist_pitch:

       goal: 0.1

I had already read the documentation on the tolerances before, but I totally overlooked the information about the goal_time tolerance . After I set it , the controllers outputs more often than not successful trajectories completion.

Again, thank you very much, and best regards,

Waldez Jr