Add ResolveConstraints logic to JointSliders (=> ModelVisualizer)

Updates JointSliders to have discrete state (previously it was
stateless). Additionally, if the new ResolveConstraints logic is
active, we publish the (rounded) resolved constraints back to Meshcat,
so that the other sliders are updated.

Since ModelVisualizer uses JointSliders, now ModelVisualizer also
satisfies the multibody constraints.

Resolves #18917.

Deprecates `JointSliders::SetPositions(q)`, which has been replaced with
`JointSliders::SetPositions(context, q)`.

Author: RussTedrake

bindings/pydrake/multibody/BUILD.bazel

@@ -167,6 +167,7 @@ drake_pybind_library(
         "//bindings/pydrake:documentation_pybind",
         "//bindings/pydrake/common:cpp_template_pybind",
         "//bindings/pydrake/common:default_scalars_pybind",
+        "//bindings/pydrake/common:deprecation_pybind",
         "//bindings/pydrake/common:serialize_pybind",
         "//bindings/pydrake/common:type_pack",
     ],
@@ -446,6 +447,7 @@ drake_py_unittest(
     deps = [
         ":meshcat_py",
         ":parsing_py",
+        "//bindings/pydrake/common/test_utilities:deprecation_py",
         "//bindings/pydrake/common/test_utilities:numpy_compare_py",
     ],
 )

bindings/pydrake/multibody/meshcat_py.cc

@@ -1,5 +1,6 @@
 #include "drake/bindings/pydrake/common/cpp_template_pybind.h"
 #include "drake/bindings/pydrake/common/default_scalars_pybind.h"
+#include "drake/bindings/pydrake/common/deprecation_pybind.h"
 #include "drake/bindings/pydrake/common/serialize_pybind.h"
 #include "drake/bindings/pydrake/common/type_pack.h"
 #include "drake/bindings/pydrake/documentation_pybind.h"
@@ -167,20 +168,22 @@ void DoScalarDependentDefinitions(py::module m, T) {
   {
     using Class = JointSliders<T>;
     constexpr auto& cls_doc = doc.JointSliders;
-    DefineTemplateClassWithDefault<JointSliders<T>, LeafSystem<T>>(
-        m, "JointSliders", param, doc.JointSliders.doc)
+    auto cls = DefineTemplateClassWithDefault<JointSliders<T>, LeafSystem<T>>(
+        m, "JointSliders", param, doc.JointSliders.doc);
+    cls  // BR
         .def(py::init<std::shared_ptr<geometry::Meshcat>,
                  const MultibodyPlant<T>*, std::optional<Eigen::VectorXd>,
                  std::variant<std::monostate, double, Eigen::VectorXd>,
                  std::variant<std::monostate, double, Eigen::VectorXd>,
                  std::variant<std::monostate, double, Eigen::VectorXd>,
-                 std::vector<std::string>, std::vector<std::string>>(),
+                 std::vector<std::string>, std::vector<std::string>, double>(),
             py::arg("meshcat"), py::arg("plant"),
             py::arg("initial_value") = py::none(),
             py::arg("lower_limit") = py::none(),
             py::arg("upper_limit") = py::none(), py::arg("step") = py::none(),
             py::arg("decrement_keycodes") = std::vector<std::string>(),
             py::arg("increment_keycodes") = std::vector<std::string>(),
+            py::arg("time_step") = 1.0 / 32.0,
             // Keep alive, reference: `self` keeps `plant` alive.
             py::keep_alive<1, 3>(),  // BR
             cls_doc.ctor.doc)
@@ -191,8 +194,18 @@ void DoScalarDependentDefinitions(py::module m, T) {
             // This is a long-running function that sleeps; for both reasons, we
             // must release the GIL.
             py::call_guard<py::gil_scoped_release>(), cls_doc.Run.doc)
-        .def("SetPositions", &Class::SetPositions, py::arg("q"),
-            cls_doc.SetPositions.doc);
+        .def("SetPositions",
+            py::overload_cast<systems::Context<T>*, const Eigen::VectorXd&>(
+                &Class::SetPositions),
+            py::arg("context"), py::arg("q"), cls_doc.SetPositions.doc);
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+    cls.def("SetPositions",
+        WrapDeprecated(doc.JointSliders.SetPositions.doc_deprecated,
+            py::overload_cast<const Eigen::VectorXd&>(&Class::SetPositions)),
+        py::arg("q"), doc.JointSliders.SetPositions.doc_deprecated);
+#pragma GCC diagnostic pop
   }
 }
 }  // namespace

bindings/pydrake/multibody/test/meshcat_test.py

@@ -17,6 +17,7 @@
 from pydrake.common.test_utilities import (
     numpy_compare,
 )
+from pydrake.common.test_utilities.deprecation import catch_drake_warnings
 from pydrake.geometry import (
     Meshcat,
     Rgba,
@@ -93,6 +94,7 @@ def test_joint_sliders(self):
             step=[0.25, 0.50],
             decrement_keycodes=["ArrowLeft", "ArrowDown"],
             increment_keycodes=["ArrowRight", "ArrowUp"],
+            time_step=0.01,
         )
 
         # Various methods should not crash.
@@ -134,7 +136,11 @@ def test_joint_sliders(self):
         np.testing.assert_equal(q, [0, 0])
 
         # The SetPositions function doesn't crash (Acrobot has two positions).
