validate trajectory before execution #63

rhaschke · 2016-08-17T13:59:26Z

Extracted trajectory validation from moveit/moveit_ros#716 and moveit/moveit_ros#728. First point of trajectory is validated to match current robot state just before execution.

First commit has code from previous PRs.
Second commit addresses review comments already raised there.
Third commit moves validation into TrajectoryExecutionManager. This requires some API changes, but ensures validation in any case.

This should be cherry-picked into Indigo/Jade finally.

davetcoleman · 2016-08-17T16:27:39Z

...execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h

@@ -288,6 +291,7 @@ class TrajectoryExecutionManager
  bool execution_duration_monitoring_;
  double allowed_execution_duration_scaling_;
  double allowed_goal_duration_margin_;
+  double allowed_start_deviation_;


add comment here explaining units - for revolute joints this would be tolerance of radians amount correct?

davetcoleman · 2016-08-17T16:32:52Z

Looks like a great improvement, thanks

davetcoleman · 2016-08-18T05:56:44Z

+1

rhaschke · 2016-08-18T11:02:10Z

@v4hn Could you have a look too?

v4hn · 2016-08-18T12:21:16Z

On Thu, Aug 18, 2016 at 04:02:10AM -0700, Robert Haschke wrote:

@v4hn Could you have a look too?

I will do that over the weekend.
Sorry I'm over the top stressed at the moment.

rhaschke · 2016-08-18T13:26:03Z

I will do that over the weekend.

Great. Thanks.

v4hn · 2016-08-22T12:06:57Z

moveit_ros/planning/trajectory_execution_manager/cfg/TrajectoryExecutionDynamicReconfigure.cfg

