recfactor: cleanup

Author: Meow404

.bazelrc

@@ -52,7 +52,7 @@ build --action_env=LD_LIBRARY_PATH=
 # customizations somehow.
 
 # build with snopt
-# build --define=WITH_SNOPT=ON
+build --define=WITH_SNOPT=ON
 build --define=WITH_GUROBI=ON
 build --define=NO_CLARABEL=ON
 
@@ -62,5 +62,3 @@ build:omp --linkopt=-fopenmp
 
 # use python3 by default
 build --python_path=python3
-
-build --local_cpu_resources=4

WORKSPACE

@@ -198,17 +198,10 @@ http_archive(
     ],
 )
 
-# load("@bazel_tools//tools/build_defs/repo:git.bzl", "git_repository")
-# git_repository(
-#     name = "c3",
-#     remote = "https://github.com/DAIRLab/c3",
-#     # branch = "stephen/dairlib-plate-balancing-example",
-#     commit = "720d12bab40d011522afe3cdb7c42ad6334a0a47",
-# )
-
-load("@bazel_tools//tools/build_defs/repo:local.bzl", "local_repository")
-local_repository(
+load("@bazel_tools//tools/build_defs/repo:git.bzl", "git_repository")
+git_repository(
     name = "c3",
-    path = "../c3"
+    remote = "https://github.com/DAIRLab/c3",
+    commit = "84ad88911358b93e44ef487730d3aae8e5f01edc",
 )
 

examples/plate-balancing/config/LCM/lcm_channels_simulation.yaml

@@ -7,11 +7,11 @@ franka_input_echo: FRANKA_INPUT_ECHO
 osc_channel: OSC_FRANKA
 osc_debug_channel: OSC_DEBUG_FRANKA
 
-c3_actor_channel: C3_TRAJECTORY_ACTOR # lcmt_timestamped_saved_traj
-c3_object_channel: C3_TRAJECTORY_TRAY # lcmt_timestamped_saved_traj
+c3_actor_channel: C3_TRAJECTORY_ACTOR # lcmt_c3_trajectory
+c3_object_channel: C3_TRAJECTORY_TRAY # lcmt_c3_trajectory
 c3_force_channel: C3_FORCES
 c3_debug_output_channel: C3_DEBUG
-c3_target_state_channel: C3_TARGET # lcmt_c3_state
-c3_actual_state_channel: C3_ACTUAL # lcmt_c3_state
+c3_target_state_channel: C3_TARGET # lcmt_robot_state
+c3_actual_state_channel: C3_ACTUAL # lcmt_robot_state
 
 radio_channel: RADIO

examples/plate-balancing/forward_kinematics_for_lcs.cc

@@ -24,6 +24,7 @@
 #include "systems/robot_lcm_systems.h"
 #include "systems/system_utils.h"
 
+// Command line flags for config file and simulation mode
 DEFINE_string(plate_balancing_config,
               "examples/plate-balancing/config/plate_balancing_config.yaml",
               "Controller settings such as channels. Attempting to minimize "
@@ -56,11 +57,12 @@ namespace dairlib {
 namespace examples {
 namespace plate_balancing {
 
+// Main function to build and run the plate balancing forward kinematics system
 int DoMain(int argc, char* argv[]) {
   gflags::ParseCommandLineFlags(&argc, &argv, true);
   drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0");
 
-  // Load parameters from YAML files
+  // Load configuration and parameters from YAML files
   drake::yaml::LoadYamlOptions yaml_options;
   yaml_options.allow_yaml_with_no_cpp = true;
 
@@ -77,7 +79,7 @@ int DoMain(int argc, char* argv[]) {
           FLAGS_simulation ? main_config.lcm_simulation_settings_file
                            : main_config.lcm_hardware_settings_file);
 
-  // Build the MultibodyPlant for Franka
+  // Build MultibodyPlant for Franka robot
   DiagramBuilder<double> plant_builder;
   MultibodyPlant<double> plant_franka(0.0);
   Parser parser_franka(&plant_franka, nullptr);
@@ -86,7 +88,7 @@ int DoMain(int argc, char* argv[]) {
       parser_franka.AddModels(
           FindResourceOrThrow(scene_params.end_effector_model))[0];
 
-  // Weld frames for Franka
+  // Weld Franka base and end effector frames
   RigidTransform<double> X_WI = RigidTransform<double>::Identity();
   plant_franka.WeldFrames(plant_franka.world_frame(),
                           plant_franka.GetFrameByName("panda_link0"), X_WI);
@@ -101,14 +103,14 @@ int DoMain(int argc, char* argv[]) {
   plant_franka.Finalize();
   auto franka_context = plant_franka.CreateDefaultContext();
 
-  // Build the MultibodyPlant for the tray
+  // Build MultibodyPlant for tray object
   MultibodyPlant<double> plant_tray(0.0);
   Parser parser_tray(&plant_tray, nullptr);
   parser_tray.AddModels(scene_params.object_models[0]);
   plant_tray.Finalize();
   auto tray_context = plant_tray.CreateDefaultContext();
 
-  // Build the overall diagram
+  // Build the overall system diagram
   DiagramBuilder<double> builder;
 
   // Add LCM subscribers for tray and radio state
@@ -129,7 +131,7 @@ int DoMain(int argc, char* argv[]) {
           scene_params.end_effector_name, "tray",
           main_config.include_end_effector_orientation);
 
-  // Add systems for radio parsing and C3 state handling
+  // Add system to parse radio input
   auto radio_to_vector = builder.AddSystem<RadioToVector>();
 
   // Add plate balancing target generator
@@ -143,7 +145,7 @@ int DoMain(int argc, char* argv[]) {
       target_config.third_target[main_config.scene_index],
       target_config.x_scale, target_config.y_scale, target_config.z_scale);
 
-  // Add multiplexer for target state
+  // Add multiplexer for target state (combines multiple input vectors)
   std::vector<int> input_sizes = {3, 7, 3, 6};
   auto target_state_mux =
       builder.AddSystem<drake::systems::Multiplexer>(input_sizes);
@@ -156,7 +158,7 @@ int DoMain(int argc, char* argv[]) {
       builder.AddSystem<drake::systems::ConstantVectorSource>(
           VectorXd::Zero(6));
 
-  // Connect the systems
+  // Connect all systems in the diagram
   builder.Connect(*radio_sub, *radio_to_vector);
   builder.Connect(tray_state_receiver->get_output_port(),
                   plate_balancing_target->get_input_port_tray_state());
@@ -177,14 +179,15 @@ int DoMain(int argc, char* argv[]) {
   builder.Connect(tray_zero_velocity_source->get_output_port(),
                   target_state_mux->get_input_port(3));
 
-  // Define state names for C3 state sender
+  // Define state names for C3 state LCM publishers
   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",
   };
+  // Add LCM publishers for target and actual C3 state
   RobotStateGenerator::AddLcmPublisherToBuilder(
       builder, state_names, false, target_state_mux->get_output_port(),
       lcm_channel_params.c3_target_state_channel, &lcm,
@@ -198,12 +201,13 @@ int DoMain(int argc, char* argv[]) {
   auto owned_diagram = builder.Build();
   owned_diagram->set_name(("franka_forward_kinematics"));
 
-  // Run lcm-driven simulation
+  // Run LCM-driven simulation loop
   LcmDrivenLoop<dairlib::lcmt_robot_output> loop(
       &lcm, std::move(owned_diagram), franka_state_receiver,
       lcm_channel_params.franka_state_channel, true);
   DrawAndSaveDiagramGraph(*loop.get_diagram());
 
+  // Wait for tray state message before starting simulation
   LcmHandleSubscriptionsUntil(
       &lcm, [&]() { return tray_state_sub->GetInternalMessageCount() > 1; });
   loop.Simulate();
@@ -214,6 +218,7 @@ int DoMain(int argc, char* argv[]) {
 }  // namespace examples
 }  // namespace dairlib
 
+// Entry point
 int main(int argc, char* argv[]) {
   return dairlib::examples::plate_balancing::DoMain(argc, argv);
 }

examples/plate-balancing/franka_osc_controller.cc

@@ -1,7 +1,7 @@
 #include <dairlib/lcmt_radio_out.hpp>
-#include <dairlib/lcmt_timestamped_saved_traj.hpp>
 #include <gflags/gflags.h>
 
+// Core includes for configuration, system building, and OSC
 #include "common/eigen_utils.h"
 #include "examples/plate-balancing/parameters/lcm_channel_config.h"
 #include "examples/plate-balancing/parameters/osc_controller_config.h"
@@ -44,11 +44,11 @@ using Eigen::MatrixXd;
 using Eigen::Vector3d;
 using Eigen::VectorXd;
 
+// Command-line flags for configuration
 DEFINE_string(plate_balancing_config,
               "examples/plate-balancing/config/plate_balancing_config.yaml",
-              "Controller settings such as channels. Attempting to minimize "
-              "number of gflags");
-DEFINE_bool(simulation, true, "Running in simulation or hardware");
+              "YAML file specifying controller and channel settings.");
+DEFINE_bool(simulation, true, "Run in simulation (true) or hardware (false)");
 
 namespace dairlib {
 
@@ -70,10 +70,11 @@ using systems::controllers::TransTaskSpaceTrackingData;
 namespace examples {
 namespace plate_balancing {
 
+// Entry point for the OSC controller application
 int DoMain(int argc, char* argv[]) {
   gflags::ParseCommandLineFlags(&argc, &argv, true);
 
-  // Load parameters from YAML files
+  // Load all configuration parameters from YAML files
   drake::yaml::LoadYamlOptions yaml_options;
   yaml_options.allow_yaml_with_no_cpp = true;
   PlateBalancingConfig main_config =
@@ -94,20 +95,20 @@ int DoMain(int argc, char* argv[]) {
           FindResourceOrThrow(main_config.osc_osqp_setting_file))
           .GetAsSolverOptions(drake::solvers::OsqpSolver::id());
 
-  // Build the Drake diagram
+  // Build the system diagram for the OSC controller
   DiagramBuilder<double> builder;
 
-  // Create and configure the MultibodyPlant
+  // Construct the MultibodyPlant for the robot and parse models
   drake::multibody::MultibodyPlant<double> plant(0.0);
   Parser parser(&plant, nullptr);
   parser.AddModelsFromUrl(controller_config.franka_model);
 
-  // Weld the robot to the world frame
+  // Weld the robot base to the world frame
   RigidTransform<double> X_WI = RigidTransform<double>::Identity();
   plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"),
                    X_WI);
 
-  // Add the end effector if specified in the config
+  // Optionally add and weld the end effector, if specified in config
   if (!controller_config.end_effector_name.empty()) {
     drake::multibody::ModelInstanceIndex end_effector_index = parser.AddModels(
         FindResourceOrThrow(controller_config.end_effector_model))[0];
@@ -126,13 +127,13 @@ int DoMain(int argc, char* argv[]) {
   plant.Finalize();
   auto plant_context = plant.CreateDefaultContext();
 
-  // Create LCM interface
+  // Create LCM interface for communication
   drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0");
 
-  // Add LCM systems for communication
+  // Add LCM systems for state, command, and trajectory communication
   auto state_receiver = builder.AddSystem<RobotOutputReceiver>(plant);
-  auto end_effector_trajectory_sub = builder.AddSystem(
-      LcmSubscriberSystem::Make<c3::lcmt_c3_trajectory>(
+  auto end_effector_trajectory_sub =
+      builder.AddSystem(LcmSubscriberSystem::Make<c3::lcmt_c3_trajectory>(
           lcm_channel_params.c3_actor_channel, &lcm));
   auto end_effector_position_receiver =
       builder.AddSystem<LcmC3TrajectoryReceiver>(
@@ -152,6 +153,8 @@ int DoMain(int argc, char* argv[]) {
           TriggerTypeSet({TriggerType::kForced})));
   auto franka_command_sender = builder.AddSystem<RobotCommandSender>(plant);
   auto osc_command_sender = builder.AddSystem<RobotCommandSender>(plant);
+
+  // Trajectory generators for end effector position, orientation, and force
   auto end_effector_trajectory =
       builder.AddSystem<systems::EndEffectorTrajectoryGenerator>(
           controller_config.neutral_position);
@@ -164,6 +167,8 @@ int DoMain(int argc, char* argv[]) {
       controller_config.track_end_effector_orientation);
   auto end_effector_force_trajectory =
       builder.AddSystem<systems::EndEffectorForceTrajectoryGenerator>();
+
+  // Radio input for remote control
   auto radio_sub =
       builder.AddSystem(LcmSubscriberSystem::Make<dairlib::lcmt_radio_out>(
           lcm_channel_params.radio_channel, &lcm));
@@ -173,7 +178,7 @@ int DoMain(int argc, char* argv[]) {
   auto osc = builder.AddSystem<OperationalSpaceControl>(
       plant, plant_context.get(), false);
 
-  // Add debug publisher if specified
+  // Optionally add debug publisher for OSC internal state
   if (controller_config.publish_debug_info) {
     auto osc_debug_pub =
         builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_osc_output>(
@@ -183,7 +188,7 @@ int DoMain(int argc, char* argv[]) {
                     osc_debug_pub->get_input_port());
   }
 
-  // Configure tracking data for OSC
+  // Configure tracking data for OSC: position, orientation, force, and joint
   auto end_effector_position_tracking_data =
       std::make_unique<TransTaskSpaceTrackingData>(
           "end_effector_target", controller_config.K_p_end_effector,
@@ -195,6 +200,7 @@ int DoMain(int argc, char* argv[]) {
       controller_config.end_effector_acceleration * Vector3d::Ones();
   end_effector_position_tracking_data->SetCmdAccelerationBounds(
       -end_effector_acceleration_limits, end_effector_acceleration_limits);
+
   auto mid_link_position_tracking_data_for_rel =
       std::make_unique<JointSpaceTrackingData>(
           "panda_joint2_target", controller_config.K_p_mid_link,
@@ -216,8 +222,11 @@ int DoMain(int argc, char* argv[]) {
           controller_config.W_end_effector_rot, plant, plant);
   end_effector_orientation_tracking_data->AddFrameToTrack(
       controller_config.end_effector_name);
+
   Eigen::VectorXd orientation_target = Eigen::VectorXd::Zero(4);
   orientation_target(0) = 1;
+
+  // Register all tracking data with the OSC
   osc->AddTrackingData(std::move(end_effector_position_tracking_data));
   osc->AddConstTrackingData(std::move(mid_link_position_tracking_data_for_rel),
                             1.6 * VectorXd::Ones(1));
@@ -232,7 +241,10 @@ int DoMain(int argc, char* argv[]) {
 
   osc->Build();
 
-  // Connect systems in the diagram
+  // Connect all systems in the diagram
+
+  // If enabled, remove gravity compensation from OSC output before sending to
+  // robot
   if (controller_config.cancel_gravity_compensation) {
     auto gravity_compensator =
         builder.AddSystem<GravityCompensationRemover>(plant, *plant_context);
@@ -251,24 +263,29 @@ int DoMain(int argc, char* argv[]) {
                     franka_command_sender->get_input_port(0));
   }
 
+  // Connect radio input to trajectory generators
   builder.Connect(*radio_sub, *radio_to_vector);
   builder.Connect(radio_to_vector->get_output_port(),
                   end_effector_trajectory->get_input_port_radio());
   builder.Connect(radio_to_vector->get_output_port(),
                   end_effector_orientation_trajectory->get_input_port_radio());
   builder.Connect(radio_to_vector->get_output_port(),
                   end_effector_force_trajectory->get_input_port_radio());
+
+  // Connect command senders to LCM publishers
   builder.Connect(franka_command_sender->get_output_port(),
                   franka_command_pub->get_input_port());
   builder.Connect(osc_command_sender->get_output_port(),
                   osc_command_pub->get_input_port());
   builder.Connect(osc->get_output_port_osc_command(),
                   osc_command_sender->get_input_port(0));
 
+  // Connect state and trajectory receivers to OSC and trajectory generators
   builder.Connect(state_receiver->get_output_port(0),
                   osc->get_input_port_robot_output());
-  builder.Connect(end_effector_trajectory_sub->get_output_port(),
-                  end_effector_position_receiver->get_input_port_lcm_trajectory());
+  builder.Connect(
+      end_effector_trajectory_sub->get_output_port(),
+      end_effector_position_receiver->get_input_port_lcm_trajectory());
   builder.Connect(end_effector_trajectory_sub->get_output_port(),
                   end_effector_force_receiver->get_input_port_lcm_trajectory());
   builder.Connect(
@@ -289,11 +306,11 @@ int DoMain(int argc, char* argv[]) {
   builder.Connect(end_effector_force_trajectory->get_output_port(0),
                   osc->get_input_port_tracking_data("end_effector_force"));
 
-  // Build the complete diagram
+  // Build the complete system diagram
   auto owned_diagram = builder.Build();
   owned_diagram->set_name(("plate_balancing/osc_controller"));
 
-  // Run lcm-driven simulation
+  // Run the LCM-driven control loop (real-time or simulation)
   LcmDrivenLoop<dairlib::lcmt_robot_output> loop(
       &lcm, std::move(owned_diagram), state_receiver,
       lcm_channel_params.franka_state_channel, true);
@@ -305,6 +322,7 @@ int DoMain(int argc, char* argv[]) {
 }  // namespace examples
 }  // namespace dairlib
 
+// Main entry point
 int main(int argc, char* argv[]) {
   return dairlib::examples::plate_balancing::DoMain(argc, argv);
 }