-        dut.SetPositions(q=[1, 2])
+        context = dut.CreateDefaultContext()
+        dut.SetPositions(context=context, q=[1, 2])
+        with catch_drake_warnings(expected_count=1) as w:
+            dut.SetPositions(q=[1, 2])
+            self.assertIn("SetPositions(context", str(w[0].message))
 
     def test_internal_point_contact_visualizer(self):
         """A very basic existence test, since this class is internal use only.

bindings/pydrake/visualization/_model_visualizer.py

@@ -406,7 +406,8 @@ def Finalize(self, position=None):
             self._diagram.plant().SetPositions(
                 self._diagram.plant().GetMyContextFromRoot(self._context),
                 position)
-            self._sliders.SetPositions(position)
+            self._sliders.SetPositions(
+                self._sliders.GetMyContextFromRoot(self._context), position)
 
         # Use Simulator to dispatch initialization events.
         # TODO(eric.cousineau): Simplify as part of #13776 (was #10015).
@@ -523,9 +524,15 @@ def Run(self, position=None, loop_once=False):
                 self._diagram.plant().SetPositions(
                     self._diagram.plant().GetMyContextFromRoot(self._context),
                     position)
-                self._sliders.SetPositions(position)
+                self._sliders.SetPositions(
+                    self._sliders.GetMyContextFromRoot(self._context),
+                    position)
                 self._diagram.ForcedPublish(self._context)
 
+        sliders_context = self._sliders.GetMyContextFromRoot(self._context)
+        plant_context = self._diagram.plant().GetMyContextFromRoot(
+            self._context)
+
         # Everything is finally fully configured. We can open the window now.
         # TODO(jwnimmer-tri) The browser_new config knob would probably make
         # more sense as an argument to Run() vs an argument to our constructor.
@@ -564,13 +571,17 @@ def has_clicks(button_name):
                     self._meshcat.DeleteButton(stop_button_name)
                     slider_values = self._get_slider_values()
                     self._reload()
+                    sliders_context = self._sliders.GetMyContextFromRoot(
+                        self._context)
+                    plant_context = self._diagram.plant().GetMyContextFromRoot(
+                        self._context)
                     self._set_slider_values(slider_values)
                     self._meshcat.AddButton(stop_button_name, "Escape")
-                q = self._sliders.get_output_port().Eval(
-                    self._sliders.GetMyContextFromRoot(self._context))
-                self._diagram.plant().SetPositions(
-                    self._diagram.plant().GetMyContextFromRoot(self._context),
-                    q)
+                updated = self._sliders.EvalUniquePeriodicDiscreteUpdate(
+                    sliders_context)
+                sliders_context.SetDiscreteState(updated)
+                q = self._sliders.get_output_port().Eval(sliders_context)
+                self._diagram.plant().SetPositions(plant_context, q)
                 self._diagram.ForcedPublish(self._context)
                 if loop_once or has_clicks(stop_button_name):
                     break

multibody/inverse_kinematics/add_multibody_plant_constraints.h

@@ -11,11 +11,12 @@ namespace multibody {
 
 /** For all kinematic constraints associated with `plant` adds a corresponding
 solver::Constraint to `prog`, using decision variables `q` to represent the
-generalized positions of the plant.
+generalized positions of the plant. The `plant` must stay alive for the
+lifetime of the added constraints.
 
-Adds joint limits constraints, unit quaternion constraints, and constraints for
-any locked joints (via Joint::Lock()). Note that you must pass a valid
-`plant_context` to use joint locking.
+Adds joint limits constraints (always), unit quaternion constraints, and
+constraints for any locked joints (via Joint::Lock()). Note that you must pass
+a valid `plant_context` to use joint locking.
 
 Adds constraints for coupler, distance, ball, and weld constraints. The
 distance constraint is implemented here as a hard kinematic constraint (i.e.,

multibody/inverse_kinematics/inverse_kinematics.h

@@ -71,12 +71,12 @@ class InverseKinematics {
    * auto plant_context = &(diagram->GetMutableSubsystemContext(items.plant,
    * diagram_context.get()));
    * ```
-   * This context will be modified during calling ik.prog.Solve(...). When
-   * Solve() returns `result`, context will store the optimized posture, namely
-   * plant.GetPositions(*context) will be the same as in
-   * result.GetSolution(ik.q()). The user could then use this context to perform
-   * kinematic computation (like computing the position of the end-effector
-   * etc.).
+   * This context will be modified during calling solvers::Solve(ik.prog, ...).
+   * When Solve() returns `result`, context will store the optimized posture,
+   * namely plant.GetPositions(*context) will be the same as in
+   * result.GetSolution(ik.q()). The user could then use this context to
+   * perform kinematic computation (like computing the position of the
+   * end-effector etc.).
    * @param with_joint_limits If set to true, then the constructor imposes the
    * joint limits (obtained from plant.GetPositionLowerLimits() and
    * plant.GetPositionUpperLimits(), and from any body/joint locks set in

multibody/meshcat/BUILD.bazel

@@ -88,7 +88,10 @@ drake_cc_library(
         "//common:scope_exit",
         "//geometry:meshcat",
         "//geometry:meshcat_graphviz",
+        "//multibody/inverse_kinematics:add_multibody_plant_constraints",
         "//multibody/plant",
+        "//solvers:choose_best_solver",
+        "//solvers:solve",
     ],
 )