Add velocity feedforward term to velocity HardwareInterfaceAdapter #227

miguelprada · 2016-06-15T10:28:59Z

In the current implementation of the VelocityJointInterface specialization for HardwareInterfaceAdapter, a velocity command for the joint will only be generated (by the PID) in the presence of tracking error, therefore perfect tracking can never be achieved.

Adding a velocity feedforward term unloads much of the burden from the PID, which only needs to correct for the position and velocity drift that may appear if (when) the robot does not perfectly follow the generated commands. This should allow to improve the controller's performance even decreasing the PID gains significantly, therefore improving also the controller's stability.

I'm not 100% sure of the effects of this change in configurations where the PIDs have been tuned to carry all the load of tracking the velocity reference. I'm inclined to believe that nothing bad will happen, although some tests or some sound theoretical proof would be nice to have in this respect.

bmagyar · 2016-06-16T13:34:45Z

Nice catch! First, could you please add some unit tests of expected outcomes? That is a good base to start a discussion on.

Also adding @ipa-fxm for further thoughts

miguelprada · 2016-06-16T14:27:20Z

I'd be glad to, but I'm honestly not quite sure of what the tests should look like.

Only thing I can think of is making this feedforward term optional, using a parameter, and to include a test which compares the evolution of the velocity-based trajectory controller with and without the feedforward term. This would require adding a VelocityJointInterface to the RRbot used for testing, although it seems quite doable.

An additional benefit would be that adding a VelocityJointInterface to the RRbot would also allow to run the rest of the existing tests on the velocity-based controller, which, if I'm not wrong, are currently run only for the position-based trajectory controller.

What do you think? Any better ideas @bmagyar, @ipa-fxm, anyone?

miguelprada · 2016-06-20T20:57:39Z

I just opened PR #228, duplicating the tests for the velocity-based joint trajectory controller, as a basis for testing this PR.

Any thoughts on how the tests for the (optional) feedforward term should look like?

And BTW, what's the nice thing to do with this PR? Rebase the only commit on top of #228 and reopen? Not quite comfortable with the rules of etiquette here yet 😅

bmagyar · 2016-07-01T07:51:10Z

Sorry Miguel for the delay.
First: PRs can be rebased without the need to reopen them. You can do this by rebasing the branch you made the pull request from, and doing a push --force on it, overwriting the history. Make sure you have a backup if you don't feel comfortable doing this.

Duplicating all that test code may not be the best solution. I mean 1000 lines of duplication are kinda hard to maintain :)

Any comments on this, @efernandez ?

I'm going to be quite busy this week but will try to get back next week, feel free to ping me.

efernandez · 2016-07-04T01:09:35Z

I'm not an expert in control. My understanding is that with this the PID actually works on the state error, which is good.

Since I'm not sure of the side effects of this, I'd feel more comfortable if you could provide a param to enable/disable the feedforward term, and set it to false by default to keep previous behaviour; regarding the default value, I honestly don't have a strong opinion. Indeed, I really like that the PIDs can have smaller values and actually work on the state error, which I think it makes more sense.

miguelprada · 2016-07-19T07:37:41Z

Sorry, guys, for the delay.

Duplicating all that test code may not be the best solution. I mean 1000 lines of duplication are kinda hard to maintain :)

Totally agree with this, but I was a bit confused by how the TEST_F macros have harcoded values for the test fixture and figured at first that it was important to make them different across tests.

I just commited to #228 with what I hope is a better solution for the tests, which duplicates no code at all, and just requires duplicating the controller configuration and test launchfile. Let me know what you think, @bmagyar.

Since I'm not sure of the side effects of this, I'd feel more comfortable if you could provide a param to enable/disable the feedforward term, and set it to false by default to keep previous behaviour; regarding the default value, I honestly don't have a strong opinion. Indeed, I really like that the PIDs can have smaller values and actually work on the state error, which I think it makes more sense.

I also thought about this and think it's a reasonable solution. Regarding the default value, I would be inclined to make it enabled by default at some point, but I understand if you want to leave that for future releases in order to avoid messing with working setups.

I'll give this a go, @efernandez.

