MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers #22698

mitiguy · 2025-03-04T01:24:05Z

This is the first PR of several to help address issue #22630. It creates MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers.

Subsequent PRs will create helper methods for more complex mobilizers (e.g., rpy_ball_mobilizer, curvilinear_mobilizer, quaternion_floating_mobilizer, etc.) to consolidate creation of the N(q) and N⁺(q) matrices that is shared between MapQDotToVelocity() and MapQDDotToAcceleration() as well as MapVelocityToQDDot() and MapAccelerationToQDDot().

In conjunction with each of the subsequent PRs, each more complex mobilizer will implement its functions MapQDDotToAcceleration() and MapAccelerationToQDDot().

FYI: Since mobilizers are Drake internal classes, after the internal mobilizer work is complete, there will be PRs (code and testing) for the public API in MultibodyPlant to address issue #22630.

This change is

mitiguy

Reviewable status: 2 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

a discussion (no related file):
Feature review +@sherm1

multibody/tree/mobilizer.h line 618 at r1 (raw file):

                                 EigenPtr<VectorX<T>> v) const = 0;

  // Uses the kinematic mapping `q̈ = N(q)⋅v̇` to calculate q̈ from v̇.

Self-blocking. Remove tic-marks around q̈ = N(q)⋅v̇ here and below.

sherm1

PR description should explain what's here and what's not (with what's planned to follow) and give some rationale.

Feature pending some comment cleanups

Reviewed 16 of 16 files at r1, all commit messages.
Reviewable status: 8 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/mobilizer.h line 618 at r1 (raw file):

                                 EigenPtr<VectorX<T>> v) const = 0;

  // Uses the kinematic mapping `q̈ = N(q)⋅v̇` to calculate q̈ from v̇.

missing term in comment

Suggestion:

q̈=N(q)⋅v̇ + Ṅ(q,v)⋅v to calculate q̈ from v and v̇

multibody/tree/mobilizer.h line 621 at r1 (raw file):

  // @param[in] vdot 1ˢᵗ time derivatives of generalized velocities (v̇).
  // @param[out] qddot 2ⁿᵈ time derivatives of the generalized positions (q̈).
  // Note: Generalized positions and velocities are stored in `context`.

BTW consider naming the variables to match the equations

Suggestion:

positions q and velocities v

multibody/tree/mobilizer.h line 628 at r1 (raw file):

                                      EigenPtr<VectorX<T>> qddot) const;

  // Uses the kinematic mapping `v̇ = N(q)⋅q̈` to calculate v̇ from q̈.

Missing term and wrong matrix!

Per f2f we should write the equations in terms of what the user is supplying in the arguments to these routines: q,v from the context, qddot or vdot as a separate argument. (They never supply qdot)

Suggestion:

v̇ = N⁺(q)⋅q̈ + Ṅ⁺(q,v)⋅q̇ where q̇ = N(q)v

multibody/tree/mobilizer.h line 629 at r1 (raw file):

  // Uses the kinematic mapping `v̇ = N(q)⋅q̈` to calculate v̇ from q̈.
  // @param[in] qddot 2ⁿᵈ time derivatives of the generalized positions (q̈).

