Replace robotlocomotion.robot_plan_t with drake.lcmt_robot_plan (Robo…

…tLocomotion#14377) This is a breaking change. EncodeKeyFrames, MoveIkDemoBase, and RobotPlanInterpolator and their associated demos (move_iiwa_ee, move_jaco_rr, kuka_plan_runner, etc.), now operate on lcmt_robot_plan messages, not robot_plan_t messages. EncodeKeyFrames no longer takes an "info" argument -- the message no longer contains the unused snopt integer. This begins to remove Drake's dependency on bot_core.
leido · Nov 20, 2020 · bf2d26e · bf2d26e
1 parent 4db3dbe
commit bf2d26e
Show file tree

Hide file tree

Showing 20 changed files with 74 additions and 103 deletions.
diff --git a/examples/kinova_jaco_arm/BUILD.bazel b/examples/kinova_jaco_arm/BUILD.bazel
@@ -31,8 +31,6 @@ drake_cc_binary(
         "//systems/primitives:demultiplexer",
         "//systems/primitives:multiplexer",
         "@gflags",
-        "@lcmtypes_bot2_core",
-        "@lcmtypes_robotlocomotion",
     ],
 )
 
@@ -71,8 +69,6 @@ drake_cc_binary(
         "//manipulation/util:move_ik_demo_base",
         "//math:geometric_transform",
         "@lcm",
-        "@lcmtypes_bot2_core",
-        "@lcmtypes_robotlocomotion",
     ],
 )
 

diff --git a/examples/kinova_jaco_arm/jaco_controller.cc b/examples/kinova_jaco_arm/jaco_controller.cc
@@ -4,7 +4,6 @@
 
 #include <memory>
 
-#include "robotlocomotion/robot_plan_t.hpp"
 #include <gflags/gflags.h>
 
 #include "drake/common/drake_assert.h"
@@ -13,6 +12,7 @@
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_jaco_command.hpp"
 #include "drake/lcmt_jaco_status.hpp"
+#include "drake/lcmt_robot_plan.hpp"
 #include "drake/manipulation/kinova_jaco/jaco_command_sender.h"
 #include "drake/manipulation/kinova_jaco/jaco_constants.h"
 #include "drake/manipulation/kinova_jaco/jaco_status_receiver.h"
@@ -28,8 +28,6 @@
 #include "drake/systems/primitives/demultiplexer.h"
 #include "drake/systems/primitives/multiplexer.h"
 
