Replace Isometry with RigidTransform class

[mitiguy]

Drake's C++ source code currently employs an Isometry to relate the orientation and position of two frames.

Isometry stores the frames' relative orientation in a 3x3 rotation matrix and the frames' relative position in a 3x1 vector. Isometry has the following defects:

1. The name of the class is unfamiliar and unrecognizable to many roboticists and dynamicists.
2. The class does not require a valid 3x3 rotation matrix or a non-NAN position vector.
3. The class is missing functionality and "sugar".

Isometry is being replace by RigidTransform, which has the following advantages:

1. In debug versions, RigidTransform requires a valid 3x3 rotation matrix and position vector or it throws an assertion.
2. RigidTransform makes for shorter more coherent code than Isometry.
3. RigidTransform has more functionality than Isometry.
4. RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class.
5. Implementing the RigidTransform and RotationMatrix classes have caught bugs in Drake that were previously undetected.
6. RigidTransform has been "pythonized".

Related issue, Upgrade Matrix3 to RotationMatrix: #10742

[jwnimmer-tri]

Well, to be more precise, some half dozen out of maybe five dozen PRs have been filed.

[EricCousineau-TRI]

In Anzu, I'm still using a heavy mixture of Isometry3 and RigidTransform for my portion of the perception bits.

I want to use RigidTransform for the advantages mentioned above, but I still prefer Isometry3 (esp. in Python code) as it's still used throughout MultibodyPlant, and I dislike having to do excessive conversions.

Is there a way we can accelerate fixing this by permitting temporary implicit conversions in C++ + Python, and spew compile-time or run-time warnings that will turn into errors, and just convert all existing APIs within the next couple of weeks?

BTW: Minor nit, but this issue states that RigidTransform has Python bindings, but I'm not sure if that's a valid contrast as Isometry3 already has bindings?

[sherm1]

@mitiguy and @amcastro-tri also proposed a temporary implicit conversion from RigidTransform to Isometry3. I've been resisting that because I thought it would entail hidden performance cost and delay the eventual conversion. But I'll change my mind if that's better -- can you say more? Are you proposing that the implicit conversion operator should issue a deprecated warning right away?

[amcastro-tri]

My proposal was:

• Add to RigidTransform those methods from Isometry3 we don't like just for backwards compatibility: ::linear(), toIsometry3(), etc.
• Add conversion operator.
• Implement construction/assignment from Isometry3 (those might be in actually).
• Start using RigidTransform in new code (even python).
• Make the internals transition from Isometry3 to RigidTransform.
• Deprecate those methods we do not want from Isometry3 (::linear(), toIsometry3(), etc.)

Would that work?

[jwnimmer-tri]

If the overall goal is to accelerate and finish the transition, then I think the most relevant factor is who is working on it, not the particular API details. If we want to get it done more quickly, then we simply should assign developers besides @mitiguy to work on it. For what conversions or overloads we offer, that should be judged based on their nature (are the correct, usable, compatible, etc.), not waffled on based on the low PR velocity.

[sherm1]

I think the critical gating item is converting the APIs for MBP and SG from accepting and returning Isometry3s to RigidTransforms (and similary Matrix3 to RotationMatrix). If that were done would it still make sense to have implicit conversions?

[jwnimmer-tri]

Isn't the best way to answer "are implicit conversion operaions needed" to inspect our using-code and see if it would be improved by (/ would take advantage of) such conversions? I'd rather trust a PR that adds conversions and by doing so cleans up the calling code, than trust some issue-comment hypotheticals without any basis in empirical reality.

[mitiguy]

We discussed this topic at our Monday morning meeting (March 18, 2019). The consensus (as of now) was to proceed as quickly as possible to change the Isometry3 arguments to public methods/functions to RigidTransform. The utility of implicit conversions was unclear. To be continued. I'll restart changing Isometry3 to RigidTransform in SceneGraph (with Sean) and Multibody soon.

[amcastro-tri]

See spike test in #10959.

[jwnimmer-tri]

I'll propose an additional related task: Update the RigidTransform class overview API with the "justify your existence" / "contrast with Isometry3d" rationale provided in this issue's overview post.

[mitiguy]

PR #11026 is in process to address issue #11020 (Update RigidTransform doc to explain why it is better than Isometry3).

[sherm1]

10959 merge is substantial progress on this issue though much more to go.

Please everyone start using RigidTransform instead of Isometry3!

[amcastro-tri]

Now that #10959 was merged, issue #11064 was created to track the progress on migrating call sites within Drake. Looking for volunteers to address it jointly.

