Skip to content

Commit d50a4b4

Browse files
Meow404Meow404
committed
feat: add lcmt systems
1 parent 119690f commit d50a4b4

23 files changed

+1451
-549
lines changed

‎.bazelrc‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -62,3 +62,5 @@ build:omp --linkopt=-fopenmp
6262

6363
# use python3 by default
6464
build --python_path=python3
65+
66+
build --local_cpu_resources=4

‎WORKSPACE‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -202,6 +202,6 @@ load("@bazel_tools//tools/build_defs/repo:git.bzl", "git_repository")
202202
git_repository(
203203
name = "c3",
204204
remote = "https://github.com/DAIRLab/c3",
205-
commit = "f0d319f71d0a20abf3550bea7855b2a936d8f43e",
205+
commit = "0ff39d9d48137f7502b2bcaa87690e2ee34cb257",
206206
)
207207

‎examples/plate-balancing/BUILD.bazel‎

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -21,10 +21,10 @@ cc_binary(
2121
":parameters",
2222
"//common",
2323
"//examples/plate-balancing/systems:franka_systems",
24-
"//systems:robot_lcm_systems",
2524
"//systems:system_utils",
2625
"//systems/controllers",
2726
"//systems/framework:lcm_driven_loop",
27+
"//systems/lcmt_systems:lcmt_systems",
2828
"//systems/primitives:radio_parser",
2929
"@drake//:drake_shared_library",
3030
"@gflags",
@@ -43,12 +43,12 @@ cc_binary(
4343
"//common",
4444
"//examples/plate-balancing/systems:franka_systems",
4545
"//lcm:lcm_trajectory_saver",
46-
"//systems:robot_lcm_systems",
4746
"//systems:system_utils",
4847
"//systems/controllers",
4948
"//systems/controllers/osc:operational_space_control",
5049
"//systems/framework:lcm_driven_loop",
5150
"//systems/primitives:radio_parser",
51+
"//systems/lcmt_systems:lcmt_systems",
5252
"//systems/trajectory_optimization:lcm_trajectory_systems",
5353
"@drake//:drake_shared_library",
5454
"@gflags",
@@ -71,13 +71,12 @@ cc_binary(
7171
"//common",
7272
"//examples/plate-balancing/systems:franka_systems",
7373
"//lcm:lcm_trajectory_saver",
74-
"//systems:robot_lcm_systems",
7574
"//systems:system_utils",
7675
"//systems/controllers",
7776
"//systems/controllers/osc:operational_space_control",
7877
"//systems/framework:lcm_driven_loop",
7978
"//systems/primitives:radio_parser",
80-
"//systems/lcmt_generators:lcmt_generators",
79+
"//systems/lcmt_systems:lcmt_systems",
8180
"@c3//:c3_shared_library",
8281
"@drake//:drake_shared_library",
8382
"@gflags",
@@ -100,11 +99,10 @@ cc_binary(
10099
"//common",
101100
"//examples/plate-balancing/systems:franka_systems",
102101
"//lcm:lcm_trajectory_saver",
103-
"//systems:robot_lcm_systems",
104102
"//systems:system_utils",
105103
"//systems/controllers",
106104
"//systems/controllers/osc:operational_space_control",
107-
"//systems/lcmt_generators:lcmt_generators",
105+
"//systems/lcmt_systems:lcmt_systems",
108106
"//systems/framework:lcm_driven_loop",
109107
"//systems/primitives:radio_parser",
110108
"@drake//:drake_shared_library",
@@ -125,9 +123,8 @@ cc_binary(
125123
"//common",
126124
"//multibody:utils",
127125
"//multibody:visualization_utils",
128-
"//systems:robot_lcm_systems",
129126
"//systems:system_utils",
130-
"//systems/lcmt_generators:lcmt_generators",
127+
"//systems/lcmt_systems:lcmt_systems",
131128
"//systems/primitives",
132129
"//systems/trajectory_optimization:lcm_trajectory_systems",
133130
"@c3//:c3_shared_library",

‎examples/plate-balancing/forward_kinematics_for_lcs.cc‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,10 @@
1919
#include "examples/plate-balancing/systems/plate_balancing_target.h"
2020
#include "multibody/multibody_utils.h"
2121
#include "systems/framework/lcm_driven_loop.h"
22-
#include "systems/lcmt_generators/robot_state_generator.h"
22+
#include "systems/lcmt_systems/object_state_systems.h"
23+
#include "systems/lcmt_systems/robot_output_systems.h"
24+
#include "systems/lcmt_systems/robot_state_generator.h"
2325
#include "systems/primitives/radio_parser.h"
24-
#include "systems/robot_lcm_systems.h"
2526
#include "systems/system_utils.h"
2627

2728
// Command line flags for config file and simulation mode
@@ -48,10 +49,10 @@ using Eigen::VectorXd;
4849
using dairlib::multibody::MakeNameToPositionsMap;
4950
using dairlib::multibody::MakeNameToVelocitiesMap;
5051
using dairlib::systems::LcmDrivenLoop;
51-
using dairlib::systems::ObjectStateReceiver;
5252
using dairlib::systems::RadioToVector;
53-
using dairlib::systems::RobotOutputReceiver;
54-
using dairlib::systems::lcmt_generators::RobotStateGenerator;
53+
using dairlib::systems::lcmt_systems::ObjectStateConsumer;
54+
using dairlib::systems::lcmt_systems::RobotOutputConsumer;
55+
using dairlib::systems::lcmt_systems::RobotStateGenerator;
5556

5657
namespace dairlib {
5758
namespace examples {
@@ -123,8 +124,8 @@ int DoMain(int argc, char* argv[]) {
123124

124125
// Add state receivers and kinematics system
125126
auto franka_state_receiver =
126-
builder.AddSystem<RobotOutputReceiver>(plant_franka);
127-
auto tray_state_receiver = builder.AddSystem<ObjectStateReceiver>(plant_tray);
127+
builder.AddSystem<RobotOutputConsumer>(plant_franka);
128+
auto tray_state_receiver = builder.AddSystem<ObjectStateConsumer>(plant_tray);
128129
auto reduced_order_model_receiver =
129130
builder.AddSystem<systems::FrankaKinematics>(
130131
plant_franka, franka_context.get(), plant_tray, tray_context.get(),

‎examples/plate-balancing/franka_osc_controller.cc‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,9 @@
1919
#include "systems/controllers/osc/rot_space_tracking_data.h"
2020
#include "systems/controllers/osc/trans_space_tracking_data.h"
2121
#include "systems/framework/lcm_driven_loop.h"
22+
#include "systems/lcmt_systems/robot_input_systems.h"
23+
#include "systems/lcmt_systems/robot_output_systems.h"
2224
#include "systems/primitives/radio_parser.h"
23-
#include "systems/robot_lcm_systems.h"
2425
#include "systems/system_utils.h"
2526
#include "systems/trajectory_optimization/lcm_trajectory_systems.h"
2627

@@ -58,14 +59,14 @@ using systems::GravityCompensationRemover;
5859
using systems::LcmC3TrajectoryReceiver;
5960
using systems::LcmDrivenLoop;
6061
using systems::RadioToVector;
61-
using systems::RobotCommandSender;
62-
using systems::RobotOutputReceiver;
6362
using systems::controllers::ExternalForceTrackingData;
6463
using systems::controllers::JointSpaceTrackingData;
6564
using systems::controllers::OperationalSpaceControl;
6665
using systems::controllers::RelativeTranslationTrackingData;
6766
using systems::controllers::RotTaskSpaceTrackingData;
6867
using systems::controllers::TransTaskSpaceTrackingData;
68+
using systems::lcmt_systems::RobotInputGenerator;
69+
using systems::lcmt_systems::RobotOutputConsumer;
6970

7071
namespace examples {
7172
namespace plate_balancing {
@@ -131,7 +132,7 @@ int DoMain(int argc, char* argv[]) {
131132
drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0");
132133

133134
// Add LCM systems for state, command, and trajectory communication
134-
auto state_receiver = builder.AddSystem<RobotOutputReceiver>(plant);
135+
auto state_receiver = builder.AddSystem<RobotOutputConsumer>(plant);
135136
auto end_effector_trajectory_sub =
136137
builder.AddSystem(LcmSubscriberSystem::Make<c3::lcmt_c3_trajectory>(
137138
lcm_channel_params.c3_actor_channel, &lcm));
@@ -151,8 +152,8 @@ int DoMain(int argc, char* argv[]) {
151152
builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_robot_input>(
152153
lcm_channel_params.osc_channel, &lcm,
153154
TriggerTypeSet({TriggerType::kForced})));
154-
auto franka_command_sender = builder.AddSystem<RobotCommandSender>(plant);
155-
auto osc_command_sender = builder.AddSystem<RobotCommandSender>(plant);
155+
auto franka_command_sender = builder.AddSystem<RobotInputGenerator>(plant);
156+
auto osc_command_sender = builder.AddSystem<RobotInputGenerator>(plant);
156157

157158
// Trajectory generators for end effector position, orientation, and force
158159
auto end_effector_trajectory =

‎examples/plate-balancing/plate_balancing_c3_controller.cc‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -25,9 +25,10 @@
2525
#include "examples/plate-balancing/systems/plate_balancing_target.h"
2626
#include "multibody/multibody_utils.h"
2727
#include "systems/framework/lcm_driven_loop.h"
28-
#include "systems/lcmt_generators/robot_state_generator.h"
28+
#include "systems/lcmt_systems/object_state_systems.h"
29+
#include "systems/lcmt_systems/robot_output_systems.h"
30+
#include "systems/lcmt_systems/robot_state_generator.h"
2931
#include "systems/primitives/radio_parser.h"
30-
#include "systems/robot_lcm_systems.h"
3132
#include "systems/system_utils.h"
3233

3334
// Command-line flags for configuration
@@ -69,10 +70,10 @@ namespace dairlib {
6970
using multibody::MakeNameToPositionsMap;
7071
using multibody::MakeNameToVelocitiesMap;
7172
using systems::LcmDrivenLoop;
72-
using systems::ObjectStateReceiver;
7373
using systems::RadioToVector;
74-
using systems::RobotOutputReceiver;
75-
using systems::lcmt_generators::RobotStateGenerator;
74+
using systems::lcmt_systems::ObjectStateConsumer;
75+
using systems::lcmt_systems::RobotOutputConsumer;
76+
using systems::lcmt_systems::RobotStateGenerator;
7677

7778
namespace examples {
7879
namespace plate_balancing {
@@ -274,8 +275,8 @@ int DoMain(std::string plate_balancing_config, bool is_simulation) {
274275
builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_object_state>(
275276
lcm_channel_params.tray_state_channel, &lcm));
276277
auto franka_state_receiver =
277-
builder.AddSystem<RobotOutputReceiver>(plant_franka);
278-
auto tray_state_receiver = builder.AddSystem<ObjectStateReceiver>(plant_tray);
278+
builder.AddSystem<RobotOutputConsumer>(plant_franka);
279+
auto tray_state_receiver = builder.AddSystem<ObjectStateConsumer>(plant_tray);
279280
auto reduced_order_model_receiver =
280281
builder.AddSystem<systems::FrankaKinematics>(
281282
plant_franka, franka_context.get(), plant_tray, tray_context.get(),

‎examples/plate-balancing/plate_balancing_simulation.cc‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,9 @@
2727
#include "examples/plate-balancing/parameters/simulation_scene_config.h"
2828
#include "examples/plate-balancing/systems/external_force_generator.h"
2929
#include "multibody/multibody_utils.h"
30+
#include "systems/lcmt_systems/common.h"
31+
#include "systems/lcmt_systems/object_state_systems.h"
3032
#include "systems/primitives/radio_parser.h"
31-
#include "systems/robot_lcm_systems.h"
3233
#include "systems/system_utils.h"
3334

3435
using drake::geometry::GeometrySet;
@@ -51,10 +52,10 @@ namespace dairlib {
5152

5253
using multibody::MakeNameToPositionsMap;
5354
using multibody::MakeNameToVelocitiesMap;
54-
using systems::AddActuationRecieverAndStateSenderLcm;
55-
using systems::ObjectStateSender;
5655
using systems::RadioToVector;
5756
using systems::SubvectorPassThrough;
57+
using systems::lcmt_systems::AddActuationRecieverAndStateSenderLcm;
58+
using systems::lcmt_systems::ObjectStateGenerator;
5859

5960
namespace examples {
6061
namespace plate_balancing {
@@ -169,15 +170,15 @@ int DoMain(int argc, char* argv[]) {
169170
franka_index, sim_params.publish_efforts, sim_params.actuator_delay);
170171

171172
// Add tray state sender and publisher.
172-
auto tray_state_sender = builder.AddSystem<ObjectStateSender>(
173+
auto tray_state_sender = builder.AddSystem<ObjectStateGenerator>(
173174
plant, sim_params.publish_object_velocities, tray_index);
174175
auto tray_state_pub =
175176
builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_object_state>(
176177
lcm_channel_params.tray_state_channel, lcm,
177178
1.0 / sim_params.tray_publish_rate));
178179

179180
// Add object state sender and publisher.
180-
auto object_state_sender = builder.AddSystem<ObjectStateSender>(
181+
auto object_state_sender = builder.AddSystem<ObjectStateGenerator>(
181182
plant, sim_params.publish_object_velocities, object_index);
182183
auto object_state_pub =
183184
builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_object_state>(

‎examples/plate-balancing/plate_balancing_visualizer.cc‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,9 @@
2828
#include "multibody/com_pose_system.h"
2929
#include "multibody/multibody_utils.h"
3030
#include "multibody/visualization_utils.h"
31+
#include "systems/lcmt_systems/object_state_systems.h"
32+
#include "systems/lcmt_systems/robot_output_systems.h"
3133
#include "systems/primitives/subvector_pass_through.h"
32-
#include "systems/robot_lcm_systems.h"
3334
#include "systems/system_utils.h"
3435
#include "systems/trajectory_optimization/lcm_trajectory_systems.h"
3536

@@ -142,11 +143,14 @@ int do_main(int argc, char* argv[]) {
142143

143144
// Convert LCM messages to MultibodyPlant state vectors.
144145
auto franka_state_receiver =
145-
builder.AddSystem<systems::RobotOutputReceiver>(plant, franka_index);
146+
builder.AddSystem<systems::lcmt_systems::RobotOutputConsumer>(
147+
plant, franka_index);
146148
auto tray_state_receiver =
147-
builder.AddSystem<systems::ObjectStateReceiver>(plant, tray_index);
149+
builder.AddSystem<systems::lcmt_systems::ObjectStateConsumer>(plant,
150+
tray_index);
148151
auto object_state_receiver =
149-
builder.AddSystem<systems::ObjectStateReceiver>(plant, object_index);
152+
builder.AddSystem<systems::lcmt_systems::ObjectStateConsumer>(
153+
plant, object_index);
150154

151155
// Extract relevant state vectors for visualization.
152156
auto franka_passthrough = builder.AddSystem<systems::SubvectorPassThrough>(

‎systems/lcmt_generators/BUILD.bazel‎

Lines changed: 0 additions & 23 deletions
This file was deleted.

‎systems/lcmt_generators/robot_state_generator.h‎

Lines changed: 0 additions & 55 deletions
This file was deleted.

0 commit comments

Comments
 (0)