@@ -7,5 +7,6 @@ gen.add("execution_duration_monitoring", bool_t, 1, "Monitor the execution durat
 gen.add("allowed_execution_duration_scaling", double_t, 2, "Accept durations that take a little more time than specified", 1.1, 1, 10)
 gen.add("allowed_goal_duration_margin", double_t, 3, "Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling)", 0.5, 0.1, 5)
 gen.add("execution_velocity_scaling", double_t, 4, "Multiplicative factor for execution speed", 1, 0.1, 10)
+gen.add("allowed_start_tolerance", double_t, 5, "Allowed joint-value tolerance for validation of trajectory's start point against current robot state", 1e-3, 0);


1e-3 feels rather small for a hard threshold.
Everyone can adapt this if required, but I would prefer 0.01 as the default in order to prevent false-positives.
We don't want people to trip over it, we want it to support the user.

1e-3 is exactly the value you proposed previously: moveit/moveit_ros#728 (comment). With a 1m lever this is about 1mm, which is fine, while 1e-2 would be 1cm already.

good point... two of them actually.. I pretty much agree, but before having merging this upstream I would be interested in the usual noise in the reported joint states of the UR5 or one or two other standard platforms. We surely do not want to have problems with this because the threshold includes the noise...

the noise in the reported joint states of the ur5 at rest is around 8e-5.
What about other platforms?

@davetcoleman what do you think about the value of the threshold?

considering 8e-5 is what UR5 has, seems like a good number. let's set it to whatever the noise of baxter is... i doubt any robot could be nosier, lol

@IanTheEngineer

if this is a tricky parameter to tune we should document it for others to tune themselves per robot. i'd recommend a new tutorial named "Tuning Magic Numbers for Real Robots" and list all the parameters in our various yaml and roslaunch files

Having 8e-5 on UR5, the suggested 1e-3 seems to be a good compromise. It's safe with regard to a 1m lever only "jumping" 1mm in that case. I agree to @v4hn that the value can be made more restrictive if necessary. I wouldn't increase further (e.g. to 1e-2 as suggested in https://github.com/ros-planning/moveit/pull/63/files#r75665102), because that would move by 1cm.

can you add a comment in the code re-capping how we've arrived at this default value?

Looking at this again, I believe it is unnecessarily complex to have this parameter in dynamic reconfigure.

it's one more place where the default value has to be provided

if anyone wants to add per-joint thresholds later on, the meaning of this parameter becomes "default if none is specified"
This meaning does not deserved to be changed dynamically imho

Personally I don't see a reason why anyone would ever change that value during runtime. This is what dynamic reconfigure is for.

Opinions?

I think I would lean toward leaving this out of dyn reconfig. Your bullets sum up the arguments I would've used as well.

Beyond this one PR though, I can see making things like this dynamically (re)configurable being useful when requirements change during runtime. Two examples:

I know of people using MoveIt with drones and other more mobile robots. These robots are mobile perhaps even just to add some 'temporary' mobility to an otherwise static manipulator, but still. I would expect -- but cannot back it up with nrs right now -- that noise in joint states would increase during motion. Manipulation during motion would then need to have (a slightly more) relaxed threshold(s). Afterwards, the old setting could be restored.

(slightly more of a stretch, as considering MoveIt as part of the safety features of any robot system is currently not even considerable): in the context of co-robots, changing thresholds / safety related distances and things like maximum velocities and forces based on the proximity of humans becomes more and more common place.

I don't see MoveIt used for these things (in production systems at least), but dynamically changeable parameters would seem to be a requirement for this kind of functionality.

On Tue, Sep 13, 2016 at 04:02:28AM -0700, G.A. vd. Hoorn wrote:

I would expect -- but cannot back it up with nrs right now -- that noise in joint states would increase during motion. Manipulation during motion would then need to have (a slightly more) relaxed threshold(s). Afterwards, the old setting could be restored.

Feel free to correct me on this, but this is not state-of-the-art but cutting-edge research and people doing research in this field, they can always disable this additional safety feature if they exceed this threshold and enforce it in their drivers (this is where it actually belongs).

in the context of co-robots, changing thresholds / safety related distances and things like maximum velocities and forces based on the proximity of humans becomes more and more common place.

Yes, I don't believe the parameter under discussion falls into this category though. At least not for now.

v4hn · 2016-08-22T12:33:11Z

Overall, this looks great.

As mentioned, this breaks API and I'm not sure there is a way around it.
For Indigo, I would prefer to change all MoveIt-internal references to the proposed two-parameter constructor of TrajectoryExecutionManager, but still provide the old constructor.
Thus, validation would be optional if the user creates an instance of that class on their own, but we can add a ROS_ERROR that tells them to switch to the new version instead.

rhaschke · 2016-08-23T08:58:45Z

As mentioned, this breaks API and I'm not sure there is a way around it.
For Indigo, I would prefer to change all MoveIt-internal references to the proposed two-parameter constructor of TrajectoryExecutionManager, but still provide the old constructor.
Thus, validation would be optional if the user creates an instance of that class on their own, but we can add a ROS_ERROR that tells them to switch to the new version instead.

Good idea. I will also add a deprecation warning during compile time then.
@v4hn If you merge this PR, I can take care of cherry-pick to Jade (without changes, right?) and I will prepare a new PR for Indigo, with the deprecation adaptations.

v4hn · 2016-08-23T11:23:20Z

On Tue, Aug 23, 2016 at 01:58:45AM -0700, Robert Haschke wrote:

Good idea. I will also add a deprecation warning during compile time then.

+1

@v4hn If you merge this PR, I can take care of cherry-pick to Jade (without changes, right?) and I will prepare a new PR for Indigo, with the deprecation adaptations.

+1 as soon as the threshold got somewhat tested. (We'll try to test the ur5 here today)

rhaschke · 2016-08-24T09:32:42Z

As mentioned, this breaks API and I'm not sure there is a way around it.

With the changes suggested in #63 (comment), API compatibility can be maintained, but ABI will break anyway, because we need to add a new class member. Alternatively, we could fallback to the version before 9f77f91, which is ABI and API compatible because it adds the validateTrajectory method at the level of MoveGroupContext, which gets called only by the trajectory execution service then.

The plan/move capability enforces setting the start state of the robot, such that it shouldn't happen that the start state deviates there. However, other capability plugins would need to call the validation method themselves if necessary.

I think, this could be a compromise to maintain ABI compatibility and having the trajectory validation in most cases.
On the other hand, as the synchronization-fix will break ABI as well, we could decide to drop it here too.
@v4hn @davetcoleman Opinions?

UPDATE:
The proposed option, would require to call the validation method in the new ExecuteAction capability.

v4hn · 2016-08-24T14:19:20Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+      return false;
+    }
+    // TODO: check multi-DoF joints ?
+    if (fabs(current_state->getJointPositions(jm)[0] - positions[i]) > allowed_start_tolerance_)


Would it be a good idea to skip this check if the allowed tolerance is set to 0.0?
This would disallow setting it to 0.0 if you think you have a perfectly unnoisy robot,
but it would give a well-defined and simple way to disable the safety check from the launch file.

I suppose I would want to do that if I'm sure my controller does this in a better way...

+1 to skipping the for loop if the tolerance is smaller than epsilon

I will do that.

v4hn · 2016-08-24T14:20:08Z

As mentioned, this breaks API and I'm not sure there is a way around it.

You misunderstood. I'm fine with breaking the ABI once in indigo to fix safety hazards.
Assuming you add the old constructor back in indigo (as I mentioned in #63 (comment) ,
my only concern left is the default threshold value. It's (1) a default value that
(2) most people will never touch, so it should allow normal workflow with most setups (1) and still do the job (2).
Pretty hard trade-off, really.

So I was waiting for some more measurements from other robot setups.

gavanderhoorn · 2016-08-24T14:24:13Z

I haven't read the code completely, but have we thought about the impact of this on things like dynamic trajectory replacement?

Are we expecting users to disable the traj.mon. in case they want to do something like that?

v4hn · 2016-08-24T14:26:04Z

By now, I heard the argument twice that this threshold should be specified per joint instead of having one value. In both cases it was suggested to make this an optional new field in the robot_moveit_config/config/joint_limits.yaml.

The most important use-case I see for this are prismatic joints. We might not want to use the same threshold for validation of rotational and prismatic joints, but we can't differentiate them at this point (and I still think this is the correct place to add the checks).

Up to now my reaction to this was: if the simple value proofs inappropriate, we can always put a per-joint mechanism on top of it and make the ros-param the default for unspecified limits.
Nevertheless I wanted to add this here.

v4hn · 2016-08-24T14:44:06Z

@gavanderhoorn Thanks!

@rhaschke why did you add the validation to the configure method? This is called for every pushed trajectory, but if you want to push multiple trajectories, this will fail.
I just had a look into the execution manager code and I believe this should be called in executeThread.

gavanderhoorn · 2016-08-24T14:59:24Z

@v4hn wrote:

@gavanderhoorn Thanks!

hm? For what? :)

v4hn · 2016-08-24T15:29:38Z

On Wed, Aug 24, 2016 at 07:59:24AM -0700, G.A. vd. Hoorn wrote:

hm? For what? :)

Looking at this :-)
And especially pointing out possible bugs :D

