From 4242e45ad60bdd93aad5f99956ffea54a7963f2b Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Thu, 5 Dec 2019 13:17:22 -0800 Subject: [PATCH] Add the lcm utility for planar manipuland. (#12439) Add the lcm utility for planar manipuland. --- examples/planar_gripper/BUILD.bazel | 20 +++++ .../planar_gripper/planar_manipuland_lcm.cc | 76 ++++++++++++++++++ .../planar_gripper/planar_manipuland_lcm.h | 80 +++++++++++++++++++ .../test/planar_manipuland_lcm_test.cc | 55 +++++++++++++ lcmtypes/BUILD.bazel | 7 ++ lcmtypes/lcmt_planar_manipuland_status.lcm | 16 ++++ 6 files changed, 254 insertions(+) create mode 100644 examples/planar_gripper/planar_manipuland_lcm.cc create mode 100644 examples/planar_gripper/planar_manipuland_lcm.h create mode 100644 examples/planar_gripper/test/planar_manipuland_lcm_test.cc create mode 100644 lcmtypes/lcmt_planar_manipuland_status.lcm diff --git a/examples/planar_gripper/BUILD.bazel b/examples/planar_gripper/BUILD.bazel index 740b7dd0bae8..718d3e86370e 100644 --- a/examples/planar_gripper/BUILD.bazel +++ b/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() diff --git a/examples/planar_gripper/planar_manipuland_lcm.cc b/examples/planar_gripper/planar_manipuland_lcm.cc new file mode 100644 index 000000000000..5dc101062db8 --- /dev/null +++ b/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(6), + &PlanarManipulandStatusDecoder::OutputStatus); + this->DeclareAbstractInputPort("manipuland_state", + Value{}); + 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& context, + systems::DiscreteValues* discrete_state) const { + const AbstractValue* input = this->EvalAbstractInput(context, 0); + DRAKE_ASSERT(input != nullptr); + const auto& status = input->get_value(); + + systems::BasicVector& 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& context, + systems::BasicVector* output) const { + Eigen::VectorBlock> 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& context, + lcmt_planar_manipuland_status* output) const { + output->utime = context.get_time() * 1e6; + const systems::BasicVector* 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 diff --git a/examples/planar_gripper/planar_manipuland_lcm.h b/examples/planar_gripper/planar_manipuland_lcm.h new file mode 100644 index 000000000000..3647d287d8b1 --- /dev/null +++ b/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 { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PlanarManipulandStatusDecoder) + + PlanarManipulandStatusDecoder(); + + ~PlanarManipulandStatusDecoder() {} + + private: + void OutputStatus(const systems::Context& context, + systems::BasicVector* output) const; + + /// Event handler of the periodic discrete state update. + systems::EventStatus UpdateDiscreteState( + const systems::Context& context, + systems::DiscreteValues* 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 { + 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& context, + lcmt_planar_manipuland_status* output) const; +}; + +} // namespace planar_gripper +} // namespace examples +} // namespace drake diff --git a/examples/planar_gripper/test/planar_manipuland_lcm_test.cc b/examples/planar_gripper/test/planar_manipuland_lcm_test.cc new file mode 100644 index 000000000000..d3ce0a67d3c0 --- /dev/null +++ b/examples/planar_gripper/test/planar_manipuland_lcm_test.cc @@ -0,0 +1,55 @@ +#include "drake/examples/planar_gripper/planar_manipuland_lcm.h" + +#include + +#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 builder; + auto status_encoder = builder.AddSystem(); + auto status_decoder = builder.AddSystem(); + 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> context = + diagram->CreateDefaultContext(); + std::unique_ptr> 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> 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(); + + 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 diff --git a/lcmtypes/BUILD.bazel b/lcmtypes/BUILD.bazel index 368b42e6e0fa..239fb787a283 100644 --- a/lcmtypes/BUILD.bazel +++ b/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", diff --git a/lcmtypes/lcmt_planar_manipuland_status.lcm b/lcmtypes/lcmt_planar_manipuland_status.lcm new file mode 100644 index 000000000000..1bd30b18b5b7 --- /dev/null +++ b/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; +}