Linearize() throws error when using kSap solver

[threebrooks]

What happened?

I created a simple model that contains 2 joints, one of which is using drake:mimic to tie it to the other joint. Because of the use of drake:mimic I am setting the kSap solver. When calling Linearize() on that model, I am getting an error

RuntimeError: SapSolver::SolveWithGuess(): Only T = double is supported when the set of constraints is non-empty.

From what I could gather from the code, this would indicate the Solver isn't of type double, but I don't see how that could be the case.

----------- SDF ---------------

<?xml version="1.0" ?>
<sdf version="1.9" xmlns:drake="http://drake.mit.edu">
  <model name="model">
    <link name="box1">
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.1 0.3</size>
          </box>
        </geometry>
        <pose>0.2 0 0 0 0 0</pose>
      </visual>
    </link>
    <joint name="world_to_box1" type="continuous">
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
      <parent>world</parent>
      <child>box1</child>
    </joint>
    <link name="box2">
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.1 0.3</size>
          </box>
        </geometry>
        <pose>-0.2 0 0 0 0 0</pose>
      </visual>
    </link>
    <joint name="world_to_box2" type="continuous">
      <drake:mimic joint="world_to_box1" multiplier="-5.0" offset="0.0"/>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
      <parent>world</parent>
      <child>box2</child>
    </joint>
  </model>
</sdf>

------ Python -------

import sys
import numpy as np
from pydrake.all import (DiscreteContactApproximation, DiagramBuilder, MultibodyPlant, Parser, Linearize)

builder = DiagramBuilder()
plant = MultibodyPlant(time_step=0.01)
builder.AddSystem(plant)
plant.set_discrete_contact_approximation(DiscreteContactApproximation.kSap)
Parser(plant).AddModels(sys.argv[1])
plant.Finalize()

builder.ExportInput(plant.get_actuation_input_port(), "input")

diagram = builder.Build()

wrapper_builder = DiagramBuilder()
wrapper_plant = wrapper_builder.AddSystem(diagram)

context = diagram.CreateDefaultContext()
wrapper_plant.get_input_port().FixValue(context, np.zeros(wrapper_plant.get_input_port().size()))

linear_sys = Linearize(wrapper_plant, context)

Version

1.24.0

What operating system are you using?

Ubuntu 20.04

What installation option are you using?

No response

Relevant log output

No response

[jwnimmer-tri]

From what I could gather from the code, this would indicate the Solver isn't of type double, but I don't see how that could be the case.

The mechanism that Linearize() uses on the plant system is to evaluate its time step update using AutoDiff (see here). Therefore, it must be able to calculate the step using MultibodyPlant_[AutoDiffXd].

As I understand it, lack of AutoDiffXd-typed dynamics when using SAP a known limitation, so I'll re-categorize this as a feature request instead of a bug.

I'll need to defer to a dynamics team member to explain if there are any tricks you could use to work around the lack of gradient support in the SAP solver. I wonder if (under linearization) there is an equivalent model that doesn't need the constraint.

[RussTedrake]

The solution inside multibodyplant is, I think, non-trivial. Discrete-time plants are solving an optimization on each time step (including constraints from mimic joints, etc). The right way to take a gradient through that using the implicit function theorem... NOT passing autodiff through the solver.

Probably a better way to do this is to linearize the unconstrained dynamics and the constriant separately, and then operate on the manifold of the linearized constraint. See https://underactuated.mit.edu/lqr.html#equality_constrained for some notes.

[threebrooks]

Thanks @jwnimmer-tri , @RussTedrake for the response. In my specific case I was actually trying to use it to simulate a gearbox to the robot's motor (to make sure simulation and actual robot are in sync). The above SDF and code was just a minimal example to reproduce the issue . Given the inherent SAP limitation mentioned above, I might actually just deal with the gear ratio someplace else in the code. That said, I will also try to wrap my brain around your lecture notes, @RussTedrake :)