Add plant_context argument to InverseDynamics{,Controller} (RobotLoco…

…motion#21287)

Co-authored-by: Xuchen Han <[email protected]>

bindings/pydrake/systems/controllers_py.cc

@@ -88,10 +88,10 @@ PYBIND11_MODULE(controllers, m) {
           .export_values();
     }
     cls  // BR
-        .def(py::init<const MultibodyPlant<double>*,
-                 Class::InverseDynamicsMode>(),
+        .def(py::init<const MultibodyPlant<double>*, Class::InverseDynamicsMode,
+                 const systems::Context<double>*>(),
             py::arg("plant"), py::arg("mode") = Class::kInverseDynamics,
-            cls_doc.ctor.doc)
+            py::arg("plant_context") = nullptr, cls_doc.ctor.doc)
         .def("is_pure_gravity_compensation",
             &Class::is_pure_gravity_compensation,
             cls_doc.is_pure_gravity_compensation.doc)
@@ -116,9 +116,11 @@ PYBIND11_MODULE(controllers, m) {
     py::class_<Class, Diagram<double>>(
         m, "InverseDynamicsController", cls_doc.doc)
         .def(py::init<const MultibodyPlant<double>&, const VectorX<double>&,
-                 const VectorX<double>&, const VectorX<double>&, bool>(),
+                 const VectorX<double>&, const VectorX<double>&, bool,
+                 const systems::Context<double>*>(),
             py::arg("robot"), py::arg("kp"), py::arg("ki"), py::arg("kd"),
             py::arg("has_reference_acceleration"),
+            py::arg("plant_context") = nullptr,
             // Keep alive, reference: `self` keeps `robot` alive.
             py::keep_alive<1, 2>(), cls_doc.ctor.doc)
         .def("set_integral_value", &Class::set_integral_value,

bindings/pydrake/systems/test/controllers_test.py

@@ -151,7 +151,8 @@ def test_inverse_dynamics(self):
 
         controller = InverseDynamics(
             plant=plant,
-            mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
+            mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation,
+            plant_context=plant.CreateDefaultContext())
         self.assertIsInstance(controller.get_input_port_estimated_state(),
                               InputPort)
         self.assertIsInstance(controller.get_output_port_generalized_force(),
@@ -183,11 +184,13 @@ def test_inverse_dynamics_controller(self):
         ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
         kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5])
 
-        controller = InverseDynamicsController(robot=plant,
-                                               kp=kp,
-                                               ki=ki,
-                                               kd=kd,
-                                               has_reference_acceleration=True)
+        controller = InverseDynamicsController(
+                robot=plant,
+                kp=kp,
+                ki=ki,
+                kd=kd,
+                has_reference_acceleration=True,
+                plant_context=plant.CreateDefaultContext())
         context = controller.CreateDefaultContext()
         output = controller.AllocateOutput()
 

systems/controllers/inverse_dynamics.cc

@@ -13,7 +13,8 @@ namespace controllers {
 template <typename T>
 InverseDynamics<T>::InverseDynamics(
     std::unique_ptr<multibody::MultibodyPlant<T>> owned_plant,
-    const MultibodyPlant<T>* plant, const InverseDynamicsMode mode)
+    const MultibodyPlant<T>* plant, const InverseDynamicsMode mode,
+    const Context<T>* plant_context)
     : LeafSystem<T>(SystemTypeTag<InverseDynamics>{}),
       owned_plant_(std::move(owned_plant)),
       plant_(owned_plant_ ? owned_plant_.get() : plant),
@@ -24,6 +25,9 @@ InverseDynamics<T>::InverseDynamics(
   DRAKE_DEMAND(owned_plant_ == nullptr || plant == nullptr);
   DRAKE_DEMAND(plant_ != nullptr);
   DRAKE_THROW_UNLESS(plant_->is_finalized());
+  if (plant_context != nullptr) {
+    plant_->ValidateContext(*plant_context);
+  }
 
   estimated_state_ =
       this->DeclareInputPort("estimated_state", kVectorValued, q_dim_ + v_dim_)
@@ -36,19 +40,21 @@ InverseDynamics<T>::InverseDynamics(
                                     {this->all_input_ports_ticket()})
           .get_index();
 
-  auto plant_context = plant_->CreateDefaultContext();
+  std::unique_ptr<Context<T>> model_plant_context =
+      plant_context == nullptr ? plant_->CreateDefaultContext()
+                               : plant_context->Clone();
+
   // Gravity compensation mode requires velocities to be zero.
   if (this->is_pure_gravity_compensation()) {
-    plant_->SetVelocities(plant_context.get(),
+    plant_->SetVelocities(model_plant_context.get(),
                           VectorX<T>::Zero(plant_->num_velocities()));
   }
 
   // Declare cache entry for the multibody plant context.
   plant_context_cache_index_ =
-      this->DeclareCacheEntry(
-              "plant_context_cache", *plant_context,
-              &InverseDynamics<T>::SetMultibodyContext,
-              {this->input_port_ticket(estimated_state_)})
+      this->DeclareCacheEntry("plant_context_cache", *model_plant_context,
+                              &InverseDynamics<T>::SetMultibodyContext,
+                              {this->input_port_ticket(estimated_state_)})
           .cache_index();
 
   // Declare external forces cache entry and desired acceleration input port if
@@ -69,22 +75,52 @@ InverseDynamics<T>::InverseDynamics(
 
 template <typename T>
 InverseDynamics<T>::InverseDynamics(const MultibodyPlant<T>* plant,
-                                    const InverseDynamicsMode mode)
-    : InverseDynamics(nullptr, plant, mode) {}
+                                    const InverseDynamicsMode mode,
+                                    const Context<T>* plant_context)
+    : InverseDynamics(nullptr, plant, mode, plant_context) {}
 
 template <typename T>
 InverseDynamics<T>::InverseDynamics(
     std::unique_ptr<multibody::MultibodyPlant<T>> plant,
-    const InverseDynamicsMode mode)
-    : InverseDynamics(std::move(plant), nullptr, mode) {}
+    const InverseDynamicsMode mode, const Context<T>* plant_context)
+    : InverseDynamics(std::move(plant), nullptr, mode, plant_context) {}
+
+template <typename T>
+template <typename U>
+typename InverseDynamics<T>::ScalarConversionData
+InverseDynamics<T>::ScalarConvertHelper(const InverseDynamics<U>& other) {
+  // Convert plant.
+  auto this_plant = systems::System<U>::template ToScalarType<T>(*other.plant_);
+  auto this_plant_context = this_plant->CreateDefaultContext();
+
+  // Create context and connect required port.
+  auto other_context = other.CreateDefaultContext();
+  const int num_states = other.get_input_port_estimated_state().size();
+  other.get_input_port_estimated_state().FixValue(
+      other_context.get(),
+      Eigen::Matrix<U, Eigen::Dynamic, 1>::Zero(num_states));
+
+  // Convert plant context.
+  const auto& other_plant_context =
+      other.get_cache_entry(other.plant_context_cache_index_)
+          .template Eval<Context<U>>(*other_context);
+  this_plant_context->SetStateAndParametersFrom(other_plant_context);
+  InverseDynamicsMode mode = other.is_pure_gravity_compensation()
+                                 ? kGravityCompensation
+                                 : kInverseDynamics;
+  return ScalarConversionData{std::move(this_plant), mode,
+                              std::move(this_plant_context)};
+}
 
 template <typename T>
 template <typename U>
 InverseDynamics<T>::InverseDynamics(const InverseDynamics<U>& other)
-    : InverseDynamics(
-          systems::System<U>::template ToScalarType<T>(*other.plant_),
-          other.is_pure_gravity_compensation() ? kGravityCompensation
-                                               : kInverseDynamics) {}
+    : InverseDynamics(ScalarConvertHelper(other)) {}
+
+template <typename T>
+InverseDynamics<T>::InverseDynamics(ScalarConversionData&& data)
+    : InverseDynamics(std::move(data.plant), data.mode,
+                      data.plant_context.get()) {}
 
 template <typename T>
 InverseDynamics<T>::~InverseDynamics() = default;
@@ -130,7 +166,7 @@ void InverseDynamics<T>::CalcOutputForce(const Context<T>& context,
 
     // Compute inverse dynamics.
     const VectorX<T>& desired_vd =
-            get_input_port_desired_acceleration().Eval(context);
+        get_input_port_desired_acceleration().Eval(context);
 
     output->get_mutable_value() =
         plant_->CalcInverseDynamics(plant_context, desired_vd, external_forces);

systems/controllers/inverse_dynamics.h

@@ -61,6 +61,8 @@ namespace controllers {
 template <typename T>
 class InverseDynamics final : public LeafSystem<T> {
  public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(InverseDynamics);
+
   enum InverseDynamicsMode {
     /// Full inverse computation mode.
     kInverseDynamics,
@@ -69,21 +71,26 @@ class InverseDynamics final : public LeafSystem<T> {
     kGravityCompensation
   };
 
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(InverseDynamics)
-
   /**
    * Constructs the InverseDynamics system.
    *
-   * @param plant Pointer to the multibody plant model. The life span of @p
-   * plant must be longer than that of this instance.
+   * @param plant Pointer to the multibody plant model. The life span of
+   * `plant` must be longer than that of this instance.
    * @param mode If set to kGravityCompensation, this instance will only
    * consider the gravity term. It also will NOT have the desired acceleration
    * input port.
+   * @param plant_context A specific context of `plant` to use for computing
+   * inverse dynamics. For example, you can use this to pass in a context with
+   * modified mass parameters.  If `nullptr`, the default context of the given
+   * `plant` is used. Note that this will be copied at time of construction, so
+   * there are no lifetime constraints.
    * @pre The plant must be finalized (i.e., plant.is_finalized() must return
-   * `true`).
+   * `true`). Also, `plant_context`, if provided, must be compatible with
+   * `plant`.
    */
   explicit InverseDynamics(const multibody::MultibodyPlant<T>* plant,
-                           InverseDynamicsMode mode = kInverseDynamics);
+                           InverseDynamicsMode mode = kInverseDynamics,
+                           const Context<T>* plant_context = nullptr);
 
   /**
    * Constructs the InverseDynamics system and takes the ownership of the
@@ -92,7 +99,8 @@ class InverseDynamics final : public LeafSystem<T> {
    * @exclude_from_pydrake_mkdoc{This overload is not bound.}
    */
   explicit InverseDynamics(std::unique_ptr<multibody::MultibodyPlant<T>> plant,
-                           InverseDynamicsMode mode = kInverseDynamics);
+                           InverseDynamicsMode mode = kInverseDynamics,
+                           const Context<T>* plant_context = nullptr);
 
   // Scalar-converting copy constructor.  See @ref system_scalar_conversion.
   template <typename U>
@@ -132,13 +140,29 @@ class InverseDynamics final : public LeafSystem<T> {
   // Other constructors delegate to this private constructor.
   InverseDynamics(std::unique_ptr<multibody::MultibodyPlant<T>> owned_plant,
                   const multibody::MultibodyPlant<T>* plant,
-                  InverseDynamicsMode mode);
+                  InverseDynamicsMode mode, const Context<T>* plant_context);
+
+  // Helper data structure for scalar conversion.
+  struct ScalarConversionData {
+    std::unique_ptr<multibody::MultibodyPlant<T>> plant;
+    InverseDynamicsMode mode{InverseDynamicsMode::kGravityCompensation};
+    std::unique_ptr<Context<T>> plant_context;
+  };
+
+  // Helper function for the scalar conversion constructor that extracts the
+  // plant context from `other` and scalar converts it to this scalar type, T.
+  template <typename U>
+  static ScalarConversionData ScalarConvertHelper(
+      const InverseDynamics<U>& other);
+
+  // Delegate constructor for scalar conversion.
+  explicit InverseDynamics(ScalarConversionData&& data);
 
-  template <typename> friend class InverseDynamics;
+  template <typename>
+  friend class InverseDynamics;
 
   // This is the calculator method for the output port.
-  void CalcOutputForce(const Context<T>& context,
-                       BasicVector<T>* force) const;
+  void CalcOutputForce(const Context<T>& context, BasicVector<T>* force) const;
 
   // Methods for updating cache entries.
   void SetMultibodyContext(const Context<T>&, Context<T>*) const;

systems/controllers/inverse_dynamics_controller.cc

@@ -19,19 +19,19 @@ template <typename T>
 void InverseDynamicsController<T>::SetUp(
     std::unique_ptr<multibody::MultibodyPlant<T>> owned_plant,
     const VectorX<double>& kp, const VectorX<double>& ki,
-    const VectorX<double>& kd) {
+    const VectorX<double>& kd, const Context<T>* plant_context) {
   DRAKE_DEMAND(multibody_plant_for_control_->is_finalized());
 
   DiagramBuilder<T> builder;
   InverseDynamics<T>* inverse_dynamics{};
   if (owned_plant) {
     inverse_dynamics = builder.template AddNamedSystem<InverseDynamics<T>>(
         "InverseDynamics", std::move(owned_plant),
-        InverseDynamics<T>::kInverseDynamics);
+        InverseDynamics<T>::kInverseDynamics, plant_context);
   } else {
     inverse_dynamics = builder.template AddNamedSystem<InverseDynamics<T>>(
         "InverseDynamics", multibody_plant_for_control_,
-        InverseDynamics<T>::kInverseDynamics);
+        InverseDynamics<T>::kInverseDynamics, plant_context);
   }
 
   const int num_positions = multibody_plant_for_control_->num_positions();
@@ -135,20 +135,22 @@ template <typename T>
 InverseDynamicsController<T>::InverseDynamicsController(
     const MultibodyPlant<T>& plant, const VectorX<double>& kp,
     const VectorX<double>& ki, const VectorX<double>& kd,
-    bool has_reference_acceleration)
+    bool has_reference_acceleration,
+    const Context<T>* plant_context)
     : multibody_plant_for_control_(&plant),
       has_reference_acceleration_(has_reference_acceleration) {
-  SetUp(nullptr, kp, ki, kd);
+  SetUp(nullptr, kp, ki, kd, plant_context);
 }
 
 template <typename T>
 InverseDynamicsController<T>::InverseDynamicsController(
     std::unique_ptr<multibody::MultibodyPlant<T>> plant,
     const VectorX<double>& kp, const VectorX<double>& ki,
-    const VectorX<double>& kd, bool has_reference_acceleration)
+    const VectorX<double>& kd, bool has_reference_acceleration,
+    const Context<T>* plant_context)
     : multibody_plant_for_control_(plant.get()),
       has_reference_acceleration_(has_reference_acceleration) {
-  SetUp(std::move(plant), kp, ki, kd);
+  SetUp(std::move(plant), kp, ki, kd, plant_context);
 }
 
 template <typename T>

systems/controllers/inverse_dynamics_controller.h

@@ -81,9 +81,14 @@ class InverseDynamicsController final
    * @param has_reference_acceleration If true, there is an extra BasicVector
    * input port for `vd_d`. If false, `vd_d` is treated as zero, and no extra
    * input port is declared.
+   * @param plant_context The context of the `plant` that can be used to
+   * override the plant's default parameters. Note that this will be copied at
+   * time of construction, so there are no lifetime constraints.
    * @pre `plant` has been finalized (plant.is_finalized() returns `true`).
+   * Also, `plant` and `plant_context` must be compatible.
    * @throws std::exception if
    *  - The plant is not finalized (see MultibodyPlant::Finalize()).
+   *  - The plant is not compatible with the plant context.
    *  - The number of generalized velocities is not equal to the number of
    *    generalized positions.
    *  - The model is not fully actuated.
@@ -95,7 +100,8 @@ class InverseDynamicsController final
       const VectorX<double>& kp,
       const VectorX<double>& ki,
       const VectorX<double>& kd,
-      bool has_reference_acceleration);
+      bool has_reference_acceleration,
+      const Context<T>* plant_context = nullptr);
 
   /**
    * Constructs an inverse dynamics controller and takes the ownership of the
@@ -107,7 +113,8 @@ class InverseDynamicsController final
                             const VectorX<double>& kp,
                             const VectorX<double>& ki,
                             const VectorX<double>& kd,
-                            bool has_reference_acceleration);
+                            bool has_reference_acceleration,
+                            const Context<T>* plant_context = nullptr);
 
   // Scalar-converting copy constructor.  See @ref system_scalar_conversion.
   template <typename U>
@@ -162,7 +169,8 @@ class InverseDynamicsController final
  private:
   void SetUp(std::unique_ptr<multibody::MultibodyPlant<T>> owned_plant,
              const VectorX<double>& kp, const VectorX<double>& ki,
-             const VectorX<double>& kd);
+             const VectorX<double>& kd,
+             const Context<T>* plant_context);
 
   const multibody::MultibodyPlant<T>* multibody_plant_for_control_{nullptr};
   PidController<T>* pid_{nullptr};