Computes Jacobian in the root frame inside the `DifferentialInverseKinematicsAction` class #967

zoctipus · 2024-09-10T08:23:10Z

Description

This pull request fix the bug where jacobian returned by DifferentialInverseKinematicsAction._compute_fame_jacobian are not truly in robot base frame because it did not consider robot's base rotation. After the change, _compute_fame_jacobian returns the correct local frame jacobian. In addition, properties get_jacobian_w and get_jacobian_b is added to differentiate frame differences

Fixes #911

Type of change

Bug fix (non-breaking change which fixes an issue)

Screenshots

Checklist

I have run the pre-commit checks with ./isaaclab.sh --format
I have made corresponding changes to the documentation
My changes generate no new warnings
I have added tests that prove my fix is effective or that my feature works
I have updated the changelog and the corresponding version in the extension's config/extension.toml file
I have added my name to the CONTRIBUTORS.md or my name already exists there

Mayankm96 · 2024-09-10T11:19:47Z

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py

+    @property
+    def get_jacobian_w(self) -> torch.Tensor:
+        return self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._joint_ids]
+
+    @property
+    def get_jacobian_b(self) -> torch.Tensor:
+        jacobian =  self.get_jacobian_w
+        base_rot = self._asset.data.root_quat_w
+        base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot))
+        jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :])
+        jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :])
+        return jacobian


Is there a reason to make these properties? If it is only used at one place then you can just keep it there?

jtigue suggested that the physx's get_jacobians() is not clear that the returned is in world frame, and keep it there solves the issue but introduced unstated feature, so I instead used property. I understand that use property in only one place is a bit overkill, and I can change it back if that makes more sense

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py

Mayankm96

@zoctipus Thanks a lot for the fix!

Could the unit test here be adapted to make sure the feature works as expected?

https://github.com/isaac-sim/IsaacLab/blob/main/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py

Also, you don't need to post a screenshot of the git diff. The screenshot is only for showing results in behavior (not code).

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst

zoctipus · 2024-09-22T22:36:54Z

@Mayankm96
Thanks for Reviewing, and sorry for the delay, my last week schedule was overwhelming.

I have added tests for checking the implement, all tests are passing!
however, there are some concerns that worries me:

the test is only for class:DifferentialIKController but not for class:DifferentialInverseKinematicsAction. DifferentialIKController doesn't calculates Jacobian but depends on Jacobian calculated by either physx or DifferentialInverseKinematicsAction. This bug fix in DifferentialInverseKinematicsAction, namely base frame's jacobian transformation formula, is not referenced by the test suite because test suite didn't use DifferentialInverseKinematicsAction. Instead this formula is re-implemented to make sure the base-frame-transformation is applied before given to DifferentialIKController. This worries me because there are two implementations of same formulat, one in unittest and the other one in DifferentialInverseKinematicsAction, and if at some day one formula changes, the other will not be sync and causing invalid test case or bugs. Perhaps there needs to be a test suite that specifically test for DifferentialInverseKinematicsAction?
There are two unit tests for DifferentialIKController in https://github.com/isaac-sim/IsaacLab/blob/main/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py , one for franka, and the other for UR10. UR 10 test is failing because UR10 is not able to be imported properly. Specifically,

# spawn single instance
prim = func(prim_paths[0], cfg, *args, **kwargs)

Will not populates UR10's links and joints into stage. Only an empty /World/envs/env_0/Robot will be created

Upon further investigation, it seems like this is an issue when ISAACLAB_NUCLEUS_DIR points to 4.2. Changing 4.2 to 4.1 resolves import issue.

Buggy usd path:
'http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.2/Isaac/IsaacLab/Robots/UniversalRobots/UR10/ur10_instanceable.usd'

OK usd path:
'http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.1/Isaac/IsaacLab/Robots/UniversalRobots/UR10/ur10_instanceable.usd'

with OK usd path, both tests can pass

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py

zoctipus · 2024-09-26T21:40:53Z

Thanks for the fix!

zoctipus · 2024-10-20T21:09:49Z

I just rebased the branch to track the newest Isaac Iab and fixed the versioning,
Please have a view and let me know what else I can fix!
@lorenwel @Mayankm96 @jtigue-bdai
Thanks for your guys help!

…bot.

zoctipus · 2024-10-29T09:38:42Z

I have updated versioning in CHANGE LOG and extension.toml, please take a look when you have time! Thanks! @jsmith-bdai @Mayankm96

jtigue-bdai

Thanks for the fix, sorry this took so long to get looked at.

Signed-off-by: Octi Zhang <[email protected]>

Signed-off-by: Kelly Guo <[email protected]>