BTW consider disambiguating (I thought at first the symbol was supposed to be generalized positions since that's what it was next to)
(same elsewhere)

Suggestion:

qddot 2ⁿᵈ time derivatives q̈ of the generalized positions q.

multibody/tree/mobilizer.h line 636 at r1 (raw file):

  virtual void MapQDDotToAcceleration(const systems::Context<T>& context,
                                      const Eigen::Ref<const VectorX<T>>& qddot,
                                      EigenPtr<VectorX<T>> v) const;

nit: v -> vdot

multibody/tree/prismatic_mobilizer.h line 184 at r1 (raw file):

  // Computes the kinematic mapping from generalized velocities v to time
  // derivatives of the generalized positions `q̇`. For this mobilizer `q̇ = v`.

BTW you didn't clean up the old comments here to match the new ones as you did for planar & screw

multibody/tree/revolute_mobilizer.h line 187 at r1 (raw file):

  bool is_velocity_equal_to_qdot() const override { return true; }

  void MapVelocityToQDot(const systems::Context<T>& context,

BTW you didn't add old comments here to match the new ones like in planar & screw

sherm1

Should be release notes: none since this is all internal. It's not a real feature until users can get to it through the public API (Joint).

Reviewable status: 8 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

mitiguy

@sherm1 Thanks for the review. PTAL after it (hopefully) passes CI.

Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/mobilizer.h line 618 at r1 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Self-blocking. Remove tic-marks around q̈ = N(q)⋅v̇ here and below.

Done.

multibody/tree/mobilizer.h line 618 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

missing term in comment

Done with slightly different wording.

multibody/tree/mobilizer.h line 621 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider naming the variables to match the equations

Done with slightly different wording.

multibody/tree/mobilizer.h line 628 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Missing term and wrong matrix!

Per f2f we should write the equations in terms of what the user is supplying in the arguments to these routines: q,v from the context, qddot or vdot as a separate argument. (They never supply qdot)

Done with slightly different wording.

multibody/tree/mobilizer.h line 629 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider disambiguating (I thought at first the symbol was supposed to be generalized positions since that's what it was next to)
(same elsewhere)

Done with slightly different wording.

multibody/tree/mobilizer.h line 636 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: v -> vdot

Done.

multibody/tree/prismatic_mobilizer.h line 184 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW you didn't clean up the old comments here to match the new ones as you did for planar & screw

Done.

multibody/tree/revolute_mobilizer.h line 187 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW you didn't add old comments here to match the new ones like in planar & screw

Done.

sherm1

The PR description only talks about mobilizers, which are internal. The original issue #22630 needs a public API in MultibodyPlant. These first PRs are just internal, but there should be some mention of the end game.

Feature

+@xuchenhan-tri for platform review per rotation, please

Reviewed 3 of 3 files at r2, all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee xuchenhan-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/mobilizer.h line 618 at r2 (raw file):

                                 EigenPtr<VectorX<T>> v) const = 0;

  // Calculates q̈ from v̇ and v using q̈ = N(q)⋅v̇ + Ṅ(q,v)⋅v.

BTW should list q as well (below also)

Suggestion:

Calculates q̈ from v̇, v, and q

multibody/tree/revolute_mobilizer.h line 187 at r2 (raw file):

  bool is_velocity_equal_to_qdot() const override { return true; }

  // Maps v to qdot, which for this mobilizer is q̇ = v̇.

minor: should be v rather than vdot

multibody/tree/prismatic_mobilizer.h line 183 at r2 (raw file):

  bool is_velocity_equal_to_qdot() const override { return true; }

  // Maps v to qdot, which for this mobilizer is q̇ = v̇.

minor: should be v rather than vdot

Suggestion:

q̇ = v

xuchenhan-tri

platform

Reviewed 13 of 16 files at r1, 3 of 3 files at r2, all commit messages.
Reviewable status: 5 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/planar_mobilizer.cc line 192 at r2 (raw file):

    const systems::Context<T>&, const Eigen::Ref<const VectorX<T>>& vdot,
    EigenPtr<VectorX<T>> qddot) const {
  DRAKE_ASSERT(vdot.size() == kNv);

BTW, it looks like these validity checks are spread across all derived classes. Is it possible to do this in MobilizerImpl?

…virtual when all subclasses have implemented the virtual function in mobilizer.h.

…le mobilizers.

sherm1

Reviewed 6 of 6 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

mitiguy

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

multibody/tree/mobilizer.h line 618 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW should list q as well (below also)

Done.

multibody/tree/planar_mobilizer.cc line 192 at r2 (raw file):

Previously, xuchenhan-tri wrote…

BTW, it looks like these validity checks are spread across all derived classes. Is it possible to do this in MobilizerImpl?

Sounds reasonable. I'll check with you and Sherm before the next PR. These validity checks will be repeated for the more complicated mobilizers. For this PR, I think it is OK to merge as-is as it mimics the current code for MapVelocityToQDot() and MapQDotToVelocity(). If we make the change, I want to change all 4 functions simultaneously in a super-simple PR.

multibody/tree/prismatic_mobilizer.h line 183 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: should be v rather than vdot

Done here and three other places.

multibody/tree/revolute_mobilizer.h line 187 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: should be v rather than vdot

Done here and 3 other places.

xuchenhan-tri

Reviewed 6 of 6 files at r3, all commit messages.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

xuchenhan-tri

Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/prismatic_mobilizer.h line 183 at r2 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Done here and three other places.

The reviewable coloring is failing so miserably here :(

mitiguy

+label squashing now

Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

mitiguy

+label status: squashing now

Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

mitiguy added priority: medium status: do not review release notes: feature This pull request contains a new feature labels Mar 4, 2025

mitiguy force-pushed the MapQDDotToVDotMobilizers branch from eec887b to f300ad0 Compare March 4, 2025 22:25

mitiguy removed the status: do not review label Mar 4, 2025

mitiguy changed the title ~~[WIP] Mobilizer functionality for MapQddotToVdot and vice-versa.~~ MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers Mar 4, 2025

mitiguy assigned sherm1 Mar 4, 2025

mitiguy commented Mar 4, 2025

View reviewed changes

sherm1 reviewed Mar 5, 2025

View reviewed changes

sherm1 added release notes: none This pull request should not be mentioned in the release notes and removed release notes: feature This pull request contains a new feature labels Mar 5, 2025

mitiguy commented Mar 5, 2025

View reviewed changes

sherm1 assigned xuchenhan-tri Mar 5, 2025

sherm1 reviewed Mar 5, 2025

View reviewed changes

xuchenhan-tri reviewed Mar 6, 2025

View reviewed changes

Paul Mitiguy added 5 commits March 6, 2025 15:10

Mobilizer functionality for MapQddotToVdot and vice-versa.

9eca67f

Added a reminder TODO to change new functions in mobilizer.h to pure …

1ff99e2

…virtual when all subclasses have implemented the virtual function in mobilizer.h.

Create MapQDDotToAcceleration() and MapAccelerationToQDDot() for simp…

77080ec

…le mobilizers.

Respond to Sherm's comments.

850da93

Respond to reviewer comments.

11b4449

mitiguy force-pushed the MapQDDotToVDotMobilizers branch from 50431cd to 11b4449 Compare March 7, 2025 00:52

sherm1 reviewed Mar 7, 2025

View reviewed changes

mitiguy commented Mar 7, 2025

View reviewed changes

xuchenhan-tri reviewed Mar 7, 2025

View reviewed changes

mitiguy added status: squashing now https://drake.mit.edu/reviewable.html#curated-commits and removed status: squashing now https://drake.mit.edu/reviewable.html#curated-commits labels Mar 7, 2025

mitiguy commented Mar 7, 2025

View reviewed changes

mitiguy added the status: squashing now https://drake.mit.edu/reviewable.html#curated-commits label Mar 7, 2025

mitiguy merged commit a513002 into RobotLocomotion:master Mar 7, 2025
10 checks passed

mitiguy deleted the MapQDDotToVDotMobilizers branch March 7, 2025 22:01

mitiguy mentioned this pull request Mar 12, 2025

[WIP] Create MapQDDotToAcceleration() for RPY ball mobilizer. #22741

Open

mitiguy mentioned this pull request May 14, 2025

Correct and improve rpy_ball_mobilizer documentation, replace E matrix with standard N matrix. #23006

Merged

mitiguy mentioned this pull request Jun 11, 2025

MapQDDotToAcceleration() and MapAccelerationToQDDot() for rpy mobilizer. #23092

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers #22698

MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers #22698

Uh oh!

mitiguy commented Mar 4, 2025 •

edited by sherm1

Loading

Uh oh!

mitiguy left a comment

Uh oh!

sherm1 left a comment

Uh oh!

sherm1 left a comment

Uh oh!

mitiguy left a comment

Uh oh!

sherm1 left a comment

Uh oh!

xuchenhan-tri left a comment

Uh oh!

sherm1 left a comment

Uh oh!

mitiguy left a comment

Uh oh!

xuchenhan-tri left a comment

Uh oh!

xuchenhan-tri left a comment

Uh oh!

mitiguy left a comment

Uh oh!

mitiguy left a comment

Uh oh!

Uh oh!

Uh oh!

MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers #22698

MapQDDotToAcceleration() and MapAccelerationToQDDot() for simple mobilizers #22698

Uh oh!

Conversation

mitiguy commented Mar 4, 2025 • edited by sherm1 Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

xuchenhan-tri left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

xuchenhan-tri left a comment

Choose a reason for hiding this comment

Uh oh!

xuchenhan-tri left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

mitiguy commented Mar 4, 2025 •

edited by sherm1

Loading