Merge pull request RobotLocomotion#9858 from RussTedrake/wsg_lcm

add WsgCommandSender and WsgStatusReceiver

manipulation/schunk_wsg/BUILD.bazel

@@ -153,6 +153,7 @@ drake_cc_googletest(
     ],
     deps = [
         ":schunk_wsg_lcm",
+        "//common/test_utilities:eigen_matrix_compare",
         "//systems/analysis:simulator",
     ],
 )

manipulation/schunk_wsg/schunk_wsg_lcm.cc

@@ -65,6 +65,67 @@ void SchunkWsgCommandReceiver::CalcForceLimitOutput(
   output->SetAtIndex(0, force_limit);
 }
 
+SchunkWsgCommandSender::SchunkWsgCommandSender()
+    : 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()) {
+  this->DeclareAbstractOutputPort("lcmt_schunk_wsg_command",
+                                  &SchunkWsgCommandSender::CalcCommandOutput);
+}
+
+void SchunkWsgCommandSender::CalcCommandOutput(
+    const drake::systems::Context<double>& context,
+    drake::lcmt_schunk_wsg_command* output) const {
+  lcmt_schunk_wsg_command& command = *output;
+
+  command.utime = context.get_time() * 1e6;
+  const double position =
+      this->EvalVectorInput(context, position_input_port_)->GetAtIndex(0);
+
+  command.target_position_mm = position * 1e3;
+
+  command.force =
+      this->EvalVectorInput(context, force_limit_input_port_)->GetAtIndex(0);
+}
+
+SchunkWsgStatusReceiver::SchunkWsgStatusReceiver()
+    : state_output_port_(this->DeclareVectorOutputPort(
+                                 "state", systems::BasicVector<double>(2),
+                                 &SchunkWsgStatusReceiver::CopyStateOut)
+                             .get_index()),
+      force_output_port_(this->DeclareVectorOutputPort(
+                                 "force", systems::BasicVector<double>(1),
+                                 &SchunkWsgStatusReceiver::CopyForceOut)
+                             .get_index()) {
+  this->DeclareAbstractInputPort("lcmt_schunk_wsg_status",
+                                 systems::Value<lcmt_schunk_wsg_status>());
+}
+
+void SchunkWsgStatusReceiver::CopyStateOut(
+    const drake::systems::Context<double>& context,
+    drake::systems::BasicVector<double>* output) const {
+  const systems::AbstractValue* input = this->EvalAbstractInput(context, 0);
+  DRAKE_ASSERT(input != nullptr);
+  const auto& status = input->GetValue<lcmt_schunk_wsg_status>();
+
+  output->SetAtIndex(0, status.actual_position_mm / 1e3);
+  output->SetAtIndex(1, status.actual_speed_mm_per_s / 1e3);
+}
+
+void SchunkWsgStatusReceiver::CopyForceOut(
+    const drake::systems::Context<double>& context,
+    drake::systems::BasicVector<double>* output) const {
+  const systems::AbstractValue* input = this->EvalAbstractInput(context, 0);
+  DRAKE_ASSERT(input != nullptr);
+  const auto& status = input->GetValue<lcmt_schunk_wsg_status>();
+
+  output->SetAtIndex(0, status.actual_force);
+}
+
 SchunkWsgStatusSender::SchunkWsgStatusSender(int input_state_size,
                                              int input_torque_size,
                                              int position_index,

manipulation/schunk_wsg/schunk_wsg_lcm.h

@@ -4,6 +4,7 @@
 /// LCM messages related to the Schunk WSG gripper.
 
 #include "drake/common/drake_deprecated.h"
+#include "drake/lcmt_schunk_wsg_command.hpp"
 #include "drake/lcmt_schunk_wsg_status.hpp"
 #include "drake/systems/framework/leaf_system.h"
 
@@ -59,6 +60,94 @@ class SchunkWsgCommandReceiver : public systems::LeafSystem<double> {
   const systems::OutputPortIndex force_limit_output_port_{};
 };
 
+
+/// Send lcmt_schunk_wsg_command messages for a Schunk WSG gripper.  Has
+/// two input ports: one for the commanded finger position represented as the
+/// desired signed distance between the fingers in meters, and one for the
+/// commanded force limit.  The commanded position and force limit are
+/// scalars (BasicVector<double> of size 1).
+///
+/// @system{ SchunkWsgCommandSender,
+///   @input_port{position}
+///   @input_port{force_limit},
+///   @output_port{lcmt_schunk_wsg_command}
+/// }
+class SchunkWsgCommandSender : public systems::LeafSystem<double> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SchunkWsgCommandSender)
+
+  SchunkWsgCommandSender();
+
+  const systems::InputPort<double>& get_position_input_port()
+  const {
+    return this->get_input_port(position_input_port_);
+  }
+
+  const systems::InputPort<double>& get_force_limit_input_port()
+  const {
+    return this->get_input_port(force_limit_input_port_);
+  }
+
+  const systems::OutputPort<double>& get_command_output_port() const {
+    return this->get_output_port(0);
+  }
+
+ private:
+  void CalcCommandOutput(
+      const systems::Context<double>& context,
+      lcmt_schunk_wsg_command* output) const;
+
+ private:
+  const systems::InputPortIndex position_input_port_{};
+  const systems::InputPortIndex force_limit_input_port_{};
+};
+
+
+/// Handles lcmt_schunk_wsg_status messages from a LcmSubscriberSystem.  Has
+/// two output ports: one for the measured state of the gripper, represented as
+/// the signed distance between the fingers in meters and its corresponding
+/// velocity, and one for the measured force.
+///
+/// @system{ SchunkWsgStatusReceiver,
+///   @input_port{lcmt_schunk_wsg_status},
+///   @output_port{state}
+///   @output_port{force}
+/// }
+class SchunkWsgStatusReceiver : public systems::LeafSystem<double> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SchunkWsgStatusReceiver)
+
+  SchunkWsgStatusReceiver();
+
+  const systems::InputPort<double>& get_status_input_port() const {
+    return this->get_input_port(0);
+  }
+
+  const systems::OutputPort<double>& get_state_output_port()
+  const {
+    return this->get_output_port(state_output_port_);
+  }
+
+  const systems::OutputPort<double>& get_force_output_port()
+  const {
+    return this->get_output_port(force_output_port_);
+  }
+
+ private:
+  void CopyStateOut(
+      const systems::Context<double>& context,
+      systems::BasicVector<double>* output) const;
+
+  void CopyForceOut(
+      const systems::Context<double>& context,
+      systems::BasicVector<double>* output) const;
+
+ private:
+  const systems::OutputPortIndex state_output_port_{};
+  const systems::OutputPortIndex force_output_port_{};
+};
+
+
 /// Sends lcmt_schunk_wsg_status messages for a Schunk WSG.  This
 /// system has one input port for the current state of the WSG, and one
 /// optional input port for the measured gripping force.

