TrajectoryController: Use desired state to calculate hold trajectory #297

agutenkunst · 2017-09-11T13:24:48Z

The current state is used for calculating the hold-trajectory, that results in a jump in the desired position. The time delay originating from the canopen communication leads to a desired position that is already in the past.

Below the effect can be clearly seen by comparing the trajectories (plotted from the CAN traces).

It can be clearly seen that the desired trajectory switches the sign of the velocity. This results in a jerk of the respective drive. If the speed is high and/or the stop_time is reduced this effect increases.
I would also argue that this behavior causes mechanical stress in the robot.

Any opinions?

Noticed with Schunk LWA4p

bmagyar · 2017-09-26T12:20:02Z

By looking at the plot and some experience with robots using this I think you are onto something.

We do have however a policy to add tests to every issue we encounter and then the fix for it. This makes sure that a newly discovered problem will stay fixed.

Could you please reproduce the issue within a test? Without your fix the test should fail. You can measure the jerk you mentioned.

mathias-luedtke · 2017-09-27T15:25:25Z

I can confirm these jump movements in certain cases with the Schunk arms, but I haven't found out why.

In ros_canopen the state should be 2 cycles (-> 20ms) ahead of the command.
(command(t) gets sent at the same time as state(t+1) gets requested, the goal should be reached at state(t+2).)

I think the fix is correct. The last command should be kept in order to prevent jumps.

To further understand the effect and to provide a proper test:
What raw data are you plotting (and how)?
Can you plot as data points (e.g. small circles) or provide a bag file?
What's the input of the controller?
What's in the state topic (the CAN data might be affected by the limit interfaces)?

jschleicher · 2017-10-16T08:39:09Z

I've added a testcase. Please Review and comment on the code / test.

New Plots from the testcase run with
roslaunch joint_trajectory_controller joint_trajectory_controller.test display_plots:=True rosrun joint_trajectory_controller joint_trajectory_controller_test

This PR is the same as #176 and #177; we only searched the issues and thus opened a duplicate PR, sorry for that.

mathias-luedtke · 2017-10-16T21:07:33Z

Reviewing the old discussions using the desired state might lead to problems as well.
This is even worse because setHoldPosition gets called if the tracking error is too high.

As a compromise the hold target could get capped by the max. allowed tracking error.
Not sure if this really makes sense..

I've added a testcase. Please Review and comment on the code / test.

The test looks good, but I haven't tried it yet.

The delay topic just enables or disables the delay, so this should get reflected in a std_msgs/Bool topic or a std_srvs//SetBoolservice.
As an alternative the UInt8 value could specify the delay (= buffer size).

jschleicher · 2017-10-17T09:01:25Z

@ipa-mdl I'm not aware of all use-cases, where ros_controllers is used. But sending out desired values to an underlying HW-interface (can be PID or any other control), I don't see any point in jumping from a desired position with error to the actual position (what is the step response of the underlying controller, if the desired position changes abruptly?).

The main point of this PR is, that while a joint is moving, the actual position has a delay and is thus behind the first point of the calculated stop trajectory. A jump backwards cannot be the desired behaviour.

Adolfo-rt suggests in a comment, to preserve the old behaviour, if stop_time is zero. For me that could be an appropriate compromise. Please give me an example, where that is needed.

See pull request for fix/jerk_on_stop for detailed description. This commit adds a delay subscriber to rrbot to emulate a hardware controller with dead time. The jump backwards in the actual position is observed in joint_trajectory_controller_test by extending the stateCB. The new testCase fails, if the velocity of the actual position is negative although motion should only be forwards.

jschleicher · 2017-10-17T09:10:13Z

I've changed the test case accordingly.

The delay topic just enables or disables the delay, so this should get reflected in a std_msgs/Bool topic or a std_srvs//SetBoolservice.

jschleicher · 2017-11-16T16:05:58Z

@bmagyar What do you think? Could this be merged?

Should I preserve the old behaviour for stop_time=0?

Adolfo-rt suggests in a comment, to preserve the old behaviour, if stop_time is zero. For me that could be an appropriate compromise