miguelprada · 2016-07-19T09:01:35Z

Hi guys, just changed the velocity feedforward term to be enabled or disabled based on a parameter named use_velocity_ff. The default case if the parameter is not found is to disable the feedforward term, although this is straightforward to change if it is decided to enable it by default.

I also rebased on top of #228, even though it hasn't been merged yet. I'm sorry if this causes any inconveniences.

Since #228 does not contain the velocity feedforward term, though, there's no specific test to validate it. Can you think of any additional test that can be added to validate this specific addition?

bmagyar · 2016-08-02T11:10:51Z

Our common practice is to have one (perhaps one set of) explicit test(s) for each parameter we add. This makes sure that branch testing is somewhat achieved.

miguelprada · 2017-05-14T20:23:02Z

I finally managed to add a dedicated test for the velocity feedforward optional term!

What I did is:

drastically lower the PID gains for the VelocityJointInterface-based controller and enable the feedforward term in rrbot_vel_controllers.yaml, which does not affect the outcome of the existing tests
add a new test¹ which executes the same trajectory twice, one with and one withouth use_velocity_ff, showing that the first one succeeds while the second one aborts due to a path tolerance violation
disable the test for PositionJointInterface
fix a typo

Thoughts?

¹ If you wonder about the place where I inserted the new test, I put it there because I initially added it at the end and this assertion was always failing. Moving it before the tolerance checking tests made it work flawlessly, so I guess there's some side effect from the latter two tests. I haven't had time to figure out what happens, though.

miguelprada · 2017-05-14T20:55:50Z

I don't understand what's going on with the tests. They:

just failed when triggered by the PR, but for the PositionJointInterface controller test here
failed as well (in different places) for my fork when I rebased the branch before the PR, a test whose results I didn't wait for because it...
passed for the very same commit (c3e0988) when I was testing on another branch before rebasing add_velocity_ff
they also consistently pass for me locally

Any ideas? Can anyone else please try?

mathias-luedtke · 2017-05-15T09:06:18Z

The current test set is very sensitive to timing and processor load. That's why we have configured travis to run only one test at a time. However, this approach does not fix the timing issues, but reduces the number of occurrences. In addition I am not 100% sure that we really limit the number of jobs properly.

Right now I am runnign the tests of the current master and your PR locally with run_ci.

Update: Tests passed locally for multiple times.

miguelprada · 2017-05-15T11:58:38Z

ROS_DISTRO=kinetic ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu rosrun industrial_ci run_ci failed for me 2 out of 10 times locally. 😕

miguelprada · 2017-05-15T12:27:46Z

Also, if this happens to be finally merged, is there any chance the changes are backported to indigo? Not sure about the compatibility policy that's followed around here.

mathias-luedtke · 2017-05-15T13:22:25Z

joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h

@@ -213,7 +216,7 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
    // Update PIDs
    for (unsigned int i = 0; i < n_joints; ++i)
    {
-      const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
+      const double command = (desired_state.velocity[i] * use_velocity_ff_) + pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);


This line multiplies a boolean with a double. value.
I would prefer a velocity_ffparam, which defaults to 0.0

Is it the unconventional double * bool multiplication which you don't like or is it restricting the range of values to effectively 0 and 1? If its the multiplication I can always reformulate in a conditional statement.

I ask because I can't think of any reasonable case where feedforward gains other than 0 or 1 make sense. Can you?

I am not sure if that really made sense, but we used 0.9 for the Care-O-bot 3 arms (pre-ros_control stack).
I don't see a reason to restrict it to either 0.0 or 1.0, but I am no contoller expert.

Me neither. I had always assumed that the feedforward was either there or not, but let's assume whoever put the 0.9 in the COB knew what he was doing and make the parameter a double.

Then, what would you suggest for values greater than 1.0? Should we raise some warning and clamp, just raise a warning or do nothing altogether?

mathias-luedtke · 2017-05-15T13:27:09Z

Travis runs

 rosrun industrial_ci run_ci ROS_DISTRO=kinetic ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu CATKIN_PARALLEL_TEST_JOBS=-p1 ROS_PARALLEL_TEST_JOBS=-j1

