Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds velocity control to JointPositionController. #1003

Merged
merged 8 commits into from
Sep 14, 2021

Conversation

arjo129
Copy link
Contributor

@arjo129 arjo129 commented Aug 31, 2021

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

🎉 New feature

Closes #

Summary

There are times when we don't nessecarily want to tune a PID for each joint. Often we
may assume that the physical robot already has a well tuned controller. This
was true in the case of the mbari LRAUV.
Hence, there should be a way to set position without the use of a PID
controller. This commit adds a use_velocity_commands option which when set to true bypasses the force
based PID giving a very predictable position. The use_velocity_command mode has a cmd_max which in turn
can be used to set a maximum velocity which the joint can move at.

Signed-off-by: Arjo Chakravarty [email protected]

Test it

TODO:

  • Add unit test.
  • The current implementation was developed for a prismatic joint. We need to add angle wrapping for a prismatic joint otherwise the controller may take the long way around. For now we maynot actually need this.

Checklist

  • Signed all commits for DCO
  • Added tests
  • Added example and/or tutorial
  • Updated documentation (as needed)
  • Updated migration guide (as needed)
  • codecheck passed (See contributing)
  • All tests passed (See test coverage)
  • While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

There are times when we don't nessecarily want to tune a PID for each joint. Often we
may assume that the physical robot already has a well tuned controller. This
was true in the case of the mbari LRAUV.
Hence, there should be a way to set position without the use of a PID
controller.  This commit adds a
`use_velocity_commands` option which when set to true bypasses the force
based PID giving a very predictable
position. The `use_velocity_command` mode has a `cmd_max` which in turn
can be used to set a maximum velocity which the joint can move at.

Signed-off-by: Arjo Chakravarty <[email protected]>
@arjo129 arjo129 requested a review from caguero August 31, 2021 06:03
@arjo129 arjo129 requested a review from chapulina as a code owner August 31, 2021 06:03
@github-actions github-actions bot added the 🏯 fortress Ignition Fortress label Aug 31, 2021
@codecov
Copy link

codecov bot commented Aug 31, 2021

Codecov Report

Merging #1003 (b5f7d05) into main (f0e235b) will increase coverage by 0.61%.
The diff coverage is 95.00%.

❗ Current head b5f7d05 differs from pull request most recent head 9e64c03. Consider uploading reports for the commit 9e64c03 to get more accurate results
Impacted file tree graph

@@            Coverage Diff             @@
##             main    #1003      +/-   ##
==========================================
+ Coverage   63.28%   63.90%   +0.61%     
==========================================
  Files         238      246       +8     
  Lines       19423    20035     +612     
==========================================
+ Hits        12292    12803     +511     
- Misses       7131     7232     +101     
Impacted Files Coverage Δ
...int_position_controller/JointPositionController.hh 100.00% <ø> (ø)
...int_position_controller/JointPositionController.cc 75.19% <95.00%> (+3.63%) ⬆️
src/Server.cc 84.33% <0.00%> (-2.41%) ⬇️
src/ServerPrivate.cc 87.15% <0.00%> (-0.92%) ⬇️
.../plugins/component_inspector/ComponentInspector.cc 6.69% <0.00%> (-0.05%) ⬇️
src/gui/plugins/shapes/Shapes.cc 25.00% <0.00%> (ø)
src/systems/breadcrumbs/Breadcrumbs.hh 100.00% <0.00%> (ø)
src/systems/touch_plugin/TouchPlugin.hh 100.00% <0.00%> (ø)
include/ignition/gazebo/gui/GuiSystem.hh 0.00% <0.00%> (ø)
include/ignition/gazebo/components/Component.hh 100.00% <0.00%> (ø)
... and 15 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 5a47f37...9e64c03. Read the comment docs.

@chapulina chapulina added the beta Targeting beta release of upcoming collection label Sep 8, 2021
Copy link
Contributor

@chapulina chapulina left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM, I just have a question about using cmd_min.

auto dt = std::chrono::duration<double>(_info.dt).count();

// Get the maximum amount in m that this joint may move
auto maxMovement = this->dataPtr->posPid.CmdMax() * dt;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we also use CmdMin for negative velocities?

Copy link
Contributor Author

@arjo129 arjo129 Sep 14, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think its absolutely essential. We could add this in a future PR/release. I guess it would simulate actuators which have asymmetric forward and backward speeds. That being said, I do not feel it is necessary immediately. If we really do want to care about motor characterisitcs then we should probably add stuff like <max_acceleration> <max_torque> etc.

arjo129 and others added 2 commits September 14, 2021 10:17
Signed-off-by: Arjo Chakravarty <[email protected]>
@arjo129 arjo129 force-pushed the arjo/JointsWithoutPID branch from 8ceaed2 to 6603a7b Compare September 14, 2021 02:18
@arjo129
Copy link
Contributor Author

arjo129 commented Sep 14, 2021

@chapulina This is ready for another round of review.

@chapulina chapulina merged commit b965575 into main Sep 14, 2021
@chapulina chapulina deleted the arjo/JointsWithoutPID branch September 14, 2021 18:04
@chapulina chapulina mentioned this pull request Sep 16, 2021
7 tasks
WilliamLewww pushed a commit to WilliamLewww/ign-gazebo that referenced this pull request Dec 7, 2021
There are times when we don't nessecarily want to tune a PID for each joint. Often we
may assume that the physical robot already has a well tuned controller. This
was true in the case of the mbari LRAUV.
Hence, there should be a way to set position without the use of a PID
controller.  This commit adds a
`use_velocity_commands` option which when set to true bypasses the force
based PID giving a very predictable
position. The `use_velocity_command` mode has a `cmd_max` which in turn
can be used to set a maximum velocity which the joint can move at.

Signed-off-by: Arjo Chakravarty <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
Signed-off-by: William Lew <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
beta Targeting beta release of upcoming collection 🏯 fortress Ignition Fortress
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants