Hold joint position if tolerances were violated #395

Martin-Oehler · 2018-12-21T10:44:14Z

Currently, the joint_trajectory_controller keeps executing a trajectory even after the goal was aborted due to a tolerance violation. In my opinion, this is a bad idea for multiple reasons:

It is unsafe to keep moving, since the robot deviated from the path. The chance of a collision is high.
The current behavior is unexpected.
Even if you wanted to, you could not stop the execution anymore, because an aborted goal can't be preempted again.

This behavior was mentioned in issue #48, but never fixed.

This PR addresses this issue by calling setHoldPosition() before aborting the goal.

I think, the current tests should be sufficient. However, I was unable to execute them since I got a lot of unrelated errors.

mathias-luedtke

Thanks for your initiative!
This is the third variantion of the same fixes.

Your code passes rt_active_goal_ and therefore might prevent these problems of other fixes.
However, I am not 100% sure of the implications. It looks like that this pointer is only used for the tolerance checks.

The current, but broken behavior was around for a long time, so I would suggest to not merge into kinetic unless we have a good reason for it.

I think, the current tests should be sufficient.

No, the tests have to be extended to check that the robot really stopped.
The test suite should fail before and pass after applying the patch.

mathias-luedtke · 2018-12-21T12:44:49Z

However, I was unable to execute them since I got a lot of unrelated errors.

Please elaborate, this should not happen.
The joint_trajectory_controller are a little bit unstable, but it is still possible to execute them.

Martin-Oehler · 2018-12-21T13:47:40Z

Your code passes rt_active_goal_ and therefore might prevent these problems of other fixes.
However, I am not 100% sure of the implications. It looks like that this pointer is only used for the tolerance checks.

I can't really comment on that. I did not read too much into the code.

The current, but broken behavior was around for a long time, so I would suggest to not merge into kinetic unless we have a good reason for it.

Which release do you think I should target? Currently, I have only a running kinetic install.

No, the tests have to be extended to check that the robot really stopped.
The test suite should fail before and pass after applying the patch.

Do you have input for that? In the current formulation of the tests, it is pretty much unknown, when and where the path tolerance violation happens. Is a test, if the joints are moving at all, sufficient?

Please elaborate, this should not happen.

I am executing

$ catkin build joint_trajectory_controller --catkin-make-args tests
$ rostest joint_trajectory_controller joint_trajectory_controller.test

and receive

[INFO] [1545399417.555784] [/controller_spawner]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1545399417.557350] [/controller_spawner]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1545399417.558517] [/controller_spawner]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1545399417.559680] [/controller_spawner]: Loading controller: rrbot_controller
[INFO] [1545399417.590825] [/controller_spawner]: Controller Spawner: Loaded controllers: rrbot_controller
[INFO] [1545399417.600804] [/controller_spawner]: Started controllers: rrbot_controller
[ERROR] [1545399418.488635752] [/rrbot]: Joints on incoming goal don't match the controller joints.
[ERROR] [1545399418.498615822] [/rrbot]: Joints on incoming goal don't match the controller joints.
[ERROR] [1545399418.509027322] [/rrbot]: Size mismatch in trajectory point position, velocity or acceleration data.
[ERROR] [1545399418.519044854] [/rrbot]: Size mismatch in trajectory point position, velocity or acceleration data.
[ERROR] [1545399418.529086941] [/rrbot]: Trajectory message contains waypoints that are not strictly increasing in time.
[ERROR] [1545399418.539248713] [/rrbot]: Joints on incoming goal don't match the controller joints.
[ERROR] [1545399482.330258427] [/rrbot]: Unexpected exception caught when initializing trajectory from ROS message data.
[INFO] [1545399495.982264] [/controller_spawner]: Shutting down spawner. Stopping and unloading controllers...
[INFO] [1545399495.982821] [/controller_spawner]: Stopping all controllers...
[WARN] [1545399496.061524] [/controller_spawner]: Controller Spawner error while taking down controllers: unable to connect to service: [Errno 104] Connection reset by peer

However, I just noticed that after a few minutes, I still get

SUMMARY
 * RESULT: SUCCESS
 * TESTS: 25
 * ERRORS: 0
 * FAILURES: 0

…ter goal abortion

Martin-Oehler · 2019-01-11T15:49:37Z

No, the tests have to be extended to check that the robot really stopped.
The test suite should fail before and pass after applying the patch.

I added the respective test cases.

Martin-Oehler · 2019-01-24T11:32:38Z

Can I have an update on this? Are more changes requested?

