trajectory_controller for velocity hardware_interface

[davetcoleman]

I see in the README for the (sweet) new trajectory controller

Position and effort control joint interfaces provided by default.

What is stopping me from implementing a hardware_interface_adapter for a VelocityJointInterface? Is there a reason you haven't made it yet, or did you just not have the need?

From what I can tell I can just copy the effort hardware_interface_adapter, and change the PID line to say something like:

const double command = pids_[i]->computeCommand(state_error.velocity[i], period);

I have a robot that only takes position and velocity commands and I believe using velocity is always the better option for controls (is this true?).

Thanks!

[adolfo-rt]

What is stopping me from implementing a hardware_interface_adapter for a VelocityJointInterface? Is there a reason you haven't made it yet, or did you just not have the need?

I currently have the usecase of position joints only. I did the effort joint part because there's a lot of robots out there with effort interfaces, and to prove the point that multiple intefaces can be supported :-)

From what I can tell I can just copy the effort hardware_interface_adapter, and change the PID line to say something like:

const double command = pids_[i]->computeCommand(state_error.velocity[i], period);

You can implement it like this, but then you won't be able to compensate position drift, but will instead accumulate it over time (and potentially without bounds!!!). You can still implement a velocity PID loop based on the position error. While in the pure position PID case you provide a corrective position increment to compensate for desired position/velocity errors, in the proposed velocity PID you instead provide a corrective velocity increment. The gains values will probably be different in both cases. I haven't tested this, but should work.

I have a robot that only takes position and velocity commands and I believe using velocity is always the better option for controls (is this true?).

Is this position OR velocity or position AND velocity?.

Thanks!

[davetcoleman]

Its a velocity OR position controller.

So what you are saying is that the hardware_interface_adapter is the same for both position and velocity controllers, except that the gains should be different - correct?

[adolfo-rt]

So what you are saying is that the hardware_interface_adapter is the same for both position and velocity controllers, except that the gains should be different - correct?

This is what I think would work best: An additive feedforward+feedback control law.

• Feedforward term: Desired velocity, as computed by the joint trajectory controller.
• Feedback: PID that compensates drift from the desired position.

Since most of the work will be done by the feedforward term, you'll probably end up with a low-gain PID controller. This strategy is similar to that used in inverse dynamics controllers to achieve compliant control, but in the effort domain.

[skohlbr]

The velocity hardware interface appears only to be available in the "velocity_position_controller" branch so far. Can this be merged into hydro_devel/indigo_devel? I can report successful use it on both Baxter and a Schunk LWA4P arm.

[adolfo-rt]

My last comments on how to implement the feed forward and feedback terms were never given closure. I suggested an implementation in #64, but didn't do any follow-up. I'm currently on holidays, and code review from my cellphone is not fun. I'll come back to this next week.

I'd be really happy to merge support for the velocity interface. Thanks for reviving this issue.

[davetcoleman]

I can't remember where I left this pull request at - I'm not sure I completely understood Adolfo's comments so I've just been using my original implementation with Baxter for months now. Actually, I attempted to do his fix in this commit: https://github.com/ros-controls/ros_controllers/commit/533ff43be53ea4996ffa9b2e079f889857e97483

But then Adolfo said I was still missing something. I'm not the biggest controls expert.

Also, PR #64 was waiting for https://github.com/ros-controls/ros_controllers/pull/62 to be merged first, but Adolfo and Wim both had suggestions to fix it that I didn't completely understand, so I never pursed it. I've also been using that PR for months now with Baxter.

[adolfo-rt]

But then Adolfo said I was still missing something.

It's not that it's missing something. There are two alternative implementations, and I'm not sure which one will work best:

1. In the current implementation proposed by @davetcoleman, the PID of the joint_trajectory_controller fully determine the tracking behavior of the controller. The only objection I see is that using state_error.velocity as the PID's error velocity here does not seem right, even though it experimentally works. Using state_error.velocity, as it originally was, seems more correct.
2. What I suggest in this inline comment is that the PID of the joint_trajectory_controller only compensates the position drift. The reference velocity signal is passed as-is to the robot.

I would suggest doing a couple of tests to determine what works best. Something like a sine at different frequencies (slow and fast), and also disturbing (holding) the joint to make the tracking error large and then releasing the joint.

Also, PR #64 was waiting for #62 to be merged first

If we come to a consensus on how to send velocity commands, we can extract the relevant changes to a separate PR that does not include #62.

[fmessmer]

I'd be interested in trying the joint_trajectory_controller with the JointVelocityInterface. What's the latest news on this issue? Have the tests mentioned above lead to any decision yet? Or is #64 not being merged as it includes #62?

[adolfo-rt]

There has not been any action in addressing point 1. above. If this is done, and a PR that excludes #62 is submitted, we can merge.

[fmessmer]

As to your point 1, see my comment here https://github.com/ros-controls/ros_controllers/commit/533ff43be53ea4996ffa9b2e079f889857e97483#commitcomment-6929720

I'm not sure what the best practise would be to isolate #64 from anything related to #62 while keeping up the history and without simply re-adding @davetcoleman 's files.... Rebase? Cherry-Pick? Any ideas?

[adolfo-rt]

The changesets can be just cherry-picked, because they are not atomic enough, and modify files/controller other than joint_trajectory_controller. There's code for the controller mode switch, but there's also a new single-joint controller for velocity interfaces.

To exercise git, you can follow an approach based on this post:

http://stackoverflow.com/questions/1526044/partly-cherry-picking-a-commit-with-git

• Create a new branch from indigo-devel.
• Find the minimum set of changes that contain relevant joint_trajectory_controller modifications.
• Cherry-pick them with git cherry-pick --no-commit, and unstage the changes with git reset. You can now discard unwanted changes, and git add or git add -p relevant changes. I expect merge conflicts at least on the CMakeLists.txt, because in indigo we dropped rosbuild support.
• Once you're done, do an interactive rebase to squash commits if needed, and end up with a clean history.
• Submit beautiful shiny pull request.

If the changes are few, and the above seems cumbersome, you can also just apply them manually on top of an indigo-devel branch, and remember to give @davetcoleman attribution for his contribution in the commit message.

[adolfo-rt]

As to your point 1, see my comment here 533ff43#commitcomment-6929720

That would be point 2. (this is getting confusing). Point 1. would be to keep the code mostly as-is, but change only the second parameter of the pid call, which expects the derivative error, not the desired velocity.

const double command = pids_[i]->computeCommand(state_error.position[i], desired_state.velocity[i], period);

to

const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);

This also mirrors how the effort hardware_interface_adapter is implemented.

I suggest making this minimal change and validating that the controller works as expected. That should be enough to merge. Since this addition is not covered by a unit test, I'd at least have someone validate it practically.

[fmessmer]

Ok, thanks! I'll try to do the split and account for your suggestions as soon as I find the time...

[fmessmer]

I tried to split the various features in separate pull requests:

108 velocity_controller/JointPositionController

109 velocity interface for JointTrajectoryController

110 joint_mode_controller