gavanderhoorn · 2016-09-13T11:04:50Z

@guihomework wrote:

I also looked at our PA-10 extended the most I could, with the shadow hand at the end.

slightly off-topic: with PA-10, you mean a Mitsubishi PA-10?

v4hn · 2016-09-13T11:23:02Z

According to the noise analyses on different robots, I agree to raise the threshold to 0.01, which corresponds then to 1cm offset with a 1m lever arm. This should be still fine for basic safety.

+1

I would not add a configuration dialog to the assistant until we change this mechanism to one-value-per-joint, because the semantics of this one value are not very intuitive and the mechanism should "just work" by default.

If you want to have a "magic numbers" widget in the assistant, that sounds pretty cool actually, but I would opt to have support for parsing the files in the loaded robot config before that and this is quite some work. Without this, we would just end up with storing all the parameters in the files and in .setup_assistant.

@guihomework Thanks a lot for the test results!

guihomework · 2016-09-13T12:52:45Z

@gavanderhoorn , indeed this is a Mitsubishi PA-10 7c with a too heavy payload (shakes when moving fast) but is stable when maintaining position.

gavanderhoorn · 2016-09-13T13:08:10Z

@guihomework: interesting. I might email you off-list.

@davetcoleman

* validate trajectory before execution * addressed review comments * moved validateTrajectory to TrajectoryExecutionManager * addressed @davetcoleman's comments * make allowed_start_tolerance dynamically configurable * addressed @v4hn's comments * moved validate to executeThread * allow_start_tolerance == 0 disables trajectory validation * moved validate to execute() * add validation test * increased default value for allowed_start_tolerance to 0.01

@davetcoleman

* validate trajectory before execution * addressed review comments * moved validateTrajectory to TrajectoryExecutionManager * addressed @davetcoleman's comments * make allowed_start_tolerance dynamically configurable * addressed @v4hn's comments * moved validate to executeThread * allow_start_tolerance == 0 disables trajectory validation * moved validate to execute() * add validation test * increased default value for allowed_start_tolerance to 0.01

davetcoleman · 2016-09-16T06:59:53Z

great work @rhaschke !

cherry-picked to jade, pull request for indigo

davetcoleman · 2016-09-16T07:43:48Z

I just realized this is not API-compatible - my downstream packages are failing in Jade because of this line

Previously in this thread @rhaschke suggested this could be directly cherry-picked into Jade, which I just did, but now that Jade is "officially" released I think we should discuss this further. And there is a C++11 auto bug that I've fixed locally but has broken the build. I'm going to revert the jade cherry-pick for now.

This reverts commit 36bbf74.

@davetcoleman

* validate trajectory before execution * addressed review comments * moved validateTrajectory to TrajectoryExecutionManager * addressed @davetcoleman's comments * make allowed_start_tolerance dynamically configurable * addressed @v4hn's comments * moved validate to executeThread * allow_start_tolerance == 0 disables trajectory validation * moved validate to execute() * add validation test * increased default value for allowed_start_tolerance to 0.01

rhaschke · 2016-09-16T11:47:36Z

@davetcoleman This should be API-, but not ABI-compatible and we agreed to accept the ABI breakage for more safety. Which compilation issues do you observe in your downstream projects?

