Add the lcm utility for planar manipuland. (RobotLocomotion#12439)

Add the lcm utility for planar manipuland.

examples/planar_gripper/BUILD.bazel

@@ -31,6 +31,17 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "planar_manipuland_lcm",
+    srcs = ["planar_manipuland_lcm.cc"],
+    hdrs = ["planar_manipuland_lcm.h"],
+    deps = [
+        "//lcmtypes:planar_manipuland_status",
+        "//systems/framework:event_collection",
+        "//systems/framework:leaf_system",
+    ],
+)
+
 drake_cc_library(
     name = "brick_static_equilibrium_constraint",
     srcs = ["brick_static_equilibrium_constraint.cc"],
@@ -135,6 +146,15 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "planar_manipuland_lcm_test",
+    deps = [
+        ":planar_manipuland_lcm",
+        "//systems/framework:diagram",
+        "//systems/framework:diagram_builder",
+    ],
+)
+
 install_data()
 
 add_lint_tests()

examples/planar_gripper/planar_manipuland_lcm.cc

@@ -0,0 +1,76 @@
+#include "drake/examples/planar_gripper/planar_manipuland_lcm.h"
+
+namespace drake {
+namespace examples {
+namespace planar_gripper {
+PlanarManipulandStatusDecoder::PlanarManipulandStatusDecoder() {
+  this->DeclareVectorOutputPort(systems::BasicVector<double>(6),
+                                &PlanarManipulandStatusDecoder::OutputStatus);
+  this->DeclareAbstractInputPort("manipuland_state",
+                                 Value<lcmt_planar_manipuland_status>{});
+  this->DeclareDiscreteState(6);
+  this->DeclarePeriodicDiscreteUpdateEvent(
+      kPlanarManipulandStatusPeriod, 0.,
+      &PlanarManipulandStatusDecoder::UpdateDiscreteState);
+  // Register a forced discrete state update event. It is added for unit test,
+  // or for potential users who require forced updates.
+  this->DeclareForcedDiscreteUpdateEvent(
+      &PlanarManipulandStatusDecoder::UpdateDiscreteState);
+}
+
+systems::EventStatus PlanarManipulandStatusDecoder::UpdateDiscreteState(
+    const systems::Context<double>& context,
+    systems::DiscreteValues<double>* discrete_state) const {
+  const AbstractValue* input = this->EvalAbstractInput(context, 0);
+  DRAKE_ASSERT(input != nullptr);
+  const auto& status = input->get_value<lcmt_planar_manipuland_status>();
+
+  systems::BasicVector<double>& state = discrete_state->get_mutable_vector(0);
+  auto state_value = state.get_mutable_value();
+
+  state_value(0) = status.position[0];
+  state_value(1) = status.position[1];
+  state_value(2) = status.theta;
+  state_value(3) = status.velocity[0];
+  state_value(4) = status.velocity[1];
+  state_value(5) = status.thetadot;
+
+  return systems::EventStatus::Succeeded();
+}
+
+void PlanarManipulandStatusDecoder::OutputStatus(
+    const systems::Context<double>& context,
+    systems::BasicVector<double>* output) const {
+  Eigen::VectorBlock<VectorX<double>> output_vec = output->get_mutable_value();
+  output_vec = context.get_discrete_state(0).get_value();
+}
+
+PlanarManipulandStatusEncoder::PlanarManipulandStatusEncoder() {
+  this->DeclareInputPort(systems::kVectorValued, 6);
+  this->DeclareAbstractOutputPort(
+      &PlanarManipulandStatusEncoder::MakeOutputStatus,
+      &PlanarManipulandStatusEncoder::OutputStatus);
+}
+
+lcmt_planar_manipuland_status PlanarManipulandStatusEncoder::MakeOutputStatus()
+    const {
+  lcmt_planar_manipuland_status msg{};
+  msg.utime = 0;
+  return msg;
+}
+
+void PlanarManipulandStatusEncoder::OutputStatus(
+    const systems::Context<double>& context,
+    lcmt_planar_manipuland_status* output) const {
+  output->utime = context.get_time() * 1e6;
+  const systems::BasicVector<double>* input = this->EvalVectorInput(context, 0);
+  output->position[0] = input->GetAtIndex(0);
+  output->position[1] = input->GetAtIndex(1);
+  output->theta = input->GetAtIndex(2);
+  output->velocity[0] = input->GetAtIndex(3);
+  output->velocity[1] = input->GetAtIndex(4);
+  output->thetadot = input->GetAtIndex(5);
+}
+}  // namespace planar_gripper
+}  // namespace examples
+}  // namespace drake

examples/planar_gripper/planar_manipuland_lcm.h

