fix: refactory and remove c3 state sender

Author: Meow404

examples/plate-balancing/BUILD.bazel

@@ -77,6 +77,7 @@ cc_binary(
         "//systems/controllers/osc:operational_space_control",
         "//systems/framework:lcm_driven_loop",
         "//systems/primitives:radio_parser",
+        "//systems/lcmt_generators:lcmt_generators",
         "@c3//core:c3",
         "@c3//core:options",
         "@c3//systems:systems",
@@ -105,6 +106,7 @@ cc_binary(
         "//systems:system_utils",
         "//systems/controllers",
         "//systems/controllers/osc:operational_space_control",
+        "//systems/lcmt_generators:lcmt_generators",
         "//systems/framework:lcm_driven_loop",
         "//systems/primitives:radio_parser",
         "@drake//:drake_shared_library",
@@ -121,14 +123,15 @@ cc_binary(
     ],
     deps = [
         ":parameters",
+        "//examples/plate-balancing/systems/visualization:lcm_visualization_systems",
         "//common",
         "//multibody:utils",
         "//multibody:visualization_utils",
         "//systems:robot_lcm_systems",
         "//systems:system_utils",
+        "//systems/lcmt_generators:lcmt_generators",
         "//systems/primitives",
         "//systems/trajectory_optimization:lcm_trajectory_systems",
-        "//systems/visualization:lcm_visualization_systems",
         "@c3//lcmtypes:lcmt_c3",
         "@drake//:drake_shared_library",
         "@gflags",

examples/plate-balancing/forward_kinematics_for_lcs.cc

@@ -15,11 +15,11 @@
 #include "examples/plate-balancing/parameters/lcm_channel_config.h"
 #include "examples/plate-balancing/parameters/plate_balancing_config.h"
 #include "examples/plate-balancing/parameters/plate_balancing_target_config.h"
-#include "examples/plate-balancing/systems/c3_state_sender.h"
 #include "examples/plate-balancing/systems/franka_kinematics.h"
 #include "examples/plate-balancing/systems/plate_balancing_target.h"
 #include "multibody/multibody_utils.h"
 #include "systems/framework/lcm_driven_loop.h"
+#include "systems/lcmt_generators/robot_state_generator.h"
 #include "systems/primitives/radio_parser.h"
 #include "systems/robot_lcm_systems.h"
 #include "systems/system_utils.h"
@@ -50,6 +50,7 @@ using dairlib::systems::LcmDrivenLoop;
 using dairlib::systems::ObjectStateReceiver;
 using dairlib::systems::RadioToVector;
 using dairlib::systems::RobotOutputReceiver;
+using dairlib::systems::lcmt_generators::RobotStateGenerator;
 
 namespace dairlib {
 namespace examples {
@@ -131,17 +132,6 @@ int DoMain(int argc, char* argv[]) {
   // Add systems for radio parsing and C3 state handling
   auto radio_to_vector = builder.AddSystem<RadioToVector>();
 
-  // Define state names for C3 state sender
-  std::vector<std::string> state_names = {
-      "end_effector_x",  "end_effector_y", "end_effector_z",  "tray_qw",
-      "tray_qx",         "tray_qy",        "tray_qz",         "tray_x",
-      "tray_y",          "tray_z",         "end_effector_vx", "end_effector_vy",
-      "end_effector_vz", "tray_wx",        "tray_wy",         "tray_wz",
-      "tray_vz",         "tray_vz",        "tray_vz",
-  };
-  auto c3_state_sender =
-      builder.AddSystem<systems::C3StateSender>(3 + 7 + 3 + 6, state_names);
-
   // Add plate balancing target generator
   auto plate_balancing_target =
       builder.AddSystem<systems::PlateBalancingTargetGenerator>(
@@ -166,16 +156,6 @@ int DoMain(int argc, char* argv[]) {
       builder.AddSystem<drake::systems::ConstantVectorSource>(
           VectorXd::Zero(6));
 
-  // Add LCM publishers for C3 state
-  auto c3_actual_state_publisher =
-      builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_c3_state>(
-          lcm_channel_params.c3_actual_state_channel, &lcm,
-          TriggerTypeSet({TriggerType::kForced})));
-  auto c3_target_state_publisher =
-      builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_c3_state>(
-          lcm_channel_params.c3_target_state_channel, &lcm,
-          TriggerTypeSet({TriggerType::kForced})));
-
   // Connect the systems
   builder.Connect(*radio_sub, *radio_to_vector);
   builder.Connect(tray_state_receiver->get_output_port(),
@@ -192,19 +172,29 @@ int DoMain(int argc, char* argv[]) {
                   reduced_order_model_receiver->get_input_port_franka_state());
   builder.Connect(tray_state_receiver->get_output_port(),
                   reduced_order_model_receiver->get_input_port_object_state());
-  builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(),
-                  c3_state_sender->get_input_port_actual_state());
-  builder.Connect(target_state_mux->get_output_port(),
-                  c3_state_sender->get_input_port_target_state());
-  builder.Connect(c3_state_sender->get_output_port_actual_c3_state(),
-                  c3_actual_state_publisher->get_input_port());
-  builder.Connect(c3_state_sender->get_output_port_target_c3_state(),
-                  c3_target_state_publisher->get_input_port());
   builder.Connect(end_effector_zero_velocity_source->get_output_port(),
                   target_state_mux->get_input_port(2));
   builder.Connect(tray_zero_velocity_source->get_output_port(),
                   target_state_mux->get_input_port(3));
 
+  // Define state names for C3 state sender
+  std::vector<std::string> state_names = {
+      "end_effector_x",  "end_effector_y", "end_effector_z",  "tray_qw",
+      "tray_qx",         "tray_qy",        "tray_qz",         "tray_x",
+      "tray_y",          "tray_z",         "end_effector_vx", "end_effector_vy",
+      "end_effector_vz", "tray_wx",        "tray_wy",         "tray_wz",
+      "tray_vz",         "tray_vz",        "tray_vz",
+  };
+  RobotStateGenerator::AddLcmPublisherToBuilder(
+      builder, state_names, false, target_state_mux->get_output_port(),
+      lcm_channel_params.c3_target_state_channel, &lcm,
+      TriggerTypeSet({TriggerType::kForced}));
+  RobotStateGenerator::AddLcmPublisherToBuilder(
+      builder, state_names, true,
+      reduced_order_model_receiver->get_output_port_lcs_state(),
+      lcm_channel_params.c3_actual_state_channel, &lcm,
+      TriggerTypeSet({TriggerType::kForced}));
+
   auto owned_diagram = builder.Build();
   owned_diagram->set_name(("franka_forward_kinematics"));
 

examples/plate-balancing/plate_balancing_c3_controller.cc

@@ -21,11 +21,11 @@
 #include "examples/plate-balancing/parameters/plate_balancing_c3_controller_options.h"
 #include "examples/plate-balancing/parameters/plate_balancing_config.h"
 #include "examples/plate-balancing/parameters/plate_balancing_target_config.h"
-#include "examples/plate-balancing/systems/c3_state_sender.h"
 #include "examples/plate-balancing/systems/franka_kinematics.h"
 #include "examples/plate-balancing/systems/plate_balancing_target.h"
 #include "multibody/multibody_utils.h"
 #include "systems/framework/lcm_driven_loop.h"
+#include "systems/lcmt_generators/robot_state_generator.h"
 #include "systems/primitives/radio_parser.h"
 #include "systems/robot_lcm_systems.h"
 #include "systems/system_utils.h"
@@ -71,6 +71,7 @@ using systems::LcmDrivenLoop;
 using systems::ObjectStateReceiver;
 using systems::RadioToVector;
 using systems::RobotOutputReceiver;
+using systems::lcmt_generators::RobotStateGenerator;
 
 namespace examples {
 namespace plate_balancing {
@@ -277,14 +278,6 @@ int DoMain(std::string plate_balancing_config, bool is_simulation) {
           scene_params.end_effector_name, "tray",
           main_config.include_end_effector_orientation);
 
-  auto c3_target_state_publisher =
-      builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_c3_state>(
-          lcm_channel_params.c3_target_state_channel, &lcm,
-          TriggerTypeSet({TriggerType::kForced})));
-  auto c3_actual_state_publisher =
-      builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_c3_state>(
-          lcm_channel_params.c3_actual_state_channel, &lcm,
-          TriggerTypeSet({TriggerType::kForced})));
   auto radio_sub =
       builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_radio_out>(
           lcm_channel_params.radio_channel, &lcm));
@@ -323,17 +316,6 @@ int DoMain(std::string plate_balancing_config, bool is_simulation) {
   auto controller = AddC3ControllerToBuilder(
       builder, plant_for_lcs, controller_options, solver_options);
 
-  // Add C3 state sender
-  std::vector<std::string> state_names = {
-      "end_effector_x",  "end_effector_y", "end_effector_z",  "tray_qw",
-      "tray_qx",         "tray_qy",        "tray_qz",         "tray_x",
-      "tray_y",          "tray_z",         "end_effector_vx", "end_effector_vy",
-      "end_effector_vz", "tray_wx",        "tray_wy",         "tray_wz",
-      "tray_vz",         "tray_vz",        "tray_vz",
-  };
-  auto c3_state_sender =
-      builder.AddSystem<systems::C3StateSender>(3 + 7 + 3 + 6, state_names);
-
   // Connect systems
   builder.Connect(*radio_sub, *radio_to_vector);
   builder.Connect(franka_state_receiver->get_output_port(),
@@ -356,14 +338,6 @@ int DoMain(std::string plate_balancing_config, bool is_simulation) {
                   lcs_factory->get_input_port_lcs_input());
   builder.Connect(radio_to_vector->get_output_port(),
                   plate_balancing_target->get_input_port_radio());
-  builder.Connect(target_state_mux->get_output_port(),
-                  c3_state_sender->get_input_port_target_state());
-  builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(),
-                  c3_state_sender->get_input_port_actual_state());
-  builder.Connect(c3_state_sender->get_output_port_target_c3_state(),
-                  c3_target_state_publisher->get_input_port());
-  builder.Connect(c3_state_sender->get_output_port_actual_c3_state(),
-                  c3_actual_state_publisher->get_input_port());
 
   // Add C3 output and contact force publishers
   C3OutputGenerator::AddLcmPublisherToBuilder(
@@ -390,6 +364,16 @@ int DoMain(std::string plate_balancing_config, bool is_simulation) {
       builder, object_config, controller->get_output_port_c3_solution(),
       lcm_channel_params.c3_object_channel, &lcm,
       TriggerTypeSet({TriggerType::kForced}));
+  RobotStateGenerator::AddLcmPublisherToBuilder(
+      builder, plant_for_lcs.GetStateNames(), false,
+      target_state_mux->get_output_port(),
+      lcm_channel_params.c3_target_state_channel, &lcm,
+      TriggerTypeSet({TriggerType::kForced}));
+  RobotStateGenerator::AddLcmPublisherToBuilder(
+      builder, plant_for_lcs.GetStateNames(), true,
+      reduced_order_model_receiver->get_output_port_lcs_state(),
+      lcm_channel_params.c3_actual_state_channel, &lcm,
+      TriggerTypeSet({TriggerType::kForced}));
 
   auto owned_diagram = builder.Build();
   owned_diagram->set_name(("run_c3_controller"));

examples/plate-balancing/plate_balancing_visualizer.cc

@@ -2,13 +2,13 @@
 #include <iostream>
 
 #include <c3/lcmt_contact_forces.hpp>
-#include <dairlib/lcmt_c3_state.hpp>
 #include <dairlib/lcmt_timestamped_saved_traj.hpp>
 #include <drake/common/find_resource.h>
 #include <drake/common/yaml/yaml_io.h>
 #include <drake/geometry/drake_visualizer.h>
 #include <drake/geometry/meshcat_visualizer.h>
 #include <drake/geometry/meshcat_visualizer_params.h>
+#include <drake/lcmt_robot_state.hpp>
 #include <drake/multibody/parsing/parser.h>
 #include <drake/systems/analysis/simulator.h>
 #include <drake/systems/framework/diagram_builder.h>
@@ -25,14 +25,14 @@
 #include "examples/plate-balancing/parameters/plate_balancing_config.h"
 #include "examples/plate-balancing/parameters/simulation_config.h"
 #include "examples/plate-balancing/parameters/simulation_scene_config.h"
+#include "examples/plate-balancing/systems/visualization/lcm_visualization_systems.h"
 #include "multibody/com_pose_system.h"
 #include "multibody/multibody_utils.h"
 #include "multibody/visualization_utils.h"
 #include "systems/primitives/subvector_pass_through.h"
 #include "systems/robot_lcm_systems.h"
 #include "systems/system_utils.h"
 #include "systems/trajectory_optimization/lcm_trajectory_systems.h"
-#include "systems/visualization/lcm_visualization_systems.h"
 
 using drake::geometry::MeshcatVisualizer;
 using drake::math::RigidTransform;
@@ -184,10 +184,10 @@ int do_main(int argc, char* argv[]) {
 
   // Create LCM subscribers for C3 states.
   auto c3_state_actual_sub =
-      builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_c3_state>(
+      builder.AddSystem(LcmSubscriberSystem::Make<drake::lcmt_robot_state>(
           lcm_channel_params.c3_actual_state_channel, lcm));
   auto c3_state_target_sub =
-      builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_c3_state>(
+      builder.AddSystem(LcmSubscriberSystem::Make<drake::lcmt_robot_state>(
           lcm_channel_params.c3_target_state_channel, lcm));
 
   // Create a system to convert MultibodyPlant positions to geometry poses.

examples/plate-balancing/systems/BUILD.bazel

@@ -4,7 +4,6 @@ cc_library(
     name = "franka_systems",
     srcs = [],
     deps = [
-        ":c3_state_sender",
         ":end_effector_force_trajectory",
         ":end_effector_orientation_trajectory",
         ":end_effector_position_trajectory",
@@ -79,21 +78,6 @@ cc_library(
     ],
 )
 
-cc_library(
-    name = "c3_state_sender",
-    srcs = [
-        "c3_state_sender.cc",
-    ],
-    hdrs = [
-        "c3_state_sender.h",
-    ],
-    deps = [
-        "//lcmtypes:lcmt_robot",
-        "//systems/framework:vector",
-        "@drake//:drake_shared_library",
-    ],
-)
-
 cc_library(
     name = "franka_kinematics",
     srcs = ["franka_kinematics.cc"],