mathias-luedtke · 2019-01-26T12:25:30Z

Thanks for your patches and your ping :)

I added the respective test cases.

It fails on travis, I think it needs some updates (see other comments).

Which release do you think I should target? Currently, I have only a running kinetic install.

I would target melodic (our current development branch).
We can think about a back-port though.

mathias-luedtke · 2019-01-26T12:32:42Z

joint_trajectory_controller/test/joint_trajectory_controller_test.cpp

+  StateConstPtr state2 =  getState();
+  for (unsigned int i = 0; i < n_joints; ++i)
+  {
+    EXPECT_NEAR(state1->desired.positions[i],     state2->desired.positions[i],     EPS);


According to the failure log, the velocity in state1 is non-zero, but in state2 it is zero.
This shows that your path is working!

I would make the test check state1 velocity and acceleration to be non-zero (=moving) and the ones in state2 should be zero. In addition the position change between state1 and state2 should be in the range of the largest possible step in the hold-trajectory.

There are a few other test cases that check for movement, for example here. I made my check similar for consistency.

These tests don't have the delay enabled and a big delay before acquiring the states.

Okay, I will update the test with your proposal.

I would make the test check state1 velocity and acceleration to be non-zero (=moving) and the ones in state2 should be zero.

state1 velocity and acceleration are only non-zero, if stop_trajectory_duration != 0. I guess, I have to test for multiple cases here.

In addition the position change between state1 and state2 should be in the range of the largest possible step in the hold-trajectory.

Do you know how to find this value? The actual implementation samples a trajectory.

I would make the test check state1 velocity and acceleration to be non-zero (=moving) and the ones in state2 should be zero. In addition the position change between state1 and state2 should be in the range of the largest possible step in the hold-trajectory.

Sounds good. I think we should strive for an assertion that reliably checks, if a hold trajectory is executed as expected. This could then be used in all relevant test cases. However, I would separate it from this PR.

mathias-luedtke · 2019-01-26T12:34:17Z

joint_trajectory_controller/test/joint_trajectory_controller_test.cpp

@@ -1244,6 +1244,17 @@ TEST_F(JointTrajectoryControllerTest, pathToleranceViolation)
  EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout));
  EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED);

+  // Check that we're not moving
+  StateConstPtr state1 = getState();
+  ros::Duration(0.5).sleep(); // Wait


This might be a little bit too high for the hold trajectory to be effective, but with the position plausibility checks this does not matter (apart from longer test time..)

I made this consistent with the other tests. See my other reply.

As long as system time is used, it is difficult to check that the robot stopped in time.
Because there can be delay in the state-publisher.

mathias-luedtke · 2019-01-26T12:49:38Z

joint_trajectory_controller/test/joint_trajectory_controller_test.cpp

