Add inverse kinematics example #538

fantaosha · 2023-06-20T23:21:37Z

Motivation and Context

How Has This Been Tested

Types of changes

Docs change / refactoring / dependency upgrade
Bug fix (non-breaking change which fixes an issue)
New feature (non-breaking change which adds functionality)
Breaking change (fix or feature that would cause existing functionality to change)

Checklist

My code follows the code style of this project.
My change requires a change to the documentation.
I have updated the documentation accordingly.
I have read the CONTRIBUTING document.
I have completed my CLA (see CONTRIBUTING)
I have added tests to cover my changes.
All new and existing tests passed.

luisenp

Thanks for working on this! Left some comments.

Besides addressing this, I think we should include comments to guide people throughout the example. The scripts in this folder are meant for people to learn how to use theseus.

examples/labs/inverse_kinematics.py

luisenp · 2023-06-21T16:48:55Z

examples/labs/inverse_kinematics.py

+theta = torch.rand(10, robot.dof, dtype=dtype)
+targeted_poses_ee: torch.Tensor = fk(theta)[0]
+theta = torch.zeros_like(theta)
+for iter in range(50):


We should be able to use theseus for this, right? We can show how to create a custom cost function with some vector optim vars for theta and wrap fk and jfk computation. Then we just add to objective and optimize.

This cost function could even be added to theseus.embodied once we merge FK from labs.

As discussed offline, let's keep the current version and focus on adding more explanations and references. We can remove the CostFunction for now, but keep the code around somewhere, in case we have time to add it and clean it up properly soon.

luisenp · 2023-06-21T16:55:25Z

examples/labs/inverse_kinematics.py

+theta = torch.zeros_like(theta)
+for iter in range(50):
+    jac_b, poses = jfk_b(theta)
+    error = SE3.log(SE3.compose(SE3.inv(poses[-1]), targeted_poses_ee)).view(-1, 6, 1)


I think this can be made more concise if we use LieTensor class, which has a local() implementation.

I will add local() to lie.functional. See #542

examples/labs/inverse_kinematics.py

tests/labs/embodied/robot/test_forward_kinematics.py

examples/labs/inverse_kinematics.py

theseus/labs/embodied/robot/forward_kinematics.py

mhmukadam · 2023-06-23T20:34:54Z

examples/labs/inverse_kinematics.py

+    (theta,) = optim_vars
+    (targeted_pose,) = aux_vars
+    pose = th.SE3(tensor=fk(theta.tensor)[0])
+    return (pose.inverse().compose(targeted_pose)).log_map()


we can call targeted_pose.local(pose)?

Is it pose.local(targeted_pose)? In either case, agree that we should call local().

That's what we have in the Local cost function in /embodied/misc.

In the longer term we should actually make a unified version of this cost function that can optionally call in to FK and apply local on pose/point/rot of a specific joint.

I think the right way to do this would be this, which unfortunately we don't have time to implement, but it would be very nice to have.

mhmukadam

LGTM

luisenp

Code looks good, but left some minor comments. I think the most important is to use this example to clearly explain the API, because we don't have any documentation for this code.

luisenp · 2023-06-23T22:36:12Z

examples/labs/inverse_kinematics.py

+# the selected links, in that order. The return types of these functions are as
+# follows:
+#
+# - fk: return poses of selected links


I think we should be more explicit here, this description doesn't help the reader understand the API.

For example, fk returns N + 1 outputs, where N is the number of selected links. The first N correspond to poses (tensors, I think?). And I think the last output is a list containing poses for all links? It would be really useful to include this level of detail.

Same goes for the jacobians.

No, fk will only return the tuple of selected link poses.

Ah, my bad, I missed this slicing for fk function. Sorry about that. You should still mention that the return is a tuple of pose tensors, one for each selected link, in the same order as the link names given as input.

Also, can you reword the jacobians part so it says "returns a tuple where the first element is a list with the body jacobians of selected links, and the second element is a tuple with the corresponding poses."?

luisenp · 2023-06-23T22:37:37Z