bmagyar · 2017-05-15T14:31:45Z

Thanks for your work on this @miguelprada . I can't join in the review discussion at the moment but I'd like to comment on the note about Indigo. Personally I'd like to eventually freeze Indigo and only allow bugfixes but we can discuss small and controlled features to be added if there's a big demand.

miguelprada · 2017-05-16T08:00:17Z

Personally I'd like to eventually freeze Indigo and only allow bugfixes but we can discuss small and controlled features to be added if there's a big demand.

I'm not sure about the demand, but there's a specific reason I think it would be nice to have this in indigo.

The ur_modern_driver's implementation performs much better when using the VelocityJointInterface than when using the PositionJointInterface (due to a non-negligible delay introduced by the latter). However, the behavior of the speedj URScript command (used by the VelocityJointInterface) has changed a couple of times in past UR software updates, introducing delays and making some users of this driver experience stability issues (myself included). As a last note, ur_modern_driver is not yet available for Kinetic.

Adding this patch to the indigo branch would allow reducing the gains in the default configuration for the joint_trajectory_controller in that driver, potentially removing/reducing the instability issues.

…y_controllers::JointTrajectoryController.

…it for the EffortJointInterface as well.

miguelprada · 2017-11-21T16:06:19Z

Just added a new commit making the parameter a double, as @ipa-mdl suggested.

As I was on it, I also added the optional feedforward available for the EffortJointInterface. In fact, it is in the latter case where I can see values different than 0 or 1 make any sense (e.g. to compensate for friction, based on velocity), but I guess it's better to have some consistency in the parameter API for the different interfaces.

Also, I thought it made more sense to be able to set a different value of the feedforward gain for each joint, so I changed how the parameter is read as well.

One missing thing would be to add tests for the EffortJointInterface, similarly as #228, but it would be much more complex than that. I think it's beyond the scope of this PR, but let me know if you think otherwise.

Thoughts, anyone?

EDIT: I also rebased on top of current kinetic-devel. Hope that's fine.

miguelprada · 2017-12-05T12:23:09Z

Friendly ping, @bmagyar, @ipa-mdl.

Not sure why job #528.3 is failing. It passes for me locally. Can anyone try to relaunch?

bmagyar · 2017-12-05T12:33:56Z

Thumbs up for the rebase!
I've also relaunched the travis job.
Disregard the failed test on four_wheel_steering_controller, we have to revise those numbers to get more stable tests.

miguelprada · 2017-12-05T15:33:55Z

And yet another failed test which I'm unable to reproduce locally 😓

The failed test case seems to be different each time, though. Not sure what to do about this, to be honest.

mathias-luedtke · 2017-12-05T16:04:48Z

And yet another failed test which I'm unable to reproduce locally 😓

It's all about timing, hard to reproduce..

Not sure what to do about this, to be honest.

I would suggest to ignore it for now, unless all jobs fail..

miguelprada · 2017-12-15T14:46:20Z

Are you waiting for any additional input from me? Maybe I'm missing something, but I think all existing comments have been addressed.

It would be a nice Xmas present to finally get this merged so I can stop including ros_controllers source in all my workspaces 😉

mathias-luedtke · 2017-12-15T15:02:46Z

joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h

@@ -277,6 +286,13 @@ class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State>
      }
    }

+    // Load velocity feedforward gains from parameter server


Can this be reused somehow?
It's the same as in line 185..
FF-enabled base class?

In fact, most code is duplicated between the VelocityJointInterface and the EffortJointInterface adapters. I'll refactor into a common base class for these specializations.

mathias-luedtke · 2017-12-15T15:04:59Z

joint_trajectory_controller/test/joint_trajectory_controller.test

@@ -35,5 +35,6 @@
  <test test-name="joint_trajectory_controller_test"
        pkg="joint_trajectory_controller"
        type="joint_trajectory_controller_test"
-        time-limit="85.0"/>
+        time-limit="85.0"
+        args="--gtest_filter=-JointTrajectoryControllerTest.jointVelocityFeedForward"/>


