adding absolute jump threshold capability in parallel #821

mlautman · 2018-04-05T00:42:42Z

Description

The git history in my pr #798 got very messy with multiple upstream merges followed by local commits. I cleaned up the git history so I could re-open the PR. I'm not sure if that was the best way and am open to feedback on better ways to cleanup my PR's in the future.

I took a stab at implementing a absolute jump threshold for the computeCartesianPath functionality. (#773) This implementation required separate jump thresholds for prismatic and revolute joints and does not support planar, floating, fixed or multi-DOF joints. This implementation does not affect or replace the old computeCartesianPath method which is used by the move_group computeCartesianPath capability since that would involve changing messages and would have a much larger scope.

The goal of this PR is to address the issue where using the existing jump_threshold method, paths with few waypoints can have huge jumps in joint space. To demonstrate this problem, I generated hundreds of thousands of trajectories with varying goals, eef_steps and jump_thresholds, filtered out anything with less than 100% completion, sorted them by the number of interpolated waypoints in the returned trajectory, and then searched through for the largest absolute jump.

eef_steps = [0.01, 0.05, 0.1]
jump_thresholds = [1.1, 1.5, 1.75, 2.0]

After testing this new implementation and comparing it to the existing computeCartesianPath, I was able to filter out jumps from 100,000 trajectories with minimal parameter tuning. For what it's worth, IMHO it is a lot easier to tune the absolute jump thresholds than the jump threshold factor.

Checklist

Required: Code is auto formatted using clang-format
Extended the tutorials / documentation, if necessary reference
Include a screenshot if changing a GUI
Optional: Created tests, which fail without this PR reference
Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)

rhaschke · 2018-04-05T10:59:29Z

moveit_core/robot_state/src/robot_state.cpp

  for (std::size_t i = 0; i < dist_vector.size(); ++i)
    if (dist_vector[i] > thres)
    {
      CONSOLE_BRIDGE_logDebug("Truncating Cartesian path due to detected jump in joint-space distance");
-      percentage = (double)i / (double)dist_vector.size();
-      traj.resize(i);
+      percentage = (double)(i + 1) / (double)(traj.size());


Looks like you identified another bug. Well done!

rhaschke

I had various style improvements and thus pushed them directly. In general I approve this.
However, I suggest to not throw an exception from within testJointSpaceJump(). What do you think, @mlautman and @davetcoleman?

rhaschke · 2018-04-05T11:00:31Z

moveit_core/robot_state/src/robot_state.cpp

+  else if (jt.prismatic_jump_threshold > 0.0 || jt.revolute_jump_threshold > 0.0)
+    CONSOLE_BRIDGE_logWarn("Attempting to test for absolute joint-space jumps, but zero threshold was provided");
+
+  return 1.0;
 }

 double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,


What about renaming these worker functions to testJointSpaceJumpRelative and testJointSpaceJumpAbsolute?

I think that's a good idea

rhaschke · 2018-04-05T11:02:52Z

moveit_core/robot_state/test/robot_state_test.cpp

+  EXPECT_EQ(traj.size(), 3);  // traj should be cut
+  EXPECT_NEAR(fraction, 3. / 4., 0.01);
+}
+


What about adding tests that ensure that testJointSpaceJump correctly relays to testJointSpaceJumpRelative and testJointSpaceJumpAbsolute?

I'll add that

rhaschke · 2018-04-05T11:07:01Z

moveit_core/robot_state/src/robot_state.cpp