-using robotlocomotion::robot_plan_t;
-
 DEFINE_string(urdf, "", "Name of urdf to load");
 DEFINE_int32(num_joints,
              drake::manipulation::kinova_jaco::kJacoDefaultArmNumJoints,
@@ -57,8 +55,8 @@ int DoMain() {
   lcm::DrakeLcm lcm;
   systems::DiagramBuilder<double> builder;
 
-  auto plan_sub =
-      builder.AddSystem(systems::lcm::LcmSubscriberSystem::Make<robot_plan_t>(
+  auto plan_sub = builder.AddSystem(
+      systems::lcm::LcmSubscriberSystem::Make<lcmt_robot_plan>(
           kLcmPlanChannel, &lcm));
 
   const std::string urdf =
@@ -182,7 +180,7 @@ int DoMain() {
       &status_context, first_status);
 
   // Run forever, using the lcmt_jaco_status message to dictate when simulation
-  // time advances.  The robot_plan_t message is handled whenever the next
+  // time advances.  The lcmt_robot_plan message is handled whenever the next
   // lcmt_jaco_status occurs.
   drake::log()->info("Controller started");
   while (true) {

diff --git a/examples/kinova_jaco_arm/move_jaco_ee.cc b/examples/kinova_jaco_arm/move_jaco_ee.cc
@@ -19,7 +19,7 @@ DEFINE_string(urdf, "", "Name of urdf to load");
 DEFINE_string(lcm_status_channel, "KINOVA_JACO_STATUS",
               "Channel on which to listen for lcmt_jaco_status messages.");
 DEFINE_string(lcm_plan_channel, "COMMITTED_ROBOT_PLAN",
-              "Channel on which to send robot_plan_t messages.");
+              "Channel on which to send lcmt_robot_plan messages.");
 DEFINE_double(x, 0.3, "x coordinate (meters) to move to");
 DEFINE_double(y, -0.26, "y coordinate (meters) to move to");
 DEFINE_double(z, 0.5, "z coordinate (meters) to move to");
@@ -67,7 +67,7 @@ int DoMain() {
         }
         demo.HandleStatus(jaco_q);
         if (demo.status_count() == 1) {
-          std::optional<robotlocomotion::robot_plan_t> plan = demo.Plan(pose);
+          std::optional<lcmt_robot_plan> plan = demo.Plan(pose);
           if (plan.has_value()) {
             lc.publish(FLAGS_lcm_plan_channel, &plan.value());
           }

diff --git a/examples/kuka_iiwa_arm/BUILD.bazel b/examples/kuka_iiwa_arm/BUILD.bazel
@@ -133,9 +133,9 @@ drake_cc_binary(
         "//common:find_resource",
         "//common/trajectories:piecewise_polynomial",
         "//lcmtypes:iiwa",
+        "//lcmtypes:robot_plan",
         "//multibody/parsing",
         "//multibody/plant",
-        "@lcmtypes_robotlocomotion",
     ],
 )
 
@@ -153,8 +153,6 @@ drake_cc_binary(
         "//manipulation/util:move_ik_demo_base",
         "//math:geometric_transform",
         "@lcm",
-        "@lcmtypes_bot2_core",
-        "@lcmtypes_robotlocomotion",
     ],
 )
 

diff --git a/examples/kuka_iiwa_arm/iiwa_controller.cc b/examples/kuka_iiwa_arm/iiwa_controller.cc
@@ -6,7 +6,6 @@
 #include <iostream>
 #include <memory>
 
-#include "robotlocomotion/robot_plan_t.hpp"
 #include <gflags/gflags.h>
 
 #include "drake/common/find_resource.h"
@@ -15,15 +14,14 @@
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_iiwa_command.hpp"
 #include "drake/lcmt_iiwa_status.hpp"
+#include "drake/lcmt_robot_plan.hpp"
 #include "drake/systems/analysis/simulator.h"
 #include "drake/systems/framework/context.h"
 #include "drake/systems/framework/diagram.h"
 #include "drake/systems/framework/diagram_builder.h"
 #include "drake/systems/lcm/lcm_publisher_system.h"
 #include "drake/systems/lcm/lcm_subscriber_system.h"
 
-using robotlocomotion::robot_plan_t;
-
 DEFINE_string(urdf, "", "Name of urdf to load");
 DEFINE_string(interp_type, "cubic",
               "Robot plan interpolation type. Can be {zoh, foh, cubic, pchip}");
@@ -32,7 +30,7 @@ DEFINE_string(lcm_status_channel, "IIWA_STATUS",
 DEFINE_string(lcm_command_channel, "IIWA_COMMAND",
               "Channel on which to publish lcmt_iiwa_command messages.");
 DEFINE_string(lcm_plan_channel, "COMMITTED_ROBOT_PLAN",
-              "Channel on which to listen for robot_plan_t messages.");
+              "Channel on which to listen for lcmt_robot_plan messages.");
 
 namespace drake {
 namespace examples {
@@ -85,8 +83,8 @@ int DoMain() {
       builder.AddSystem<LcmPlanInterpolator>(urdf, interpolator_type);
   const int kNumJoints = plan_interpolator->num_joints();
 
-  auto plan_sub =
-      builder.AddSystem(systems::lcm::LcmSubscriberSystem::Make<robot_plan_t>(
+  auto plan_sub = builder.AddSystem(
+      systems::lcm::LcmSubscriberSystem::Make<lcmt_robot_plan>(
           kLcmPlanChannel, &lcm));
   plan_sub->set_name("plan_sub");
 
@@ -129,7 +127,7 @@ int DoMain() {
       &plan_interpolator_context, status_sub.message());
 
   // Run forever, using the lcmt_iiwa_status message to dictate when simulation
-  // time advances.  The robot_plan_t message is handled whenever the next
+  // time advances.  The lcmt_robot_plan message is handled whenever the next
   // lcmt_iiwa_status occurs.
   drake::log()->info("Controller started");
   while (true) {

diff --git a/examples/kuka_iiwa_arm/kuka_plan_runner.cc b/examples/kuka_iiwa_arm/kuka_plan_runner.cc
@@ -1,7 +1,7 @@
 /// @file
 ///
 /// kuka_plan_runner is designed to wait for LCM messages constraining
-/// a robot_plan_t message, and then execute the plan on an iiwa arm
+/// a lcmt_robot_plan message, and then execute the plan on an iiwa arm
 /// (also communicating via LCM using the
 /// lcmt_iiwa_command/lcmt_iiwa_status messages).
 ///
@@ -15,13 +15,13 @@
 #include <memory>
 
 #include "lcm/lcm-cpp.hpp"
-#include "robotlocomotion/robot_plan_t.hpp"
 
 #include "drake/common/drake_assert.h"
 #include "drake/common/find_resource.h"
 #include "drake/common/trajectories/piecewise_polynomial.h"
 #include "drake/lcmt_iiwa_command.hpp"
 #include "drake/lcmt_iiwa_status.hpp"
+#include "drake/lcmt_robot_plan.hpp"
 #include "drake/multibody/parsing/parser.h"
 #include "drake/multibody/plant/multibody_plant.h"
 
@@ -112,7 +112,7 @@ class RobotPlanRunner {
   }
 
   void HandlePlan(const ::lcm::ReceiveBuffer*, const std::string&,
-                  const robotlocomotion::robot_plan_t* plan) {
+                  const lcmt_robot_plan* plan) {
     std::cout << "New plan received." << std::endl;
     if (iiwa_status_.utime == -1) {
       std::cout << "Discarding plan, no status message received yet"
@@ -166,7 +166,7 @@ class RobotPlanRunner {
   }
 
   void HandleStop(const ::lcm::ReceiveBuffer*, const std::string&,
-                  const robotlocomotion::robot_plan_t*) {
+                  const lcmt_robot_plan*) {
     std::cout << "Received stop command. Discarding plan." << std::endl;
     plan_.reset();
   }

diff --git a/examples/kuka_iiwa_arm/move_iiwa_ee.cc b/examples/kuka_iiwa_arm/move_iiwa_ee.cc
@@ -7,20 +7,20 @@
 /// during, and after the commanded motion.
 
 #include "lcm/lcm-cpp.hpp"
-#include "robotlocomotion/robot_plan_t.hpp"
 #include <gflags/gflags.h>
 
 #include "drake/common/find_resource.h"
 #include "drake/examples/kuka_iiwa_arm/iiwa_common.h"
 #include "drake/lcmt_iiwa_status.hpp"
+#include "drake/lcmt_robot_plan.hpp"
 #include "drake/manipulation/util/move_ik_demo_base.h"
 #include "drake/math/rigid_transform.h"
 
 DEFINE_string(urdf, "", "Name of urdf to load");
 DEFINE_string(lcm_status_channel, "IIWA_STATUS",
               "Channel on which to listen for lcmt_iiwa_status messages.");
 DEFINE_string(lcm_plan_channel, "COMMITTED_ROBOT_PLAN",
-              "Channel on which to send robot_plan_t messages.");
+              "Channel on which to send lcmt_robot_plan messages.");
 DEFINE_double(x, 0., "x coordinate to move to");
 DEFINE_double(y, 0., "y coordinate to move to");
 DEFINE_double(z, 0., "z coordinate to move to");
@@ -61,7 +61,7 @@ int DoMain() {
         }
         demo.HandleStatus(iiwa_q);
         if (demo.status_count() == 1) {
-          std::optional<robotlocomotion::robot_plan_t> plan = demo.Plan(pose);
+          std::optional<lcmt_robot_plan> plan = demo.Plan(pose);
           if (plan.has_value()) {
             lc.publish(FLAGS_lcm_plan_channel, &plan.value());
           }

diff --git a/lcmtypes/BUILD.bazel b/lcmtypes/BUILD.bazel
@@ -234,6 +234,15 @@ drake_lcm_cc_library(
     lcm_srcs = ["lcmt_resolved_contact.lcm"],
 )
 
+drake_lcm_cc_library(
+    name = "robot_plan",
+    lcm_package = "drake",
+    lcm_srcs = [
+        "lcmt_robot_plan.lcm",
+        "lcmt_robot_state.lcm",
+    ],
+)
+
 drake_lcm_cc_library(
     name = "scope",
     lcm_package = "drake",
@@ -389,6 +398,7 @@ LCMTYPES_CC = [
     ":qp_controller_input",
     ":qp_input",
     ":resolved_contact",
+    ":robot_plan",
     ":schunk",
     ":scope",
     ":support_data",

diff --git a/lcmtypes/lcmt_robot_plan.lcm b/lcmtypes/lcmt_robot_plan.lcm
@@ -0,0 +1,8 @@
+package drake;
+
+struct lcmt_robot_plan
+{
+  int64_t utime;
+  int32_t num_states;
+  lcmt_robot_state plan[num_states]; // each individual state is also timed.
+}
diff --git a/lcmtypes/lcmt_robot_state.lcm b/lcmtypes/lcmt_robot_state.lcm
@@ -1,20 +1,9 @@
 package drake;
 
-struct lcmt_robot_state {
-  // The timestamp in milliseconds.
-  int64_t timestamp;
-
-  int32_t num_robots;
-  string robot_name[num_robots];
-
-  int32_t num_joints;
-
-  // The following variable defines which robot each joint is
-  // associated with. The range of the values is [0, num_robots).
-  int32_t joint_robot[num_joints];
-
+struct lcmt_robot_state
+{
+  int64_t utime;
+  int16_t num_joints;
   string joint_name[num_joints];
-
-  float joint_position[num_joints];  // q
-  float joint_velocity[num_joints];  // qd
+  float joint_position[num_joints];
 }
diff --git a/manipulation/planner/BUILD.bazel b/manipulation/planner/BUILD.bazel
@@ -63,10 +63,9 @@ drake_cc_library(
     hdrs = ["robot_plan_interpolator.h"],
     deps = [
         "//common/trajectories:piecewise_polynomial",
+        "//lcmtypes:robot_plan",
         "//multibody/parsing",
         "//systems/framework:leaf_system",
-        "@lcmtypes_bot2_core",
-        "@lcmtypes_robotlocomotion",
     ],
 )
 
@@ -121,7 +120,6 @@ drake_cc_googletest(
         ":robot_plan_interpolator",
         "//common:find_resource",
         "//systems/framework",
-        "@lcmtypes_bot2_core",
         "@lcmtypes_robotlocomotion",
     ],
 )

diff --git a/manipulation/planner/robot_plan_interpolator.cc b/manipulation/planner/robot_plan_interpolator.cc
@@ -8,14 +8,11 @@
 #include <utility>
 #include <vector>
 
-#include "robotlocomotion/robot_plan_t.hpp"
-
 #include "drake/common/text_logging.h"
 #include "drake/common/trajectories/piecewise_polynomial.h"
+#include "drake/lcmt_robot_plan.hpp"
 #include "drake/multibody/parsing/parser.h"
 
-using robotlocomotion::robot_plan_t;
-
 namespace drake {
 namespace manipulation {
 namespace planner {
@@ -41,7 +38,7 @@ RobotPlanInterpolator::RobotPlanInterpolator(
     const std::string& model_path, const InterpolatorType interp_type,
     double update_interval)
     : plan_input_port_(this->DeclareAbstractInputPort(
-          "plan", Value<robot_plan_t>()).get_index()),
+          "plan", Value<lcmt_robot_plan>()).get_index()),
       interp_type_(interp_type) {
   multibody::Parser(&plant_).AddModelFromFile(model_path);
 
@@ -179,8 +176,8 @@ void RobotPlanInterpolator::DoCalcUnrestrictedUpdate(
     systems::State<double>* state) const {
   PlanData& plan =
       state->get_mutable_abstract_state<PlanData>(plan_index_);
-  const robot_plan_t& plan_input =
-      get_plan_input_port().Eval<robot_plan_t>(context);
+  const lcmt_robot_plan& plan_input =
+      get_plan_input_port().Eval<lcmt_robot_plan>(context);
 
   // I (sammy-tri) wish I could think of a more effective way to
   // determine that a new message has arrived, but unfortunately

diff --git a/manipulation/planner/robot_plan_interpolator.h b/manipulation/planner/robot_plan_interpolator.h
@@ -22,7 +22,7 @@ enum class InterpolatorType {
 };
 
 /// This class implements a source of joint positions for a robot.
-/// It has one input port for robot_plan_t messages containing a
+/// It has one input port for lcmt_robot_plan messages containing a
 /// plan to follow.
 ///
 /// The system has two output ports, one with the current desired

diff --git a/manipulation/planner/test/robot_plan_interpolator_test.cc b/manipulation/planner/test/robot_plan_interpolator_test.cc
@@ -1,9 +1,9 @@
 #include "drake/manipulation/planner/robot_plan_interpolator.h"
 
-#include "robotlocomotion/robot_plan_t.hpp"
 #include <gtest/gtest.h>
 
 #include "drake/common/find_resource.h"
+#include "drake/lcmt_robot_plan.hpp"
 
 namespace drake {
 namespace manipulation {
@@ -78,12 +78,11 @@ void DoTrajectoryTest(InterpolatorType interp_type) {
 
   const int num_time_steps = q.cols();
 
-  // Encode into a robot_plan_t structure.
-  robotlocomotion::robot_plan_t plan{};
+  // Encode into an lcmt_robot_plan structure.
+  lcmt_robot_plan plan{};
   plan.num_states = num_time_steps;
-  const bot_core::robot_state_t default_robot_state{};
+  const lcmt_robot_state default_robot_state{};
   plan.plan.resize(num_time_steps, default_robot_state);
-  plan.plan_info.resize(num_time_steps, 0);
 
   std::vector<std::string> joint_names;
   for (int i = 0; i < dut.plant().num_joints(); ++i) {
@@ -97,14 +96,12 @@ void DoTrajectoryTest(InterpolatorType interp_type) {
   ASSERT_EQ(joint_names.size(), kNumJoints);
 
   for (int i = 0; i < num_time_steps; i++) {
-    bot_core::robot_state_t& step = plan.plan[i];
+    lcmt_robot_state& step = plan.plan[i];
     step.utime = t[i] * 1e6;
     step.num_joints = q.rows();
     step.joint_name = joint_names;
     for (int j = 0; j < step.num_joints; j++) {
       step.joint_position.push_back(q(j, i));
-      step.joint_velocity.push_back(0);
-      step.joint_effort.push_back(0);
     }
   }