Make force limit input port for Schunk WSG optional (RobotLocomotion#…

…13878)

bindings/pydrake/manipulation/schunk_wsg_py.cc

@@ -25,10 +25,11 @@ PYBIND11_MODULE(schunk_wsg, m) {
     constexpr auto& cls_doc = doc.SchunkWsgPositionController;
     py::class_<Class, Diagram<double>>(
         m, "SchunkWsgPositionController", cls_doc.doc)
-        .def(py::init<double, double, double, double, double>(),
+        .def(py::init<double, double, double, double, double, double>(),
             py::arg("time_step") = 0.05, py::arg("kp_command") = 200.,
             py::arg("kd_command") = 5., py::arg("kp_constraint") = 2000.,
-            py::arg("kd_constraint") = 5., cls_doc.ctor.doc)
+            py::arg("kd_constraint") = 5.,
+            py::arg("default_force_limit") = 40.0, cls_doc.ctor.doc)
         .def("get_desired_position_input_port",
             &Class::get_desired_position_input_port, py_rvp::reference_internal,
             cls_doc.get_desired_position_input_port.doc)
@@ -63,7 +64,8 @@ PYBIND11_MODULE(schunk_wsg, m) {
     constexpr auto& cls_doc = doc.SchunkWsgCommandSender;
     py::class_<Class, LeafSystem<double>>(
         m, "SchunkWsgCommandSender", cls_doc.doc)
-        .def(py::init(), cls_doc.ctor.doc)
+        .def(py::init<double>(), py::arg("default_force_limit") = 40.0,
+            cls_doc.ctor.doc)
         .def("get_position_input_port", &Class::get_position_input_port,
             py_rvp::reference_internal, cls_doc.get_position_input_port.doc)
         .def("get_force_limit_input_port", &Class::get_force_limit_input_port,

bindings/pydrake/manipulation/test/schunk_wsg_test.py

@@ -11,9 +11,12 @@
 
 class TestSchunkWsg(unittest.TestCase):
     def test_schunk_wsg_controller(self):
-        controller = mut.SchunkWsgPositionController(
-            time_step=0.01, kp_command=100., kd_command=1.,
-            kp_constraint=1000., kd_constraint=1.)
+        controller = mut.SchunkWsgPositionController(time_step=0.01,
+                                                     kp_command=100.,
+                                                     kd_command=1.,
+                                                     kp_constraint=1000.,
+                                                     kd_constraint=1.,
+                                                     default_force_limit=37.0)
 
         self.assertIsInstance(
             controller.get_desired_position_input_port(), InputPort)
@@ -34,7 +37,7 @@ def test_schunk_wsg_lcm(self):
         self.assertIsInstance(
             command_rec.get_force_limit_output_port(), OutputPort)
 
-        command_send = mut.SchunkWsgCommandSender()
+        command_send = mut.SchunkWsgCommandSender(default_force_limit=37.0)
         self.assertIsInstance(
             command_send.get_position_input_port(), InputPort)
         self.assertIsInstance(

examples/manipulation_station/manipulation_station.h

@@ -46,7 +46,7 @@ enum class Setup { kNone, kManipulationClass, kClutterClearing, kPlanarIiwa };
 /// - iiwa_position
 /// - iiwa_feedforward_torque
 /// - wsg_position
-/// - wsg_force_limit
+/// - wsg_force_limit (optional)
 /// output_ports:
 /// - iiwa_position_commanded
 /// - iiwa_position_measured

examples/manipulation_station/manipulation_station_hardware_interface.h

@@ -27,7 +27,7 @@ namespace manipulation_station {
 /// - iiwa_position
 /// - iiwa_feedforward_torque
 /// - wsg_position
-/// - wsg_force_limit
+/// - wsg_force_limit (optional)
 /// output_ports:
 /// - iiwa_position_commanded
 /// - iiwa_position_measured

manipulation/schunk_wsg/schunk_wsg_lcm.cc

@@ -65,14 +65,15 @@ void SchunkWsgCommandReceiver::CalcForceLimitOutput(
   output->SetAtIndex(0, force_limit);
 }
 
-SchunkWsgCommandSender::SchunkWsgCommandSender()
+SchunkWsgCommandSender::SchunkWsgCommandSender(double default_force_limit)
     : position_input_port_(this->DeclareVectorInputPort(
                                    "position", systems::BasicVector<double>(1))
                                .get_index()),
       force_limit_input_port_(
           this->DeclareVectorInputPort("force_limit",
                                        systems::BasicVector<double>(1))
-              .get_index()) {
+              .get_index()),
+      default_force_limit_(default_force_limit) {
   this->DeclareAbstractOutputPort("lcmt_schunk_wsg_command",
                                   &SchunkWsgCommandSender::CalcCommandOutput);
 }
@@ -84,7 +85,11 @@ void SchunkWsgCommandSender::CalcCommandOutput(
 
   command.utime = context.get_time() * 1e6;
   command.target_position_mm = get_position_input_port().Eval(context)[0] * 1e3;
-  command.force = get_force_limit_input_port().Eval(context)[0];
+  if (get_force_limit_input_port().HasValue(context)) {
+    command.force = get_force_limit_input_port().Eval(context)[0];
+  } else {
+    command.force = default_force_limit_;
+  }
 }
 
 SchunkWsgStatusReceiver::SchunkWsgStatusReceiver()

manipulation/schunk_wsg/schunk_wsg_lcm.h

@@ -75,7 +75,7 @@ class SchunkWsgCommandReceiver : public systems::LeafSystem<double> {
 /// name: SchunkWsgCommandSender
 /// input_ports:
 /// - position
-/// - force_limit
+/// - force_limit (optional)
 /// output_ports:
 /// - lcmt_schunk_wsg_command
 /// @endsystem
@@ -84,7 +84,7 @@ class SchunkWsgCommandSender : public systems::LeafSystem<double> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SchunkWsgCommandSender)
 
-  SchunkWsgCommandSender();
+  explicit SchunkWsgCommandSender(double default_force_limit = 40.0);
 
   const systems::InputPort<double>& get_position_input_port()
   const {
@@ -108,6 +108,7 @@ class SchunkWsgCommandSender : public systems::LeafSystem<double> {
  private:
   const systems::InputPortIndex position_input_port_{};
   const systems::InputPortIndex force_limit_input_port_{};
+  const double default_force_limit_;
 };
 
 

manipulation/schunk_wsg/schunk_wsg_position_controller.cc

@@ -16,11 +16,13 @@ const int kNumJoints = 2;
 SchunkWsgPdController::SchunkWsgPdController(double kp_command,
                                              double kd_command,
                                              double kp_constraint,
-                                             double kd_constraint)
+                                             double kd_constraint,
+                                             double default_force_limit)
     : kp_command_(kp_command),
       kd_command_(kd_command),
       kp_constraint_(kp_constraint),
-      kd_constraint_(kd_constraint) {
+      kd_constraint_(kd_constraint),
+      default_force_limit_(default_force_limit) {
   DRAKE_DEMAND(kp_command >= 0);
   DRAKE_DEMAND(kd_command >= 0);
   DRAKE_DEMAND(kp_constraint >= 0);
@@ -54,7 +56,9 @@ Vector2d SchunkWsgPdController::CalcGeneralizedForce(
     const drake::systems::Context<double>& context) const {
   // Read the input ports.
   const auto& desired_state = get_desired_state_input_port().Eval(context);
-  const double force_limit = get_force_limit_input_port().Eval(context)[0];
+  const double force_limit = get_force_limit_input_port().HasValue(context)
+                                 ? get_force_limit_input_port().Eval(context)[0]
+                                 : default_force_limit_;
   // TODO(russt): Declare a proper input constraint.
   DRAKE_DEMAND(force_limit > 0);
   const auto& state = get_state_input_port().Eval(context);
@@ -88,14 +92,13 @@ void SchunkWsgPdController::CalcGripForceOutput(
   output_vector->SetAtIndex(0, std::abs(force[0] - force[1]));
 }
 
-SchunkWsgPositionController::SchunkWsgPositionController(double time_step,
-                                                         double kp_command,
-                                                         double kd_command,
-                                                         double kp_constraint,
-                                                         double kd_constraint) {
+SchunkWsgPositionController::SchunkWsgPositionController(
+    double time_step, double kp_command, double kd_command,
+    double kp_constraint, double kd_constraint, double default_force_limit) {
   systems::DiagramBuilder<double> builder;
   auto pd_controller = builder.AddSystem<SchunkWsgPdController>(
-      kp_command, kd_command, kp_constraint, kd_constraint);
+      kp_command, kd_command, kp_constraint, kd_constraint,
+      default_force_limit);
   state_interpolator_ =
       builder.AddSystem<systems::StateInterpolatorWithDiscreteDerivative>(
           1, time_step, true /* suppress_initial_transient */);

manipulation/schunk_wsg/schunk_wsg_position_controller.h

@@ -38,7 +38,7 @@ namespace schunk_wsg {
 /// name: SchunkWSGPdController
 /// input_ports:
 /// - desired_state
-/// - force_limit
+/// - force_limit (optional)
 /// - state
 /// output_ports:
 /// - generalized_force
@@ -47,13 +47,13 @@ namespace schunk_wsg {
 ///
 /// The desired_state is a BasicVector<double> of size 2 (position and
 /// velocity of the distance between the fingers).  The force_limit is a
-/// scalar (BasicVector<double> of size 1).  The state is a
-/// BasicVector<double> of size 4 (positions and velocities of the two
-/// fingers).  The output generalized_force is a BasicVector<double> of size
-/// 2 (generalized force inputs to the two fingers).  The output grip_force is
-/// a scalar surrogate for the force measurement from the driver,
-/// f = abs(f₀-f₁) which, like the gripper itself, only reports a positive
-/// force.
+/// scalar (BasicVector<double> of size 1) and is optional; if the input port is
+/// not connected then the constant value passed into the constructor is used.
+/// The state is a BasicVector<double> of size 4 (positions and velocities of
+/// the two fingers).  The output generalized_force is a BasicVector<double> of
+/// size 2 (generalized force inputs to the two fingers).  The output grip_force
+/// is a scalar surrogate for the force measurement from the driver, f =
+/// abs(f₀-f₁) which, like the gripper itself, only reports a positive force.
 ///
 class SchunkWsgPdController : public systems::LeafSystem<double> {
  public:
@@ -67,7 +67,8 @@ class SchunkWsgPdController : public systems::LeafSystem<double> {
   // kuka tests.  They could be tuned in better with more simulation effort.
   SchunkWsgPdController(double kp_command = 200.0, double kd_command = 5.0,
                         double kp_constraint = 2000.0,
-                        double kd_constraint = 5.0);
+                        double kd_constraint = 5.0,
+                        double default_force_limit = 40.0);
 
   const systems::InputPort<double>& get_desired_state_input_port() const {
     return get_input_port(desired_state_input_port_);
@@ -104,6 +105,7 @@ class SchunkWsgPdController : public systems::LeafSystem<double> {
   const double kd_command_;
   const double kp_constraint_;
   const double kd_constraint_;
+  const double default_force_limit_;
 
   systems::InputPortIndex desired_state_input_port_{};
   systems::InputPortIndex force_limit_input_port_{};
@@ -121,7 +123,7 @@ class SchunkWsgPdController : public systems::LeafSystem<double> {
 /// name: SchunkWSGPositionController
 /// input_ports:
 /// - desired_position
-/// - force_limit
+/// - force_limit (optional)
 /// - state
 /// output_ports:
 /// - generalized_force
@@ -141,7 +143,8 @@ class SchunkWsgPositionController : public systems::Diagram<double> {
                               double kp_command = 200.0,
                               double kd_command = 5.0,
                               double kp_constraint = 2000.0,
-                              double kd_constraint = 5.0);
+                              double kd_constraint = 5.0,
+                              double default_force_limit = 40.0);
 
   // The controller stores the last commanded desired position as state.
   // This is a helper method to reset that state.

manipulation/schunk_wsg/test/schunk_wsg_lcm_test.cc

@@ -50,20 +50,28 @@ GTEST_TEST(SchunkWsgLcmTest, SchunkWsgCommandReceiverTest) {
 }
 
 GTEST_TEST(SchunkWsgLcmTest, SchunkWsgCommandSenderTest) {
-  SchunkWsgCommandSender dut;
+  const double default_force_limit = 37.0;
+  SchunkWsgCommandSender dut(default_force_limit);
   std::unique_ptr<systems::Context<double>> context =
       dut.CreateDefaultContext();
 
   const double position = 0.0015;
   dut.get_position_input_port().FixValue(context.get(), position);
-  const double force_limit = 25.0;
-  dut.get_force_limit_input_port().FixValue(context.get(), force_limit);
 
   const lcmt_schunk_wsg_command& command =
       dut.get_output_port(0).Eval<lcmt_schunk_wsg_command>(*context);
 
   EXPECT_EQ(command.target_position_mm, 1.5);
-  EXPECT_EQ(command.force, 25.0);
+  EXPECT_EQ(command.force, default_force_limit);
+
+  const double force_limit = 25.0;
+  dut.get_force_limit_input_port().FixValue(context.get(), force_limit);
+
+  const lcmt_schunk_wsg_command& command_w_force =
+      dut.get_output_port(0).Eval<lcmt_schunk_wsg_command>(*context);
+
+  EXPECT_EQ(command_w_force.target_position_mm, 1.5);
+  EXPECT_EQ(command_w_force.force, force_limit);
 }
 
 GTEST_TEST(SchunkWsgLcmTest, SchunkWsgStatusReceiverTest) {

manipulation/schunk_wsg/test/schunk_wsg_position_controller_test.cc

@@ -40,6 +40,21 @@ GTEST_TEST(SchunkWsgPdControllerTest, BasicTest) {
             &controller.GetOutputPort("grip_force"));
 }
 
+GTEST_TEST(SchunkWsgPdControllerTest, DefaultForceLimitTest) {
+    SchunkWsgPdController controller;
+    auto context = controller.CreateDefaultContext();
+
+    controller.get_desired_state_input_port().FixValue(context.get(),
+                                                       Eigen::Vector2d(0.1, 0));
+    controller.get_state_input_port().FixValue(
+        context.get(), Eigen::Vector4d(-0.05, 0.05, 0, 0));
+    // Do NOT connect the force limit input port.
+
+    // Make sure that both output ports still evaluate without error.
+    controller.get_generalized_force_output_port().Eval(*context);
+    controller.get_grip_force_output_port().Eval(*context);
+}
+
 void FixInputsAndHistory(const SchunkWsgPositionController& controller,
                          double desired_position, double force_limit,
                          systems::Context<double>* controller_context) {