Why do you exclude this test?

I guess this test file is for the position interface only?
Since the code gets built twice anyway, It could be an option to add macro switch to emphasize that this test won't get run for the

At least a comment should be added.

I guess this test file is for the position interface only?

Exactly.

Since the code gets built twice anyway

Yeah, not very happy about this either. I'll see if I can come up with some way to only build them once.

And I'll add some comment anyways.

I'll see if I can come up with some way to only build them once.

Something like
target_compile_definitions(joint_trajectory_controller_vel_test PRIVATE TEST_VELOCITY_FF=1) would justify somehow to build it twice ;)

IMHO building it twice is not too bad.

Building it just once would require (?) to:

move tests (without FF) into a library

expose text fixture class as header (in test directory)

add source file for the main function

add_rostest_gtest with main, linked to lib

add_rostest_gtest with main + ff-test, linked to lib

Finally went for the #ifdef TEST_VELOCITY_FF check. Is it more or less what you had in mind, @ipa-mdl ?

Finally went for the #ifdef TEST_VELOCITY_FF check. Is it more or less what you had in mind,

It's exactly what I had in mind :D

bmagyar · 2017-12-15T17:39:05Z

I won't have sleepless nights over building some tests twice :) Just make sure we test all we need to. Me or Mathias can merge this once finished.

Most code in Velocity and EffortJointInterface specializations of HardwareInterfaceAdapter was shared and has been refactored into a common base class.

…ecutable

mathias-luedtke · 2017-12-18T14:25:44Z

joint_trajectory_controller/test/joint_trajectory_controller_test.cpp

@@ -981,6 +981,9 @@ TEST_F(JointTrajectoryControllerTest, ignorePartiallyOldActionTraj)
 }

 // Velocity FF parameter ///////////////////////////////////////////////////////////////////////////////////////////////
+// This test will only be built and run for the VelocityJointInterface-based version of the JointTrajectoryController
+
+#ifdef TEST_VELOCITY_FF


Please consider #if TEST_VELOCITY_FF to handle TEST_VELOCITY_FF=0 as well.

Just noticed this comment. Updated.

mathias-luedtke · 2017-12-18T14:27:09Z

Thanks for the refactoring!
Why did you name it ClosedLoopHardwareInterfaceAdapter? Can't position controllers be closed-loop as well?

Somehow the joint trajectory controller have become unstable with the last commit.

miguelprada · 2017-12-18T15:09:09Z

Why did you name it ClosedLoopHardwareInterfaceAdapter? Can't position controllers be closed-loop as well?

In those cases the adapter closes a loop by computing their commands based on the desired state and the state error. This is not the case for the rest of the adapter specializations.

Somehow the joint trajectory controller have become unstable with the last commit.

Which one? The one modifying the tests or the refactor? I must admit I just run the tests locally and pushed when I saw that they passed 😳

I'll play with it locally a bit.

mathias-luedtke · 2017-12-18T15:18:43Z

In those cases the adapter closes a loop by computing their commands based on the desired state and the state error. This is not the case for the rest of the adapter specializations.

I see, this makes sense!

Which one? The one modifying the tests or the refactor?

79bf8d1. I have restarted 537.2.

537.3 failed as well, but with a different errror

mathias-luedtke · 2017-12-18T17:15:08Z

Thanks for the update!

i have restarted all test jobs, which failed for unrelated errors :-/.

mathias-luedtke

Thanks a lot for your patience!
This is definitely ready for merge now.

I will wait some time (a couple of days?) for others to review it as well.

miguelprada · 2017-12-18T17:26:24Z

No hurries. What's a couple of days compared with a year an a half I've been dragging this along 😉

mathias-luedtke · 2017-12-24T11:52:54Z

ho ho ho 🎅

miguelprada force-pushed the add_velocity_ff branch from 3c7fafa to d02dd6d Compare June 15, 2016 12:42

miguelprada force-pushed the add_velocity_ff branch from d02dd6d to b292feb Compare July 19, 2016 08:55

miguelprada force-pushed the add_velocity_ff branch from b292feb to c3e0988 Compare May 14, 2017 19:58