…nematicsAction` class (isaac-sim#967) # Description This pull request fix the bug where jacobian returned by DifferentialInverseKinematicsAction._compute_fame_jacobian are not truly in robot base frame because it did not consider robot's base rotation. After the change, _compute_fame_jacobian returns the correct local frame jacobian. In addition, properties get_jacobian_w and get_jacobian_b is added to differentiate frame differences Fixes isaac-sim#911 ## Type of change - Bug fix (non-breaking change which fixes an issue) ## Screenshots <img width="1109" alt="image" src="https://github.com/user-attachments/assets/5d47dfb3-8c44-4115-b5a0-28c671e99459"> ## Checklist - [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [ ] I have made corresponding changes to the documentation - [x] My changes generate no new warnings - [ ] I have added tests that prove my fix is effective or that my feature works - [x] I have updated the changelog and the corresponding version in the extension's `config/extension.toml` file - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there  Signed-off-by: Octi Zhang <[email protected]> Signed-off-by: Kelly Guo <[email protected]> Co-authored-by: Kelly Guo <[email protected]>

zoctipus requested review from Dhoeller19, Mayankm96, jsmith-bdai and kellyguo11 as code owners September 10, 2024 08:23

Mayankm96 reviewed Sep 10, 2024

View reviewed changes

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py Outdated Show resolved Hide resolved

Mayankm96 requested changes Sep 10, 2024

View reviewed changes

Mayankm96 reviewed Sep 10, 2024

View reviewed changes

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst Outdated Show resolved Hide resolved

Mayankm96 changed the title ~~added base rotation to jacobian when compute_fame_jacobian in DifferentialInverseKinematicsAction~~ Computes Jacobian in the root frame inside the DifferentialInverseKinematicsAction class Sep 10, 2024

Mayankm96 added the bug Something isn't working label Sep 10, 2024

ozhanozen mentioned this pull request Sep 13, 2024

Adds OperationSpaceController to docs and tests and implement corresponding action/action_cfg classes #913

Merged

6 tasks

Mayankm96 assigned zoctipus Sep 19, 2024

lorenwel suggested changes Sep 26, 2024

View reviewed changes

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py Outdated Show resolved Hide resolved

zoctipus requested a review from Mayankm96 October 20, 2024 20:42

zoctipus requested review from hhansen-bdai, jtigue-bdai, masoudmoghani, nikitardn and pascal-roth as code owners October 20, 2024 20:43

zoctipus force-pushed the main branch from 397ba26 to a12dc59 Compare October 20, 2024 21:03

use the geometric jacobian computed w.r.t to the root frame of the ro…

a8f3a2f

…bot.

zoctipus force-pushed the main branch from fa4c86e to a8f3a2f Compare October 29, 2024 04:40

jtigue-bdai approved these changes Dec 4, 2024

View reviewed changes

Merge branch 'main' into main

6f6975e

Signed-off-by: Octi Zhang <[email protected]>

jtigue-bdai mentioned this pull request Dec 6, 2024

[Bug Report] Ik Absolute seems to not work properly with rotated base frame #911

Closed

3 tasks

Merge branch 'main' into main

27029fe

Signed-off-by: Kelly Guo <[email protected]>

kellyguo11 merged commit 762f4e3 into isaac-sim:main Dec 12, 2024
5 checks passed

amatsukaze233 mentioned this pull request Apr 15, 2025

[Bug Report] Issue with DifferentialIKController Not Working After Adding rot to InitialStateCfg #1433

Closed

4 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Computes Jacobian in the root frame inside the `DifferentialInverseKinematicsAction` class #967

Computes Jacobian in the root frame inside the `DifferentialInverseKinematicsAction` class #967

Uh oh!

zoctipus commented Sep 10, 2024

Uh oh!

Mayankm96 Sep 10, 2024

Uh oh!

zoctipus Sep 12, 2024

Uh oh!

Uh oh!

Mayankm96 left a comment •

edited

Loading

Uh oh!

Uh oh!

zoctipus commented Sep 22, 2024 •

edited

Loading

Uh oh!

Uh oh!

zoctipus commented Sep 26, 2024

Uh oh!

zoctipus commented Oct 20, 2024

Uh oh!

zoctipus commented Oct 29, 2024

Uh oh!

jtigue-bdai left a comment •

edited

Loading

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

Computes Jacobian in the root frame inside the DifferentialInverseKinematicsAction class #967

Computes Jacobian in the root frame inside the DifferentialInverseKinematicsAction class #967

Uh oh!

Conversation

zoctipus commented Sep 10, 2024

Description

Type of change

Screenshots

Checklist

Uh oh!

Mayankm96 Sep 10, 2024

Choose a reason for hiding this comment

Uh oh!

zoctipus Sep 12, 2024

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Mayankm96 left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

zoctipus commented Sep 22, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

zoctipus commented Sep 26, 2024

Uh oh!

zoctipus commented Oct 20, 2024

Uh oh!

zoctipus commented Oct 29, 2024

Uh oh!

jtigue-bdai left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

Computes Jacobian in the root frame inside the `DifferentialInverseKinematicsAction` class #967

Computes Jacobian in the root frame inside the `DifferentialInverseKinematicsAction` class #967

Mayankm96 left a comment •

edited

Loading

zoctipus commented Sep 22, 2024 •

edited

Loading

jtigue-bdai left a comment •

edited

Loading