v4hn · 2016-09-16T15:13:33Z

it's not API compatible, because you didn't add the old constructor back in the kinetic request.
We agreed to add it only in indigo @rhaschke

davetcoleman · 2016-09-16T19:12:58Z

It broke some of my downstream released packages in Jade because of the change in the constructor, so I think we want it in Jade also

* Port trajectory_processing to ROS 2 * trajectory_processing, fix logging according to moveit/moveit2#21 * moveit_core trajectory processing - library as SHARED * Fixing trajectory_processing tests * moveit_core trajectory_processing - fixing time

* Add service file to control moveit_jog_arm free dimensions * A better-sounding description * Change service name to ChangeDriftDimensions * Reword control_x_translation -> drift_x_translation * Add a transform to the service * Update srv/ChangeDriftDimensions.srv comment Co-Authored-By: Dave Coleman <dave@picknik.ai> Co-authored-by: Dave Coleman <dave@picknik.ai>

davetcoleman reviewed Aug 17, 2016
View reviewed changes

davetcoleman assigned v4hn Aug 18, 2016

rhaschke mentioned this pull request Aug 19, 2016

adaptions to testing code to match ros-planning/moveit_resources#7 #83

Merged

v4hn reviewed Aug 22, 2016
View reviewed changes

v4hn mentioned this pull request Aug 22, 2016

Initial Indigo release from ros-planning/moveit repo #100

Closed

15 tasks

v4hn reviewed Aug 24, 2016
View reviewed changes

v4hn assigned rhaschke Sep 13, 2016

increased default value for allowed_start_tolerance to 0.01

4aeffa6

davetcoleman approved these changes Sep 16, 2016

View reviewed changes

davetcoleman merged commit ce65970 into moveit:kinetic-devel Sep 16, 2016

davetcoleman mentioned this pull request Sep 16, 2016

[indigo] validate trajectory before execution #223

Closed

davetcoleman added a commit that referenced this pull request Sep 16, 2016

Revert "validate trajectory before execution (#63)"

448383c

This reverts commit 36bbf74.

davetcoleman mentioned this pull request Sep 16, 2016

[jade] validate trajectory before execution #225

Merged

davetcoleman added this to the Kinetic Release milestone Sep 16, 2016

davetcoleman mentioned this pull request Sep 20, 2016

MSA: improve author information gui #221

Merged

rhaschke deleted the validate-plan branch September 20, 2016 21:49

rhaschke mentioned this pull request Sep 20, 2016

fix race conditions when updating PlanningScene #232

Closed

v4hn mentioned this pull request Oct 12, 2016

Invalid Trajectory: start point deviates #283

Closed

This was referenced Oct 17, 2016

normalize joints in start-state validation #292

Merged

Disable validate trajectory feature temporarily #287

Closed

IanTheEngineer mentioned this pull request Oct 5, 2017

moveit execution fails on real robot RethinkRobotics/sawyer_moveit#28

Closed

k-okada mentioned this pull request Dec 10, 2017

use waitForCurrentState(ros::Time::now(), instead of waitForCurrentSt… #717

Merged

5 tasks

validate trajectory before execution #63

validate trajectory before execution #63

Conversation

rhaschke commented Aug 17, 2016

Choose a reason for hiding this comment

davetcoleman commented Aug 17, 2016

davetcoleman commented Aug 18, 2016

rhaschke commented Aug 18, 2016

v4hn commented Aug 18, 2016

rhaschke commented Aug 18, 2016

Choose a reason for hiding this comment

Choose a reason for hiding this comment

v4hn Aug 23, 2016 via email

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gavanderhoorn Sep 13, 2016 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

v4hn commented Aug 22, 2016

rhaschke commented Aug 23, 2016

v4hn commented Aug 23, 2016

rhaschke commented Aug 24, 2016 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

v4hn commented Aug 24, 2016

gavanderhoorn commented Aug 24, 2016

v4hn commented Aug 24, 2016

v4hn commented Aug 24, 2016

gavanderhoorn commented Aug 24, 2016

v4hn commented Aug 24, 2016

gavanderhoorn commented Sep 13, 2016

v4hn commented Sep 13, 2016 • edited

guihomework commented Sep 13, 2016

gavanderhoorn commented Sep 13, 2016

davetcoleman commented Sep 16, 2016

davetcoleman commented Sep 16, 2016

rhaschke commented Sep 16, 2016 • edited by v4hn

v4hn commented Sep 16, 2016

davetcoleman commented Sep 16, 2016

gavanderhoorn Sep 13, 2016 •

edited

rhaschke commented Aug 24, 2016 •

edited

v4hn commented Sep 13, 2016 •

edited

rhaschke commented Sep 16, 2016 •

edited by v4hn