@@ -1266,7 +1277,7 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation)
  // Make robot respond with a delay
  {
    std_msgs::Float64 smoothing;
-    smoothing.data = 0.95;
+    smoothing.data = 0.98;


Why did you chnage this?

If I do not make the movement slower, the joints are too close to the goal position and the position check always passes.

This makes the movement faster.

A smoothing value of 0 corresponds to direct control whereas a value of 1 makes the robot not respond at all. Therefore, we movement is delayed more. "Slower" was the wrong term, I guess.

Ah, you are right, my bad!

mathias-luedtke · 2019-01-26T12:51:44Z

joint_trajectory_controller/test/joint_trajectory_controller_test.cpp

+  StateConstPtr state2 =  getState();
+  for (unsigned int i = 0; i < n_joints; ++i)
+  {
+    EXPECT_NEAR(state1->actual.positions[i],     state2->actual.positions[i],     EPS);


You check it the same way as in the other test.
To rule out other effect, just use the desired values as well.

In this case, taking the desired values is not possible. The trajectory has already finished when the goal tolerance is checked. Therefore, desired values do not change anymore.

This test should check the behavior of the controller (=desired value). And according to the test output, this code samples the hold trajectory properly.

The desired position should match the actual position at the time of the goal tolerance violation. I could rewrite the test to check that.

The desired position should match the actual position at the time of the goal tolerance violation

The final position is based on the hold trajectory -> will move a little bit to obey the speed limits etc.

I am not sure, if I understand you correctly. Before my fix, the desired velocity and acceleration of state1 would be 0, because that is the final state of the trajectory. After my fix and with stop_trajectory_duration == 0 vel and acc of state1 would also be zero, because that is what they are set to.

Before your fix, the desired velocity and acceleration of state1 would ne non-zero, because the controller is still moving towards the goal and motion does not stop.
Your fix make it stop.

The tests must command a goal that takes much longer to reach than the hold trajectory (to stop the motion).

I don't think, we are talking about the same problem here. I try to summarize the scenario to clarify the situation:
The robot is moving towards some goal but is delayed and therefore does not follow the trajectory very closely. Now the desired trajectory is finished (desired velocity/acceleration are zero) and goal tolerances are checked. The robot is still moving as it is delayed.
Previous to my PR the robot would keep moving and reach the goal at an unknown time. My PR changes the behavior so the robot stops moving towards the original goal (as moving there would be useless/dangerous now) and holds its position.

I think what should be tested here, is that the robot stays close to its current position after the goal tolerance violation occurred. I agree, that the desired trajectory should be checked, but simply checking velocity/acceleration is not an option as the original behavior also commands a zero velocity/acceleration in its final state, as the trajectory has finished.

The tests must command a goal that takes much longer to reach than the hold trajectory (to stop the motion).

I thought the same. This could be done in the path-tolerance test case.

I suggest to synchronize with #457 in order to avoid doubling the effort.

@Martin-Oehler Are you still interested in continuing on this?

Unfortunately, I am currently occupied by other projects and won't have time to further look into this. Feel free to take over if you are interested.

martiniil · 2020-02-14T07:16:03Z

Hi.

I think, the current tests should be sufficient. However, I was unable to execute them since I got a lot of unrelated errors.

I tried to make some progress here. See #457. Currently, only the tolerance-tests fail. They should pass once this issue is solved. So I would like to comment them out and get the changes into melodic quickly. Then we should uncomment them here.
Also makes it more convenient to add new test cases.
@ipa-mdl What do you think?

Martin-Oehler added 2 commits December 11, 2018 10:56

Holding the current position if path tolerances were violated

f19e7a4

holding position when goal tolerance was violated

b7b23f5

mathias-luedtke requested changes Dec 21, 2018

View reviewed changes

Martin-Oehler mentioned this pull request Jan 9, 2019

Preempt trajectory execution if one controller aborts moveit/moveit#1299

Open

extended test cases for tolerance violations to check for movement af…

3d7f134

…ter goal abortion

mathias-luedtke reviewed Jan 26, 2019

View reviewed changes

martiniil mentioned this pull request Jun 5, 2020

joint_trajectory_controller: unreliable stop trajectory #493

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Hold joint position if tolerances were violated #395

Hold joint position if tolerances were violated #395

Martin-Oehler commented Dec 21, 2018

mathias-luedtke left a comment

mathias-luedtke commented Dec 21, 2018

Martin-Oehler commented Dec 21, 2018

Martin-Oehler commented Jan 11, 2019

Martin-Oehler commented Jan 24, 2019

mathias-luedtke commented Jan 26, 2019

mathias-luedtke Jan 26, 2019

Martin-Oehler Jan 29, 2019

mathias-luedtke Jan 29, 2019

Martin-Oehler Jan 29, 2019

Martin-Oehler Mar 18, 2019

martiniil Feb 14, 2020

mathias-luedtke Jan 26, 2019

Martin-Oehler Jan 29, 2019

martiniil Feb 14, 2020

mathias-luedtke Jan 26, 2019

Martin-Oehler Jan 29, 2019

mathias-luedtke Jan 29, 2019

Martin-Oehler Jan 29, 2019

mathias-luedtke Jan 29, 2019

mathias-luedtke Jan 26, 2019

Martin-Oehler Jan 29, 2019

mathias-luedtke Jan 29, 2019

Martin-Oehler Jan 29, 2019

mathias-luedtke Jan 29, 2019

Martin-Oehler Jan 29, 2019

mathias-luedtke Jan 29, 2019

Martin-Oehler Mar 18, 2019

martiniil Feb 14, 2020

Martin-Oehler Mar 19, 2020

martiniil commented Feb 14, 2020

Hold joint position if tolerances were violated #395

Are you sure you want to change the base?

Hold joint position if tolerances were violated #395

Conversation

Martin-Oehler commented Dec 21, 2018

mathias-luedtke left a comment

Choose a reason for hiding this comment

mathias-luedtke commented Dec 21, 2018

Martin-Oehler commented Dec 21, 2018

Martin-Oehler commented Jan 11, 2019

Martin-Oehler commented Jan 24, 2019

mathias-luedtke commented Jan 26, 2019

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

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

martiniil commented Feb 14, 2020