examples/labs/inverse_kinematics.py

+    (theta,) = optim_vars
+    (targeted_pose,) = aux_vars
+    pose = th.SE3(tensor=fk(theta.tensor)[0])
+    return (pose.inverse().compose(targeted_pose)).log_map()


Is it pose.local(targeted_pose)? In either case, agree that we should call local().

fantaosha added the enhancement New feature or request label Jun 20, 2023

fantaosha requested review from luisenp and mhmukadam June 20, 2023 23:21

fantaosha self-assigned this Jun 20, 2023

facebook-github-bot added the CLA Signed This label is managed by the Facebook bot. Authors need to sign the CLA before a PR can be reviewed. label Jun 20, 2023

luisenp force-pushed the lep.suddhus_fork_changes branch from 8da0591 to 8a1d55a Compare June 21, 2023 14:43

Base automatically changed from lep.suddhus_fork_changes to main June 21, 2023 14:44

fantaosha force-pushed the taoshaf.ik_example branch from 31a5014 to db873bc Compare June 21, 2023 14:56

luisenp requested changes Jun 21, 2023

View reviewed changes

fantaosha requested a review from luisenp June 21, 2023 22:12

fantaosha force-pushed the taoshaf.ik_example branch from 67af5f8 to c1959ef Compare June 23, 2023 20:28

mhmukadam reviewed Jun 23, 2023

View reviewed changes

mhmukadam approved these changes Jun 23, 2023

View reviewed changes

luisenp requested changes Jun 23, 2023

View reviewed changes

luisenp force-pushed the taoshaf.ik_example branch from d69e976 to 961e595 Compare June 24, 2023 02:51

fantaosha force-pushed the taoshaf.ik_example branch from 961e595 to 204bc00 Compare June 26, 2023 02:19

fantaosha requested a review from luisenp June 26, 2023 02:20

fantaosha force-pushed the taoshaf.ik_example branch from 204bc00 to 0c42338 Compare June 27, 2023 14:59

fantaosha changed the base branch from main to lep.graduate_fk_labs June 27, 2023 15:11

Base automatically changed from lep.graduate_fk_labs to main June 27, 2023 18:16

luisenp and others added 7 commits June 27, 2023 11:32

Added more explanations to example and cleanup some of the code.

89b4b08

add more comments

31449e2

Fix torchlie import.

f9bb5f4

use local for losses

13d5f5e

add detailed descriptions in the examples

fb3dffa

List->Tuple to fix test bugs

b6c0e3e

minor revision of comments

ece2df5

fantaosha force-pushed the taoshaf.ik_example branch from 9948b6c to ece2df5 Compare June 27, 2023 18:33

fantaosha added 2 commits June 27, 2023 11:35

update comments

ee52fba

move inverse_kinematics out of labs

5b461f9

fantaosha merged commit d7f898c into main Jun 27, 2023

fantaosha deleted the taoshaf.ik_example branch June 27, 2023 18:38

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add inverse kinematics example #538

Add inverse kinematics example #538

fantaosha commented Jun 20, 2023

luisenp left a comment

luisenp Jun 21, 2023

luisenp Jun 21, 2023

luisenp Jun 21, 2023

fantaosha Jun 21, 2023

mhmukadam Jun 23, 2023

luisenp Jun 23, 2023

mhmukadam Jun 23, 2023 •

edited

Loading

luisenp Jun 24, 2023

mhmukadam left a comment

luisenp left a comment

luisenp Jun 23, 2023

fantaosha Jun 26, 2023

luisenp Jun 26, 2023

luisenp Jun 23, 2023

Add inverse kinematics example #538

Add inverse kinematics example #538

Conversation

fantaosha commented Jun 20, 2023

Motivation and Context

How Has This Been Tested

Types of changes

Checklist

luisenp left a 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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mhmukadam Jun 23, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mhmukadam left a comment

Choose a reason for hiding this comment

luisenp left a 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

Choose a reason for hiding this comment

mhmukadam Jun 23, 2023 •

edited

Loading