bmagyar · 2018-01-11T11:42:19Z

...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

@@ -755,12 +762,14 @@ setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh)
  const unsigned int n_joints = joints_.size();
  for (unsigned int i = 0; i < n_joints; ++i)
  {
-    hold_start_state_.position[0]     =  joints_[i].getPosition();
-    hold_start_state_.velocity[0]     =  joints_[i].getVelocity();
+    // If there is a time delay in the system it is better to use calculate the hold trajectory starting from the


"better to use calculate the hold trajectory"

bmagyar · 2018-01-11T11:44:07Z

@jschleicher Yes, please preserve that behaviour.

Also, I know this request is pretty blunt, but can you think of more ways to add tests for your fix? One could be to ensure that old behaviour is preserved.

jschleicher · 2018-01-11T11:45:32Z

Yes, I'll add that.

Thanks for the review.

Adding a topic to rrbot to set an upper bound allows to test the actual position after setHoldPosition() has been called.

old behaviour for stop_trajectory_duration==0

mathias-luedtke · 2018-03-05T15:11:11Z

The tests are blocked..
Please rebase on top of the current kinetic-devel branch and resolve the merge conflicts.
Thanks ;)

jschleicher · 2018-03-05T15:52:20Z

I've added the requested test case, please review again.

mathias-luedtke · 2018-03-05T16:49:22Z

...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

@@ -138,6 +138,13 @@ starting(const ros::Time& time)
  time_data.uptime = ros::Time(0.0);
  time_data_.initRT(time_data);

+  // Initialize the desired_state with the current state on startup


This does not change the behavior, it gets overwritten in update anyway.

exactly: It gets overwritten on the next update(). But starting calls setHoldPosition three lines later and reads the desired_state (if stop_trajectory_duration_ != 0), so we have to initialize it beforehand.

Yes, I have just written this for clarification/justfication during the review :)

mathias-luedtke

After reading through the dicussions in #144 again, the consensus was that the stop_trajectory_duration option is not ideal..

I think adding a parameter/flag might be better to not break existing behavior.

However, the code and the tests look good.

mathias-luedtke · 2018-03-05T16:51:54Z

...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