mathias-luedtke reviewed May 15, 2017

View reviewed changes

miguelprada added 5 commits November 21, 2017 15:34

Add velocity feedforward term to HardwareInterfaceAdapter for velocit…

f10f05c

…y_controllers::JointTrajectoryController.

Add parameter to control velocity feedforward term.

a4c56e3

Add tests for use_velocity_ff.

5b00a3b

Fix inconsistency.

8584132

Make velocity feedforward a 'double' param, instead of bool, and use …

906b85d

…it for the EffortJointInterface as well.

miguelprada force-pushed the add_velocity_ff branch from c3e0988 to 906b85d Compare November 21, 2017 15:50

miguelprada mentioned this pull request Dec 15, 2017

Add arguments to ros_control launchfiles to select loaded controllers. ros-industrial/ur_modern_driver#149

Merged

mathias-luedtke reviewed Dec 15, 2017

View reviewed changes

miguelprada added 2 commits December 18, 2017 12:26

Refactor common code into common base class

45a14ff

Most code in Velocity and EffortJointInterface specializations of HardwareInterfaceAdapter was shared and has been refactored into a common base class.

Avoid building velocity interface-specific tests for position test ex…

79bf8d1

…ecutable

mathias-luedtke reviewed Dec 18, 2017

View reviewed changes

Properly handle TEST_VELOCITY_FF=0 case in conditional build

8313f63

mathias-luedtke approved these changes Dec 18, 2017

View reviewed changes

mathias-luedtke merged commit 6591a56 into ros-controls:kinetic-devel Dec 24, 2017

miguelprada mentioned this pull request Oct 24, 2018

Adjust the PID gains for the vel_based_pos_traj_controller to reasona… tecnalia-medical-robotics/ur_modern_driver#1

Closed

Add velocity feedforward term to velocity HardwareInterfaceAdapter #227

Add velocity feedforward term to velocity HardwareInterfaceAdapter #227

Conversation

miguelprada commented Jun 15, 2016

bmagyar commented Jun 16, 2016

miguelprada commented Jun 16, 2016

miguelprada commented Jun 20, 2016

bmagyar commented Jul 1, 2016

efernandez commented Jul 4, 2016

miguelprada commented Jul 19, 2016

miguelprada commented Jul 19, 2016

bmagyar commented Aug 2, 2016

miguelprada commented May 14, 2017 • edited Loading

miguelprada commented May 14, 2017 • edited Loading

mathias-luedtke commented May 15, 2017 • edited Loading

miguelprada commented May 15, 2017 • edited Loading

miguelprada commented May 15, 2017

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

mathias-luedtke commented May 15, 2017

bmagyar commented May 15, 2017

miguelprada commented May 16, 2017

miguelprada commented Nov 21, 2017 • edited Loading

miguelprada commented Dec 5, 2017

bmagyar commented Dec 5, 2017 • edited Loading

miguelprada commented Dec 5, 2017

mathias-luedtke commented Dec 5, 2017

miguelprada commented Dec 15, 2017

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

mathias-luedtke Dec 15, 2017 • 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

bmagyar commented Dec 15, 2017 via email • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mathias-luedtke commented Dec 18, 2017

miguelprada commented Dec 18, 2017

mathias-luedtke commented Dec 18, 2017

mathias-luedtke commented Dec 18, 2017 • edited Loading

mathias-luedtke left a comment

Choose a reason for hiding this comment

miguelprada commented Dec 18, 2017

mathias-luedtke commented Dec 24, 2017

miguelprada commented May 14, 2017 •

edited

Loading

miguelprada commented May 14, 2017 •

edited

Loading

mathias-luedtke commented May 15, 2017 •

edited

Loading

miguelprada commented May 15, 2017 •

edited

Loading

miguelprada commented Nov 21, 2017 •

edited

Loading

bmagyar commented Dec 5, 2017 •

edited

Loading

mathias-luedtke Dec 15, 2017 •

edited

Loading

bmagyar commented Dec 15, 2017 via email •

edited

Loading

mathias-luedtke commented Dec 18, 2017 •

edited

Loading