manipulation/schunk_wsg/test/schunk_wsg_lcm_test.cc

@@ -4,6 +4,7 @@
 
 #include <gtest/gtest.h>
 
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/lcmt_schunk_wsg_command.hpp"
 #include "drake/lcmt_schunk_wsg_status.hpp"
 #include "drake/systems/analysis/simulator.h"
@@ -58,6 +59,61 @@ GTEST_TEST(SchunkWsgLcmTest, SchunkWsgCommandReceiverTest) {
             40);
 }
 
+GTEST_TEST(SchunkWsgLcmTest, SchunkWsgCommandSenderTest) {
+  SchunkWsgCommandSender dut;
+  std::unique_ptr<systems::Context<double>> context =
+      dut.CreateDefaultContext();
+
+  const double position = 0.0015;
+  context->FixInputPort(dut.get_position_input_port().get_index(),
+                        Vector1d(position));
+  const double force_limit = 25.0;
+  context->FixInputPort(dut.get_force_limit_input_port().get_index(),
+                        Vector1d(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);
+}
+
+GTEST_TEST(SchunkWsgLcmTest, SchunkWsgStatusReceiverTest) {
+  SchunkWsgStatusReceiver dut;
+  std::unique_ptr<systems::Context<double>> context =
+      dut.CreateDefaultContext();
+
+  // Check that we get zeros before receiving any messages (using the POD
+  // initialization via {}, as used in LcmSubscriberSystem).
+  lcmt_schunk_wsg_status status{};
+  context->FixInputPort(
+      0, systems::AbstractValue::Make<lcmt_schunk_wsg_status>(status));
+  EXPECT_TRUE(CompareMatrices(dut.get_state_output_port()
+                                  .Eval<BasicVector<double>>(*context)
+                                  .get_value(),
+                              Vector2d::Zero()));
+  EXPECT_EQ(dut.get_force_output_port()
+                .Eval<BasicVector<double>>(*context)
+                .GetAtIndex(0),
+            0.0);
+
+  // Check that we can read out valid input.
+  status.utime = 1;
+  status.actual_position_mm = 100;
+  status.actual_speed_mm_per_s = 324;
+  status.actual_force = 40;
+  context->FixInputPort(
+      0, systems::AbstractValue::Make<lcmt_schunk_wsg_status>(status));
+  EXPECT_TRUE(CompareMatrices(dut.get_state_output_port()
+                                  .Eval<BasicVector<double>>(*context)
+                                  .get_value(),
+                              Vector2d(.1, .324)));
+  EXPECT_EQ(dut.get_force_output_port()
+                .Eval<BasicVector<double>>(*context)
+                .GetAtIndex(0),
+            40.0);
+}
+
 GTEST_TEST(SchunkWsgLcmTest, SchunkWsgStatusSenderTest) {
   SchunkWsgStatusSender dut;
   std::unique_ptr<systems::Context<double>> context =