+      hold_start_state_.position[0]     =  joints_[i].getPosition();
+      hold_start_state_.velocity[0]     =  0.0;
+      hold_start_state_.acceleration[0] =  0.0;
+      (*hold_trajectory_ptr_)[i].front().init(time.toSec(),  hold_start_state_,


nitpick: start_time could be used as well.

mathias-luedtke · 2018-03-05T17:50:03Z

The desired state is not aligned to the hardware resolution (the current state should be), so commanding the desired state might result in small alternating motions.

Perhaps we should command the current state (joint-wise) if commanding the desired state would change the direction of motion.
Update: This would need a stateful hold with recomputing/adapting the hold trajectory in every cycle step.

mathias-luedtke · 2018-03-06T13:10:49Z

The time parameter of setHoldPosition holds time_data_.uptime (time of last update). However, the desired_state will be reached at time_data_.uptime+ time_data_.period.

So with regard to the non-RT timing, it might make sense to set a 2 segment trajectory:

current_state @ time_data_.uptime
desired_state @ time_data_.uptime + time_data_.period
stop_state @ time_data_.uptime + time_data_.period + stop_trajectory_duration

Not sure if this really makes a difference..
However, this ensures that there is a valid sement for the next update cycle.

mathias-luedtke · 2018-03-06T13:27:46Z

As discussed with @jschleicher directly, this PR fixes a bug and the new behavior should be the default. If legacy users rely on the old behavior, it can be enabled by setting stop_trajectory_duration to zero (which is the default anyway). @bmagyar, do you agree?

jschleicher · 2018-03-06T14:10:14Z

The time parameter of setHoldPosition holds time_data_.uptime (time of last update). However, the desired_state will be reached

From the plots and the update function I'd say desired_state is written at time_data.uptime:

ros_controllers/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

Line 469 in e5a6c5a

hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,

So the hold trajectory is (using this PR) calculated starting from desired_state@time_data_.uptime. The first published value on the stop trajectory is calculated on the next update-call @time_data_.uptime + time_data_.period automatically.

bmagyar · 2018-03-06T19:34:17Z

joint_trajectory_controller/test/joint_trajectory_controller.test

@@ -35,5 +35,5 @@
  <test test-name="joint_trajectory_controller_test"
        pkg="joint_trajectory_controller"
        type="joint_trajectory_controller_test"
-        time-limit="85.0"/>
+        time-limit="95.0"/>


Is this necessary?

Yes, the additional test takes some more seconds. I think, some "sleep" in the old tests aren't really needed, I'll try to optimize a bit to stay at 85 seconds here.

bmagyar · 2018-03-06T19:35:48Z

joint_trajectory_controller/test/joint_trajectory_controller_vel.test

-        type="joint_trajectory_controller_test"
-        time-limit="85.0"/>
+        type="joint_trajectory_controller_vel_test"
+        time-limit="120.0"/>


Is this necessary?

This is "rebase" on develop, picked the change from there to avoid merge conflicts.

bmagyar · 2018-03-06T19:54:45Z

Guys, I think you are very close (or there already) to the source of #144 . I looked at it now, I think the approach is good and I've changed my stance on the question of the flag: we should put this by default.

BTW I experience this jerk every time I switch from the gravity compensation controller on Tiago to the joint_trajectory_contorller (the arm doesn't have time to fall, only a wee change on encoders).

As this is more of a major change, allow me to come back to this again later this week.

* use start_time variable in setHoldPosition * doxygen documentation in header file * drop sleep() in testcases in favor of waiting for PREEMPTED/SUCCEEDED state to speedup the tests a bit and stay below the former timeout of 85s

bmagyar · 2018-03-13T10:56:11Z

@ipa-mdl you may merge when ready.

mathias-luedtke · 2018-03-15T18:12:50Z

@jschleicher, regarding the timing:
The update function will compute the spline interpolation from the trajectory's starting state (at last uptime) to the final state (at last uptime + stop trajectory duration). So if the stop trajectoy duration is smaller than the cycle time, the spline interpolation will be skipped.
This does not matter for the postion interface, but it might be used for for the velocity and effort interface.

I guess for most users this should not be an issue because the stop trajectory duration should be big enough anyway for practical reasons.

mathias-luedtke

Please let me know if you want change anything w.r.t. the timing.
However, I am ready to merge this as-is.

Are you okay with squash-merging?
If not, please rebase accordingly, especially the last commit.

jschleicher · 2018-03-16T12:08:07Z

Yes, please squash-merge.
Thank you.

mathias-luedtke · 2018-03-16T13:10:51Z

Finally 🎉
Thanks for your patience and the many iterations!

bmagyar · 2018-03-16T21:51:00Z

Thanks guys for the effort!

Use desired state to calculate hold trajectory

e53b364

bmagyar self-assigned this Sep 26, 2017

bmagyar self-requested a review September 26, 2017 12:20

bmagyar added bug enhancement labels Sep 26, 2017

jschleicher force-pushed the fix/jerk_on_stop branch from 0eca54b to 9bc9fe7 Compare October 17, 2017 09:09

bmagyar requested changes Jan 11, 2018

View reviewed changes

bmagyar requested a review from mathias-luedtke January 11, 2018 11:44

jschleicher added 2 commits March 5, 2018 16:07

add test case with upper bound (simulating a wall)

e0d7bee

Adding a topic to rrbot to set an upper bound allows to test the actual position after setHoldPosition() has been called.

preserve jump, if stop_trajectory_duration==0

e717d26

old behaviour for stop_trajectory_duration==0

jschleicher force-pushed the fix/jerk_on_stop branch from 8a9e3db to e717d26 Compare March 5, 2018 15:08

mathias-luedtke reviewed Mar 5, 2018

View reviewed changes

bmagyar reviewed Mar 6, 2018

View reviewed changes

incorporate review comments

7a47b0b

* use start_time variable in setHoldPosition * doxygen documentation in header file * drop sleep() in testcases in favor of waiting for PREEMPTED/SUCCEEDED state to speedup the tests a bit and stay below the former timeout of 85s

bmagyar approved these changes Mar 13, 2018

View reviewed changes

mathias-luedtke approved these changes Mar 15, 2018

View reviewed changes

mathias-luedtke merged commit b10c2b7 into ros-controls:kinetic-devel Mar 16, 2018

bmagyar mentioned this pull request Mar 23, 2018

How to use joint_trajectory_controller to smooth motion between multiple poses? #311

Open

This was referenced May 15, 2018

Set hold using desired - only for discussion first #177

Closed

Set desired in starting #176

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

TrajectoryController: Use desired state to calculate hold trajectory #297

TrajectoryController: Use desired state to calculate hold trajectory #297

agutenkunst commented Sep 11, 2017

bmagyar commented Sep 26, 2017

mathias-luedtke commented Sep 27, 2017 •

edited

Loading

jschleicher commented Oct 16, 2017

mathias-luedtke commented Oct 16, 2017

jschleicher commented Oct 17, 2017

jschleicher commented Oct 17, 2017

jschleicher commented Nov 16, 2017

bmagyar Jan 11, 2018

bmagyar commented Jan 11, 2018

jschleicher commented Jan 11, 2018

mathias-luedtke commented Mar 5, 2018 •

edited

Loading

jschleicher commented Mar 5, 2018

mathias-luedtke Mar 5, 2018

jschleicher Mar 6, 2018

mathias-luedtke Mar 6, 2018

mathias-luedtke left a comment •

edited

Loading

mathias-luedtke Mar 5, 2018

jschleicher Mar 6, 2018

mathias-luedtke commented Mar 5, 2018 •

edited

Loading

mathias-luedtke commented Mar 6, 2018 •

edited

Loading

mathias-luedtke commented Mar 6, 2018

jschleicher commented Mar 6, 2018

bmagyar Mar 6, 2018

jschleicher Mar 7, 2018

bmagyar Mar 6, 2018

jschleicher Mar 7, 2018

bmagyar commented Mar 6, 2018

bmagyar commented Mar 13, 2018

mathias-luedtke commented Mar 15, 2018

mathias-luedtke left a comment

jschleicher commented Mar 16, 2018

mathias-luedtke commented Mar 16, 2018

bmagyar commented Mar 16, 2018

TrajectoryController: Use desired state to calculate hold trajectory #297

TrajectoryController: Use desired state to calculate hold trajectory #297

Conversation

agutenkunst commented Sep 11, 2017

bmagyar commented Sep 26, 2017

mathias-luedtke commented Sep 27, 2017 • edited Loading

jschleicher commented Oct 16, 2017

mathias-luedtke commented Oct 16, 2017

jschleicher commented Oct 17, 2017

jschleicher commented Oct 17, 2017

jschleicher commented Nov 16, 2017

Choose a reason for hiding this comment

bmagyar commented Jan 11, 2018

jschleicher commented Jan 11, 2018

mathias-luedtke commented Mar 5, 2018 • edited Loading

jschleicher commented Mar 5, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mathias-luedtke left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mathias-luedtke commented Mar 5, 2018 • edited Loading

mathias-luedtke commented Mar 6, 2018 • edited Loading

mathias-luedtke commented Mar 6, 2018

jschleicher commented Mar 6, 2018

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

bmagyar commented Mar 6, 2018

bmagyar commented Mar 13, 2018

mathias-luedtke commented Mar 15, 2018

mathias-luedtke left a comment

Choose a reason for hiding this comment

jschleicher commented Mar 16, 2018

mathias-luedtke commented Mar 16, 2018

bmagyar commented Mar 16, 2018

mathias-luedtke commented Sep 27, 2017 •

edited

Loading

mathias-luedtke commented Mar 5, 2018 •

edited

Loading

mathias-luedtke left a comment •

edited

Loading

mathias-luedtke commented Mar 5, 2018 •

edited

Loading

mathias-luedtke commented Mar 6, 2018 •

edited

Loading