Skip to content

Make getJacobian simpler and faster#2389

Merged
sjahr merged 4 commits intomoveit:mainfrom
marioprats:jacobian_benchmark
Sep 27, 2023
Merged

Make getJacobian simpler and faster#2389
sjahr merged 4 commits intomoveit:mainfrom
marioprats:jacobian_benchmark

Conversation

@marioprats
Copy link
Copy Markdown
Contributor

@marioprats marioprats commented Sep 26, 2023

Description

This change makes getJacobian a little bit more readable (hopefully!), while also making it 2x faster, by removing string comparisons where possible and other little optimizations.
The jacobian benchmark shows a ~2x improvement with respect to HEAD (>500ns), and ~4x with respect to the KDL Jacobian, for the panda robot:

------------------------------------------------------------
Benchmark                  Time             CPU   Iterations
------------------------------------------------------------
BM_MoveItJacobian        248 ns          249 ns      2867707
BM_KDLJacobian          1099 ns         1100 ns       634762

The recently added Jacobian tests for the different types of joints are also passing.
Also manually tested this with Servo, which I know uses the MoveIt! Jacobian.

Note: a_pose_b is a notation I have used in the past to denote a transformation matrix to convert points in b to points in a, e.g: point_a = a_pose_b * point_b. Not sure if there's another established notation if MoveIt!. Happy to change to whatever makes more sense.

Checklist

  • Required by CI: Code is auto formatted using clang-format
  • Extend the tutorials / documentation reference
  • Document API changes relevant to the user in the MIGRATION.md notes
  • Create tests, which fail without this PR reference
  • Include a screenshot if changing a GUI
  • While waiting for someone to review your request, please help review another open pull request to support the maintainers

sea-bass
sea-bass reviewed Sep 26, 2023
View reviewed changes
Copy link
Copy Markdown
Contributor

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

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

Very cool!

@codecov
Copy link
Copy Markdown

codecov bot commented Sep 26, 2023 •
edited
Loading

Codecov Report

Attention: 8 lines in your changes are missing coverage. Please review.

Comparison is base (cc75143) 50.74% compared to head (eee6ffe) 50.77%.
Report is 2 commits behind head on main.

❗ Current head eee6ffe differs from pull request most recent head 9da0948. Consider uploading reports for the commit 9da0948 to get more accurate results

Additional details and impacted files 
@@            Coverage Diff             @@
##             main    #2389      +/-   ##
==========================================
+ Coverage   50.74%   50.77%   +0.03%     
==========================================
  Files         386      386              
  Lines       31971    31972       +1     
==========================================
+ Hits        16221    16230       +9     
+ Misses      15750    15742       -8
Files Coverage Δ
...bot_state/include/moveit/robot_state/robot_state.h 90.27% <ø> (+0.14%) ⬆️
moveit_core/robot_state/src/robot_state.cpp 51.28% <84.32%> (+0.78%) ⬆️

... and 4 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@marioprats marioprats requested a review from sea-bass September 27, 2023 06:52
sjahr
sjahr reviewed Sep 27, 2023
View reviewed changes
Copy link
Copy Markdown
Contributor

@sjahr sjahr left a comment

Choose a reason for hiding this comment

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

Do you mind adding some comments in front of the main blocks to explain what is happening there? I think that will help maintaining this in the future

@marioprats
Copy link
Copy Markdown
Contributor Author

marioprats commented Sep 27, 2023

Thanks for the reviews.
While going again through the code adding comments I realized of an issue when the joint group origin doesn't match the URDF origin. Added a test for it and should be ok now.

sea-bass
sea-bass approved these changes Sep 27, 2023
View reviewed changes
@sjahr @sea-bass
Update moveit_core/robot_state/src/robot_state.cpp
9da0948 
Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
@sjahr sjahr enabled auto-merge (squash) September 27, 2023 13:08
@sjahr sjahr merged commit f998962 into moveit:main Sep 27, 2023
m-elwin pushed a commit to m-elwin/moveit2 that referenced this pull request Dec 4, 2023
@marioprats @m-elwin
Make getJacobian simpler and faster (moveit#2389)
bc359b7 
* Make getJacobian simpler and faster

* readability and const-correctness

* fix issue when joint group is not at URDF origin

* Update moveit_core/robot_state/src/robot_state.cpp
@marioprats marioprats mentioned this pull request Dec 22, 2023
Merged
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@sjahr sjahr sjahr approved these changes

@henningkayser henningkayser Awaiting requested review from henningkayser

+1 more reviewer

@sea-bass sea-bass sea-bass approved these changes

Reviewers whose approvals may not affect merge requirements

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

3 participants

@marioprats @sea-bass @sjahr