[tri-ltyyu]

What is the remaining work for this epic? I see #11064 and #10742 referenced that are opened still. Are these the remaining work for the scope of this issue?

[sherm1]

Are these the remaining work for the scope of this issue?

Yes. #10742 refers to RotationMatrix rather than RigidTransform but they are related because a RigidTransform contains a RotationMatrix (plus a Vector3).

[tri-ltyyu]

Putting a note that there's a need to port SG to RigidTransform. Not time critical right now, but needs to be tracked.

[tri-ltyyu]

11193 in progress but needs more help with PR. #10742 on deck. \CC @mitiguy, please provide update. Thanks!

[mitiguy]

1. Resolved HongKai's issue #10816 which does two things: a. Provided RigidTransform operator to multiply n position vectors (similar to Eigen Isometry functionality needed by Hongkai for upcoming work). b. Ensured RigidTransform operator disallows multiplication with weirdly-sized matrices -- which was previously possible due to what appears to be a bug in Eigen.

2. Landed PR# which made RigidTransform easy to use with Eigen::Translation3 in C++.

[mitiguy]

Progress update Monday May 20th -- background information only All progress last week was on updating the Jacobian interface (Issue in progress #10155), none on this issue. Merged PR#11414 12 days ago which checked off the following boxes in this issue: multibody/benchmarks/kuka_iiwa_robot/drake_kuka_iiwa_robot.h: 1 @mitiguy multibody/benchmarks/kuka_iiwa_robot/make_kuka_iiwa_model.cc: 1 @mitiguy multibody/plant/test/frame_kinematics_test.cc: 7 @mitiguy multibody/plant/test/kuka_iiwa_model_tests.h: 1 @mitiguy multibody/plant/test/multibody_plant_jacobians_test.cc: 1 @mitiguy multibody/plant/test/multibody_plant_test.cc: 11 @mitiguy

[mitiguy]

Update as of June 11, 2019: Removed instance of GetAsIsometry() in MultibodyTree::CalcPointsPositions(...) with new RigidTransform operator *, which multiplies a set of position vectors. Next steps are to remove many calls to .linear() in favor of RotationMatrix() class.

[mitiguy]

Update as of June 17, 2019: No substantial progress recently as I am working on Jacobians.

[amcastro-tri]

@mitiguy, @jwnimmer-tri just realized that we forgot to add to our checklist the deprecation of RigidTransform::linear() and sibling methods within the DRAKE_DOXYGEN_CXX in rigid_transform.h. Essentially everything here must be deprecated and ultimately removed.

[mitiguy]

I am working towards eventually deprecating .linear(). However, it is not a first priority as it is in high use in the code and Isometry is still awaiting conversion to RigidTransform in SceneGraph (via Sean, Paul, ...). The reasons for deprecating .linear() are twofold (or more). Firstly, it returns a generic Matrix3 not a RotationMatrix&. Secondly, the semantics of .linear() are poor in the context of the RigidTransform class.

[mitiguy]

Zenhub update June 24, 2019: Planning for progress this week, concurrent with work on Jacobians.

[sherm1]

@mitiguy can we close this now?

[mitiguy]

I did a grep for Isometry on the project.
Other than the attic (soon to disappear), there are about 340 instances of Isometry. The vast majority are related to Python or examples.

[EricCousineau-TRI]

Given this, can I ask what the next action item is, and who's on point for it? I'm happy to help on some of the Python examples (~25%).

[SeanCurtis-TRI]

There are a couple in geometry that are inescapable -- SceneGraph has to convert from RigidTransform to Isometry3 to talk to FCL (note: this is strictly internal and not art of the public API). In all of geometry, that accounts for six incarnations.

The one exception is SceneGraph's current dependence on PoseBundle which has an Isometry3. Ultimately, we want the dependency on PoseBundle to simply go away -- a system that strictly uses QueryObject to communicate with DrakeVisualizer. If PoseBundle were to change its contents, then SceneGraph would lose the Isometry3 as well, whether we introduce the visualization system or not.

[EricCousineau-TRI]

Ultimately, we want the dependency on PoseBundle to simply go away -- a system that strictly uses QueryObject [...]

Relates #10482

[jwnimmer-tri]

Notes that #11888 already calls out some acute uses of Isometry3 to be fixed.

[jwnimmer-tri]

I landed here because we've again had three months pass with little to no progress, and so the deprecation date is going to bump forward again another three months.

The vast majority are related to Python or examples.

I don't think that's a helpful summary. It might be factually accurate, but is is not the relevant metric to survey. Most of the pydrake uses are just mirroring whatever C++ uses exist. (So when the C++ goes away, so does the Python.) There are several novel uses of Isometry3 elsewhere.

For example, there's a whole bunch of //drake/manipulation/perception uses that need to be ported. Also the DiffIK API, bot_core integration, and some miscellany.

I've opened #13595 to measure how close we are to being able to deprecate the implicit conversions. That's an important milestone to lock in progress.

Beyond that question, for a "Priority: High" issue we really need someone to step up and create a checklist of things remaining to be ported. So far, I've been poking at it with:

find . -name bindings -prune -o -name doc -prune -o -name attic -prune -o -type f -exec grep --color -l -e Isometry3 {} + | sort

It's possible that a big part of finishing this is just deprecate dead code. For example, ./manipulation/util/bot_core_lcm_encode_decode.h:28:void EncodePose(...) is dead code that mentions Isometry3. Once we deprecation-badge it, then we wait three months and that's one less thing on the hitlist here.

[EricCousineau-TRI]

FTR I've updated Anzu to remove deprecations, which seems to indicate that all Anzu Python code has now switched from Isometry3 to RigidTransform, except for usages of DiffIk.

[RussTedrake]

I just arrived here because I'm touching DiffIK, which (as Eric noted) still has Isometry3. I know it will involve changes to drake + anzu, but if we resolve it now, I can be sure to avoid introducting Isometry3 in my course notes. @mitiguy -- perhaps you can take a look once #13877 merges?

[mitiguy]

@RussTedrake Yes, I'll take another look at what I can do to push this issue forward, some of which may be to encourage others to update the parts of the code in which they are well versed.

[sherm1]

In Drake I think the only Isometrys that ought to be RigidTransforms are just a couple of simple uses in optitrack_sender.cc and in symbolic_expression_transform_test.cc. @mitiguy do you want to fix those and then call this done? Or did I miss some?

[jwnimmer-tri]

The symbolic_expression_transform_test is a unit test for #6604. You should not change or remove the test unless you also change or remove the operator* overloads.

[jwnimmer-tri]

I think Global IK also has uses that should be changed \CC @hongkai-dai ?

https://github.com/RobotLocomotion/drake/blob/7ce35ee63c8c26813b0ad80d65febf482cac215a/multibody/inverse_kinematics/global_inverse_kinematics.h#L149-L152

---

The PoseSmoother class under manipulation/perception has Value<Isometry3> on input and output ports. We should also fix that?

[mitiguy]

I'll take a look and see what simple ones that I can change.

[jwnimmer-tri]

One year later, we still have inverse kinematics and perception-related systems using Isometry3d. I'll work on fixing those and/or farming out the fixes.

Edited to add a checklist:

• [x] #15909
• [x] #15930
  • Missed 15909 call site in ./examples/manipulation_station/differential_ik.py
  • Dead imports
    • ./multibody/parsing/process_model_directives.cc:21:using Eigen::Isometry3d;
    • ./bindings/pydrake/systems/meshcat_visualizer.py:15:from pydrake.common.eigen_geometry import Quaternion, Isometry3
    • ./bindings/pydrake/systems/test/rendering_test.py:30: Isometry3,
    • ./examples/manipulation_station/manipulation_station_hardware_interface.cc:27:using Eigen::Isometry3d;
    • ./examples/manipulation_station/end_effector_teleop_sliders.py:145: @param pose is an Isometry3.
    • ./examples/manipulation_station/end_effector_teleop_dualshock4.py:181: @param pose is an Isometry3.
    • ./examples/manipulation_station/end_effector_teleop_mouse.py:152: @param pose is an Isometry3.
  • Internal optitrack_sender use:
    • ./systems/sensors/optitrack_sender.cc:13:void PopulateRigidBody(const Eigen::Isometry3d& T_WF,
    • ./systems/sensors/optitrack_sender.cc:55: const Eigen::Isometry3d& pose = poses.value(frame.first).GetAsIsometry3();
• [ ] #15929
• [ ] #16006
• [ ] #16011
• [ ] #16007
• [ ] #16008
• [x] ~./common/eigen_types.h:148:using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;~
  • Actually this one is slightly useful (for users who are converting to/from a RigidTransform<T>) and is not any kind of real problem, so we can keep it around.

[sherm1]

@mitiguy may want to participate in nuking some of those

[jwnimmer-tri]

All done. All unwanted uses of Isometry3 are now either deprecated or fixed already, filed as pull requests, or filed as separate issues.

[sherm1]

Woo hoo!