-      traj.insert(traj.end(), start, waypoint_traj.end());
+      if (!joint->getType() == JointModel::PRISMATIC && !joint->getType() == JointModel::REVOLUTE)
+      {
+        CONSOLE_BRIDGE_logError("Unsupported joint type %zu in JointModelGroup %s testJointSpaceJump can only support "


Throwing an exception here, seems rather hard. I think it should be perfectly valid to test a trajectory involving multi-dof joints and just ignore them. Probably there should be a warning once to emphasize the fact that we ignore them.

I think Cartesian planning with multi-dof joints is an unusual circumstance that has not yet been addressed in moveit and probably should be in the future. However allowing it to return a solution that is wrong is very misleading and perhaps even potentially dangerous. I don't see why this is perfectly valid. Please give an example of why you would allow a multi-dof joint to not be checked for jumps.

The joint-space jump test is only supported for revolute or prismatic joints. If the trajectory involves multi-dof joints, why shouldn't it be possible to test all the remaining, normal joints?
I'm not proposing to silently ignore multi-dof joints.

My point is that multi-dof joints should also be checked for jumps for potential safety reasons. Until we support this, we should disallow someone to be mislead into thinking it has been checked by throwing an error.

Another option would be to just implement this now, but I'm not convinced this use-case is very common or necessary and it would delay getting "MVP" features out.

I don't think that testing for joint-space jumps for Cartesian-pose-based joints is necessary at all. If we do Cartesian interpolation for those poses with a given max_step, they will have the given max_step distance anyway...

rhaschke · 2018-04-05T11:08:56Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

+                              const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
+                              const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
+  {
+    return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance, max_step,


I moved the implementation of these functions into the header, as this clearly states that they simply relay to the actual worker function. Having nice constructors for the the JumpThreshold struct facilitates readability.

This is great

rhaschke · 2018-04-05T11:12:12Z

moveit_core/robot_state/src/robot_state.cpp

@@ -2007,6 +2007,13 @@ double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<
 double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
                                      double jump_threshold_factor)
 {
+  if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH)
+  {
+    CONSOLE_BRIDGE_logWarn("The computed trajectory is too short to detect jumps in joint-space "


This warning should stay here, shouldn't it?

rhaschke · 2018-04-05T11:14:06Z

moveit_core/robot_state/src/robot_state.cpp

-  if (jump_threshold.prismatic_jump_threshold > 0.0 || jump_threshold.revolute_jump_threshold > 0.0)
-    percentage *= testJointSpaceJump(group, traj, jump_threshold.revolute_jump_threshold,
-                                     jump_threshold.prismatic_jump_threshold);
+  else if (jt.prismatic_jump_threshold > 0.0 && jt.revolute_jump_threshold > 0.0)


Switched to else if here. For me, it doesn't make sense to test for both, relative and absolute jumps.

I generally agree

rhaschke · 2018-04-05T11:15:15Z

moveit_core/robot_state/src/robot_state.cpp


-  return percentage;
+  else if (jt.prismatic_jump_threshold > 0.0 || jt.revolute_jump_threshold > 0.0)


For me it doesn't make sense to test only revolute or only prismatic joints. Thus, both parameters need to be positive.

I'm not sure if I agree with this. If I have a robot with only revolute joints, I don't want to have to pass in a made up prismatic_jump_threshold and vice versa

I think one should provide reasonable thresholds for both cases. If you are very sure that you have no joints of either type, you can still pass any value > 0.

I still don't like the idea of asking the user to make up prismatic joint cutoff values for their fully revolute robot. I just pushed a commit that deals with this issue in a way that I think is clean.

I also think that else if should be if to cover the use case when someone wants to be super safe and run both checks

I still don't like the idea of asking the user to make up prismatic joint cutoff values for their fully revolute robot.

I guess we keep to have different opinions on that. What about the following: The thresholds are kept zero by default. If there is a joint of "wrong" type in the trajectory (although not expected), the zero threshold will reject the trajectory if this joint moves at all.
Then the absolute testing can be triggered if (jt.prismatic > 0 || jt.revolute > 0)

I just pushed a commit that deals with this issue in a way that I think is clean.

In my point of view, your last commit 8566096 is only bloating the code without any benefit. Please revert this.

I also think that else if should be if to cover the use case when someone wants to be super safe and run both checks.

If you like. Maybe that's the expected behaviour when all params are set. However, you should update the comments to match this behaviour. I actually adapted the behaviour to the behaviour originally promised in the comments...

rhaschke · 2018-04-05T11:15:55Z

moveit_core/robot_state/src/robot_state.cpp

+  else if (jt.prismatic_jump_threshold > 0.0 || jt.revolute_jump_threshold > 0.0)
+    CONSOLE_BRIDGE_logWarn("Attempting to test for absolute joint-space jumps, but zero threshold was provided");
+
+  return 1.0;


Probably, there should be a warning about illegal ( < 0) parameters here.

mlautman · 2018-04-05T15:17:18Z

moveit_core/robot_state/src/robot_state.cpp

  unsigned int steps = floor(distance / max_step) + 1;
-  if (test_joint_space_jump && steps < MIN_STEPS_FOR_JUMP_THRESH)
+  if (jump_threshold.jump_threshold_factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH)


Good catch!

mlautman · 2018-04-05T15:18:48Z

moveit_core/robot_state/src/robot_state.cpp

-  if (jump_threshold.prismatic_jump_threshold > 0.0 || jump_threshold.revolute_jump_threshold > 0.0)
-    percentage *= testJointSpaceJump(group, traj, jump_threshold.revolute_jump_threshold,
-                                     jump_threshold.prismatic_jump_threshold);
+  else if (jt.prismatic_jump_threshold > 0.0 && jt.revolute_jump_threshold > 0.0)


I generally agree

mlautman · 2018-04-05T15:20:34Z

moveit_core/robot_state/src/robot_state.cpp


-  return percentage;
+  else if (jt.prismatic_jump_threshold > 0.0 || jt.revolute_jump_threshold > 0.0)


I'm not sure if I agree with this. If I have a robot with only revolute joints, I don't want to have to pass in a made up prismatic_jump_threshold and vice versa

mlautman · 2018-04-05T15:21:32Z

moveit_core/robot_state/src/robot_state.cpp

+  else if (jt.prismatic_jump_threshold > 0.0 || jt.revolute_jump_threshold > 0.0)
+    CONSOLE_BRIDGE_logWarn("Attempting to test for absolute joint-space jumps, but zero threshold was provided");
+
+  return 1.0;
 }

 double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,


I think that's a good idea

mlautman · 2018-04-05T15:21:50Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

+  {
+  }
+
+  explicit JumpThreshold(double factor) : JumpThreshold()


mlautman · 2018-04-05T15:22:05Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -1087,9 +1102,13 @@ as the new values that correspond to the group */

  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
                              const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
-                              double max_step, double jump_threshold,
+                              double max_step, double jump_threshold_factor,


mlautman · 2018-04-05T15:22:35Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

+                              const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
+                              const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
+  {
+    return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance, max_step,


This is great

mlautman · 2018-04-05T15:25:28Z

moveit_core/robot_state/src/robot_state.cpp

@@ -2007,6 +2007,13 @@ double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<
 double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
                                      double jump_threshold_factor)
 {
+  if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH)
+  {
+    CONSOLE_BRIDGE_logWarn("The computed trajectory is too short to detect jumps in joint-space "


mlautman · 2018-04-05T15:29:05Z

moveit_core/robot_state/test/robot_state_test.cpp

+  EXPECT_EQ(traj.size(), 3);  // traj should be cut
+  EXPECT_NEAR(fraction, 3. / 4., 0.01);
+}
+


I'll add that

v4hn · 2018-04-05T18:19:14Z

I agree with @rhaschke on this one. Just print a warning and skip joints we don't want to handle at the moment.

rhaschke

I like the source code changes.
However, I'm puzzled about the test changes.

rhaschke · 2018-04-05T19:34:20Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -70,23 +70,23 @@ typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joi
    thresholds for detecting joint space jumps. */
 struct JumpThreshold


+1 to the name cleanup

rhaschke · 2018-04-05T19:35:34Z

moveit_core/robot_state/src/robot_state.cpp

-                                "prismatic and revolute joints.",
-                                joint->getType(), group->getName().c_str());
-        throw Exception("Unsupported joint type");
+        CONSOLE_BRIDGE_logWarn("Unsupported joint type %s in JointModelGroup %s testAbsoluteJointSpaceJump can only "


Please split the message into two separate sentences. Maybe even separated by a newline char \n.

rhaschke · 2018-04-05T19:37:29Z

moveit_core/robot_state/src/robot_state.cpp

      }

      double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint);
      if ((joint->getType() == JointModel::PRISMATIC && distance > prismatic_jump_threshold) ||
          (joint->getType() == JointModel::REVOLUTE && distance > revolute_jump_threshold))
      {
-        CONSOLE_BRIDGE_logDebug("Truncating Cartesian path due to detected jump of %.4f > %.4f in joint-space distance",
+        CONSOLE_BRIDGE_logError("Truncating Cartesian path due to detected jump of %.4f > %.4f in joint-space distance",


Also name the joint:
"Cartesian path due to jump of %.4f > %.4f in joint %s"

rhaschke · 2018-04-05T19:38:51Z

moveit_core/robot_state/test/robot_state_test.cpp

-void generateTestTraj(std::vector<std::shared_ptr<robot_state::RobotState>>& traj,
-                      const moveit::core::RobotModelConstPtr& robot_model,
-                      const robot_model::JointModelGroup* joint_model_group)
+std::size_t generateTestTraj(std::vector<std::shared_ptr<robot_state::RobotState>>& traj,


I don't understand why you made these tests so much more complicated. This doesn't add any functionality.
Please revert this part or explain.

@rhaschke I did a few things here. I changed the tests to use the testJointSpaceJump with the struct parameter rather than calling the absolute and relative tests directly. To make it clear which test is being used by testJointSpaceJump under the hood I changed the test trajectory so that the absolute jump threshold and relative jump threshold trip at different waypoints. I also now use the PR2's default values as a base instead of a vector of all zeros since the zero vector is out of the joint limits for the PR2. This isn't that important for the absolute and relative jump tests but it becomes important for the cartesian tests that I am going to open a PR for after this get merged.

I noticed these changes. But I think they are not necessary to perform the testing. Particularly adding many more waypoints make the whole test less comprehensible (for a short glance). Having exactly a single jump is much clearer.

I reverted the test to 4 waypoints with a single obvious jump at the end

…st coverage

… removing throw error from abs jump test

…alues for prismatic and revolute cutoffs

rhaschke · 2018-04-07T01:38:31Z

@mlautman, as I still didn't like some of your recent changes, I assembled an alternative proposal.
Please have a look at this using the following git commands:

git fetch https://github.com/ubi-agni/moveit.git jump-threshold
git checkout -b rhaschke-abs-jt FETCH_HEAD

This should fetch my changes and checkout locally as branch rhaschke-abs-jt.
If you like them, you can reset your branch to the alternative and push as follows:

git checkout kinetic-abs-jt
git reset --hard rhaschke-abs-jt
git push --force <your picknick origin>

rhaschke · 2018-04-16T20:33:35Z

Closing this in favour of #843.

mlautman · 2018-04-16T20:36:08Z

@rhaschke why did you close this? I don't think we had resolved whether the changes you wanted to make are an improvement

mlautman · 2018-04-16T20:49:33Z

I finally had a chance to look through your changes a minute ago. I like the changes to robot_state but I think the tests are better in this one.

rhaschke · 2018-04-16T21:22:12Z

Why did you close this?

@mlautman, as I didn't get any feedback from you for a while, I thought to close this in favour of #843, which is a PR comprising my changes applied to your original branch. This way, I was hoping to facilitate reviewing my alternative branch. As we shouldn't have two similar PRs open at a time, I closed this one.

mlautman · 2018-05-17T17:33:52Z

@rhaschke In the future, do not do that again. You could have ping'd me, you could have emailed me but instead you decided to go ahead and overwrite my changes with yours which I found to be pretty disrespectful

rhaschke reviewed Apr 5, 2018

View reviewed changes

rhaschke requested changes Apr 5, 2018

View reviewed changes

rhaschke reviewed Apr 5, 2018

View reviewed changes

mlautman commented Apr 5, 2018

View reviewed changes

davetcoleman approved these changes Apr 5, 2018

View reviewed changes

rhaschke requested changes Apr 5, 2018

View reviewed changes

mlautman and others added 11 commits April 5, 2018 14:05

adding absolute jump threshold capability in parallel, also adding te…

729cc3d

…st coverage

formatting of comments, reduce redundancy

33aa3ec

inline definition of convienency methods

23ac30a

improve readability: always call testJointSpaceJump()

9226726

reduce redundancy, improve efficiency

4fe556c

restore old code were applicable

876da2b

cleanup tests

97d7b59

changing struct variable names to be less redundant, improving tests,…

12457ad

… removing throw error from abs jump test

adding bool flags to JumpThreshold to reduce confusion over default v…

8566096

…alues for prismatic and revolute cutoffs

robot_state improving debug info

aa8dcc3

simplifying robot_state_test

20dc432

v4hn mentioned this pull request Apr 10, 2018

computeCartesianPath: only test for jump if there's something to check #831

Closed

rhaschke mentioned this pull request Apr 16, 2018

improved checking for joint-space jumps of Cartesian path #843

Merged

rhaschke closed this Apr 16, 2018

mlautman mentioned this pull request Apr 17, 2018

Testing for Cartesian space Jumps in Cartesian paths #844

Closed

5 tasks

mlautman mentioned this pull request Apr 24, 2018

adding absolute jump filtering moveit/moveit_msgs#44

Closed

davetcoleman deleted the kinetic-abs-jt branch June 8, 2018 00:30

s-trinh mentioned this pull request Nov 9, 2020

ComputeCartesianPath related improvements #797

Closed

7 tasks

Akumar201 mentioned this pull request Jan 12, 2024

How to tune computeCartesianWaypoints jump_threshold to avoid jerk #3550

Open


		return percentage;
		else if (jt.prismatic_jump_threshold > 0.0 \|\| jt.revolute_jump_threshold > 0.0)

		@@ -70,23 +70,23 @@ typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joi
		thresholds for detecting joint space jumps. */
		struct JumpThreshold

adding absolute jump threshold capability in parallel #821

adding absolute jump threshold capability in parallel #821

Conversation

mlautman commented Apr 5, 2018 • edited Loading

Description

Checklist

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke left a 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

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

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

mlautman Apr 5, 2018 • edited Loading

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

Choose a reason for hiding this comment

mlautman Apr 5, 2018 • edited Loading

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

Choose a reason for hiding this comment

v4hn commented Apr 5, 2018 via email

rhaschke left a 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

mlautman Apr 6, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke commented Apr 7, 2018

rhaschke commented Apr 16, 2018

mlautman commented Apr 16, 2018

mlautman commented Apr 16, 2018

rhaschke commented Apr 16, 2018

mlautman commented May 17, 2018

mlautman commented Apr 5, 2018 •

edited

Loading

mlautman Apr 5, 2018 •

edited

Loading

mlautman Apr 5, 2018 •

edited

Loading

mlautman Apr 6, 2018 •

edited

Loading