@@ -0,0 +1,80 @@
+#pragma once
+
+/// @file
+/// This file contains classes dealing with sending/receiving
+/// LCM messages related to the planar manipuland.
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+#include "drake/lcmt_planar_manipuland_status.hpp"
+#include "drake/systems/framework/event_status.h"
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace examples {
+namespace planar_gripper {
+/// Handles lcmt_planar_manipuland_status messages from a LcmSubscriberSystem.
+///
+/// This system has one abstract valued input port which expects a
+/// Value object templated on type `lcmt_planar_manipuland_status`.
+///
+/// This system has one vector valued output port which reports
+/// measured pose (y, z, theta) and velocity (ydot, zdot, thetadot) of the
+/// manipuland.
+///
+/// All ports will continue to output their initial state (typically
+/// zero) until a message is received.
+
+// This should be the update frequency of the mocap system.
+constexpr double kPlanarManipulandStatusPeriod = 0.010;
+
+class PlanarManipulandStatusDecoder : public systems::LeafSystem<double> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PlanarManipulandStatusDecoder)
+
+  PlanarManipulandStatusDecoder();
+
+  ~PlanarManipulandStatusDecoder() {}
+
+ private:
+  void OutputStatus(const systems::Context<double>& context,
+                    systems::BasicVector<double>* output) const;
+
+  /// Event handler of the periodic discrete state update.
+  systems::EventStatus UpdateDiscreteState(
+      const systems::Context<double>& context,
+      systems::DiscreteValues<double>* discrete_state) const;
+};
+
+/**
+ * Creates and outputs lcmt_planar_manipuland_status messages.
+ *
+ * This system has one vector-valued input port containing the current pose
+ * (y, z, theta) and velocity (ẏ, ż, thetadot) of the manipuland, in the order
+ * (y, z, theta, ydot, zdot, thetadot).
+ *
+ * This system has one abstract valued output port that contains a Value object
+ * templated on type `lcmt_planar_manipuland_status`. Note that this system
+ * does NOT actually send this message on an LCM channel. To send the message,
+ * the output of this system should be connected to an input port of a
+ * systems::lcm::LcmPublisherSystem that accepts a Value object templated on
+ * type `lcmt_planar_manipuland_status`.
+ */
+class PlanarManipulandStatusEncoder : public systems::LeafSystem<double> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PlanarManipulandStatusEncoder)
+
+  PlanarManipulandStatusEncoder();
+
+ private:
+  // This is the method to use for the output port allocator.
+  lcmt_planar_manipuland_status MakeOutputStatus() const;
+
+  // This is the calculator method for the output port.
+  void OutputStatus(const systems::Context<double>& context,
+                    lcmt_planar_manipuland_status* output) const;
+};
+
+}  // namespace planar_gripper
+}  // namespace examples
+}  // namespace drake

examples/planar_gripper/test/planar_manipuland_lcm_test.cc

@@ -0,0 +1,55 @@
+#include "drake/examples/planar_gripper/planar_manipuland_lcm.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/systems/framework/diagram.h"
+#include "drake/systems/framework/diagram_builder.h"
+
+namespace drake {
+namespace examples {
+namespace planar_gripper {
+GTEST_TEST(PlanarManipulandLcmTest, PlanarManipulandStatusPassthroughTest) {
+  systems::DiagramBuilder<double> builder;
+  auto status_encoder = builder.AddSystem<PlanarManipulandStatusEncoder>();
+  auto status_decoder = builder.AddSystem<PlanarManipulandStatusDecoder>();
+  builder.Connect(status_decoder->get_output_port(0),
+                  status_encoder->get_input_port(0));
+  builder.ExportInput(status_decoder->get_input_port(0));
+  builder.ExportOutput(status_encoder->get_output_port(0));
+  auto diagram = builder.Build();
+
+  std::unique_ptr<systems::Context<double>> context =
+      diagram->CreateDefaultContext();
+  std::unique_ptr<systems::SystemOutput<double>> output =
+      diagram->AllocateOutput();
+
+  lcmt_planar_manipuland_status status{};
+  status.position[0] = 0.1;
+  status.position[1] = 0.2;
+  status.theta = 0.3;
+  status.velocity[0] = 0.4;
+  status.velocity[1] = 0.5;
+  status.thetadot = 0.6;
+
+  diagram->get_input_port(0).FixValue(context.get(), status);
+
+  std::unique_ptr<systems::DiscreteValues<double>> update =
+      diagram->AllocateDiscreteVariables();
+  update->SetFrom(context->get_mutable_discrete_state());
+  diagram->CalcDiscreteVariableUpdates(*context, update.get());
+  context->get_mutable_discrete_state().SetFrom(*update);
+  diagram->CalcOutput(*context, output.get());
+
+  lcmt_planar_manipuland_status status_out =
+      output->get_data(0)->get_value<lcmt_planar_manipuland_status>();
+
+  EXPECT_DOUBLE_EQ(status.position[0], status_out.position[0]);
+  EXPECT_DOUBLE_EQ(status.position[1], status_out.position[1]);
+  EXPECT_DOUBLE_EQ(status.theta, status_out.theta);
+  EXPECT_DOUBLE_EQ(status.velocity[0], status_out.velocity[0]);
+  EXPECT_DOUBLE_EQ(status.velocity[1], status_out.velocity[1]);
+  EXPECT_DOUBLE_EQ(status.thetadot, status_out.thetadot);
+}
+}  // namespace planar_gripper
+}  // namespace examples
+}  // namespace drake

lcmtypes/BUILD.bazel

@@ -193,6 +193,12 @@ drake_lcm_cc_library(
     ],
 )
 
+drake_lcm_cc_library(
+    name = "planar_manipuland_status",
+    lcm_package = "drake",
+    lcm_srcs = ["lcmt_planar_manipuland_status.lcm"],
+)
+
 drake_lcm_cc_library(
     name = "resolved_contact",
     lcm_package = "drake",
@@ -328,6 +334,7 @@ LCMTYPES_CC = [
     ":joint_pd_override",
     ":manipulator_plan_move_end_effector",
     ":piecewise_polynomial",
+    ":planar_manipuland_status",
     ":plan_eval_debug_info",
     ":point_pair_contact_info_for_viz",
     ":polynomial",

lcmtypes/lcmt_planar_manipuland_status.lcm

@@ -0,0 +1,16 @@
+package drake;
+
+struct lcmt_planar_manipuland_status
+{
+  // The timestamp in microseconds
+  int64_t utime;
+
+  // The translational position of the manipuland.
+  double position[2];
+  // The angle of the manipuland.
+  double theta;
+  // The translational velocity of the manipuland.
+  double velocity[2];
+  // The angular velocity of the manipuland.
+  double thetadot;
+}