doc: add documentation for publishers

Author: Meow404

systems/publishers/force_publisher.cc

@@ -2,6 +2,7 @@
 
 #include "systems/framework/c3_output.h"
 
+// Drake and Eigen namespace usages for convenience.
 using drake::systems::Context;
 using drake::systems::DiagramBuilder;
 using drake::systems::lcm::LcmPublisherSystem;
@@ -13,10 +14,14 @@ using Eigen::VectorXd;
 namespace c3 {
 namespace systems {
 namespace publishers {
+
+// Publishes contact force information to LCM for visualization and logging.
 ContactForcePublisher::ContactForcePublisher() {
+  // Declare input port for the C3 solution.
   c3_solution_port_ = this->DeclareAbstractInputPort(
                               "solution", drake::Value<C3Output::C3Solution>{})
                           .get_index();
+  // Declare input port for contact Jacobian and contact points.
   lcs_contact_info_port_ =
       this->DeclareAbstractInputPort(
               "J_lcs, p_lcs",
@@ -25,12 +30,14 @@ ContactForcePublisher::ContactForcePublisher() {
           .get_index();
 
   this->set_name("c3_contact_force_publisher");
+  // Declare output port for publishing contact forces.
   contact_force_output_port_ =
       this->DeclareAbstractOutputPort("lcmt_force", lcmt_forces(),
                                       &ContactForcePublisher::DoCalc)
           .get_index();
 }
 
+// Calculates and outputs the contact forces based on the current context.
 void ContactForcePublisher::DoCalc(const Context<double>& context,
                                    lcmt_forces* output) const {
   // Get Solution from C3
@@ -53,6 +60,7 @@ void ContactForcePublisher::DoCalc(const Context<double>& context,
 
   int contact_var_start;
   int force_index;
+  // Iterate over all contact points and compute forces.
   for (int contact_index = 0; contact_index < (int)contact_points.size();
        ++contact_index) {
     contact_var_start =
@@ -75,13 +83,17 @@ void ContactForcePublisher::DoCalc(const Context<double>& context,
         }
       }
       auto force = lcmt_contact_force();
+      // Set contact point position.
       force.contact_point[0] = contact_points.at(contact_index)[0];
       force.contact_point[1] = contact_points.at(contact_index)[1];
       force.contact_point[2] = contact_points.at(contact_index)[2];
       // TODO(yangwill): find a cleaner way to figure out the equivalent forces
       // VISUALIZING FORCES FOR THE FIRST KNOT POINT
       // 6, 7, 8 are the indices for the x,y,z components of the tray
       // expressed in the world frame
+      // Compute force vector in world frame.
+      std::cout << J_c << std::endl;
+      exit(0);
       force.contact_force[0] = solution->lambda_sol_(contact_var_index, 0) *
                                J_c.row(contact_jacobian_row)(6);
       force.contact_force[1] = solution->lambda_sol_(contact_var_index, 0) *
@@ -91,21 +103,25 @@ void ContactForcePublisher::DoCalc(const Context<double>& context,
       output->forces[force_index + i] = force;
     }
   }
+  // Set output timestamp in microseconds.
   output->utime = context.get_time() * 1e6;
 }
 
+// Adds this publisher and an LCM publisher system to the diagram builder.
 LcmPublisherSystem* ContactForcePublisher::AddLcmPublisherToBuilder(
     DiagramBuilder<double>& builder,
     const drake::systems::OutputPort<double>& solution_port,
     const drake::systems::OutputPort<double>& lcs_contact_info_port,
     const std::string& channel, drake::lcm::DrakeLcmInterface* lcm,
     const drake::systems::TriggerTypeSet& publish_triggers,
     double publish_period, double publish_offset) {
+  // Add and connect the ContactForcePublisher system.
   auto force_publisher = builder.AddSystem<ContactForcePublisher>();
   builder.Connect(solution_port, force_publisher->get_input_port_c3_solution());
   builder.Connect(lcs_contact_info_port,
                   force_publisher->get_input_port_lcs_contact_info());
 
+  // Add and connect the LCM publisher system.
   auto lcm_force_publisher =
       builder.AddSystem(LcmPublisherSystem::Make<c3::lcmt_forces>(
           channel, lcm, publish_triggers, publish_period, publish_offset));

systems/publishers/force_publisher.h

@@ -13,27 +13,73 @@
 namespace c3 {
 namespace systems {
 namespace publishers {
+
+/**
+ * @class ContactForcePublisher
+ * @brief Converts solution vectors and LCS contact information into LCM contact
+ * force messages for publishing.
+ * @details
+ *   - Provides input ports for the C3 solution vector and LCS contact
+ * information.
+ *   - Computes contact forces based on the provided inputs.
+ *   - Offers an output port for the constructed contact force message.
+ *   - Includes a static helper to add an LCM publisher system to a
+ * DiagramBuilder for message transmission.
+ */
 class ContactForcePublisher : public drake::systems::LeafSystem<double> {
  public:
+  /**
+   * @brief Constructs a ContactForcePublisher system.
+   *
+   * Declares input and output ports for the solution vector, LCS contact info,
+   * and contact force output.
+   */
   ContactForcePublisher();
 
+  /**
+   * @brief Returns the input port for the c3 solution vector.
+   * @return Reference to the input port for the c3 solution.
+   */
   const drake::systems::InputPort<double>& get_input_port_c3_solution() const {
     return this->get_input_port(c3_solution_port_);
   }
 
+  /**
+   * @brief Returns the input port for the LCS contact information.
+   * @return Reference to the input port for LCS contact info.
+   */
   const drake::systems::InputPort<double>& get_input_port_lcs_contact_info()
       const {
     return this->get_input_port(lcs_contact_info_port_);
   }
 
-  // LCS contact force, not actor input forces
+  /**
+   * @brief Returns the output port for the computed contact forces.
+   * @return Reference to the output port for contact forces.
+   */
   const drake::systems::OutputPort<double>& get_output_port_contact_force()
       const {
     return this->get_output_port(contact_force_output_port_);
   }
 
-  static drake::systems::lcm::LcmPublisherSystem*
-  AddLcmPublisherToBuilder(
+  /**
+   * @brief Adds an LCM publisher system to the given DiagramBuilder for
+   * publishing contact forces.
+   *
+   * @param builder The DiagramBuilder to which the publisher will be added.
+   * @param solution_port Output port providing the solution vector.
+   * @param lcs_contact_info_port Output port providing LCS contact information.
+   * @param channel The LCM channel name to publish on.
+   * @param lcm The LCM interface to use for publishing.
+   * @param publish_triggers Set of triggers that determine when publishing
+   * occurs.
+   * @param publish_period Optional period for periodic publishing (default:
+   * 0.0).
+   * @param publish_offset Optional offset for periodic publishing (default:
+   * 0.0).
+   * @return Pointer to the created LcmPublisherSystem.
+   */
+  static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder(
       drake::systems::DiagramBuilder<double>& builder,
       const drake::systems::OutputPort<double>& solution_port,
       const drake::systems::OutputPort<double>& lcs_contact_info_port,
@@ -42,12 +88,21 @@ class ContactForcePublisher : public drake::systems::LeafSystem<double> {
       double publish_period = 0.0, double publish_offset = 0.0);
 
  private:
+  /**
+   * @brief Calculates the contact forces and populates the output message.
+   *
+   * @param context The system context containing input values.
+   * @param c3_forces_output Pointer to the output message to populate.
+   */
   void DoCalc(const drake::systems::Context<double>& context,
               c3::lcmt_forces* c3_forces_output) const;
 
-  drake::systems::InputPortIndex c3_solution_port_;
-  drake::systems::InputPortIndex lcs_contact_info_port_;
-  drake::systems::OutputPortIndex contact_force_output_port_;
+  drake::systems::InputPortIndex
+      c3_solution_port_;  ///< Index of the c3 solution input port.
+  drake::systems::InputPortIndex
+      lcs_contact_info_port_;  ///< Index of the LCS contact info input port.
+  drake::systems::OutputPortIndex
+      contact_force_output_port_;  ///< Index of the contact force output port.
 };
 
 }  // namespace publishers

systems/publishers/output_publisher.cc

@@ -10,48 +10,58 @@ namespace c3 {
 namespace systems {
 namespace publishers {
 
+// Publishes C3Output as an LCM message.
 C3OutputPublisher::C3OutputPublisher() {
+  // Declare input port for the C3 solution.
   c3_solution_port_ =
       this->DeclareAbstractInputPort("c3_solution",
                                      drake::Value<C3Output::C3Solution>{})
           .get_index();
+  // Declare input port for C3 intermediates.
   c3_intermediates_port_ =
       this->DeclareAbstractInputPort("c3_intermediates",
                                      drake::Value<C3Output::C3Intermediates>{})
           .get_index();
 
   this->set_name("c3_output_publisher");
+  // Declare output port for the LCM message.
   c3_output_port_ =
       this->DeclareAbstractOutputPort("lcmt_c3_output", c3::lcmt_output(),
                                       &C3OutputPublisher::DoCalc)
           .get_index();
 }
 
+// Calculates the LCM output message from the input ports.
 void C3OutputPublisher::DoCalc(const drake::systems::Context<double>& context,
                                c3::lcmt_output* output) const {
+  // Retrieve input values.
   const auto& c3_solution =
       this->EvalInputValue<C3Output::C3Solution>(context, c3_solution_port_);
   const auto& c3_intermediates =
       this->EvalInputValue<C3Output::C3Intermediates>(context,
                                                       c3_intermediates_port_);
 
+  // Construct C3Output and generate the LCM message.
   C3Output c3_output = C3Output(*c3_solution, *c3_intermediates);
   *output = c3_output.GenerateLcmObject(context.get_time());
 }
 
+// Adds this publisher and an LCM publisher system to the diagram builder.
 LcmPublisherSystem* C3OutputPublisher::AddLcmPublisherToBuilder(
     DiagramBuilder<double>& builder,
     const drake::systems::OutputPort<double>& solution_port,
     const drake::systems::OutputPort<double>& intermediates_port,
     const std::string& channel, drake::lcm::DrakeLcmInterface* lcm,
     const drake::systems::TriggerTypeSet& publish_triggers,
     double publish_period, double publish_offset) {
+  // Add and connect the C3OutputPublisher system.
   auto output_publisher = builder.AddSystem<C3OutputPublisher>();
   builder.Connect(solution_port,
                   output_publisher->get_input_port_c3_solution());
   builder.Connect(intermediates_port,
                   output_publisher->get_input_port_c3_intermediates());
 
+  // Add and connect the LCM publisher system.
   auto lcm_output_publisher =
       builder.AddSystem(LcmPublisherSystem::Make<c3::lcmt_output>(
           channel, lcm, publish_triggers, publish_period, publish_offset));

systems/publishers/output_publisher.h

@@ -13,24 +13,56 @@
 namespace c3 {
 namespace systems {
 namespace publishers {
-/// Converts a OutputVector object to LCM type lcmt_robot_output
+
+/**
+ * @class C3OutputPublisher
+ * @brief Converts OutputVector objects to LCM type lcmt_output for publishing.
+ * @details
+ *   - Provides input ports for C3 solution and intermediates.
+ *   - Provides an output port for the constructed lcmt_output message.
+ *   - Can be connected to an LCM publisher system for message transmission.
+ */
 class C3OutputPublisher : public drake::systems::LeafSystem<double> {
  public:
+  /**
+   * @brief Constructor. Declares input and output ports.
+   */
   C3OutputPublisher();
 
+  /**
+   * @brief Returns the input port for the C3 solution vector.
+   */
   const drake::systems::InputPort<double>& get_input_port_c3_solution() const {
     return this->get_input_port(c3_solution_port_);
   }
 
+  /**
+   * @brief Returns the input port for the C3 intermediates vector.
+   */
   const drake::systems::InputPort<double>& get_input_port_c3_intermediates()
       const {
     return this->get_input_port(c3_intermediates_port_);
   }
 
+  /**
+   * @brief Returns the output port for the lcmt_output message.
+   */
   const drake::systems::OutputPort<double>& get_output_port_c3_output() const {
     return this->get_output_port(c3_output_port_);
   }
 
+  /**
+   * @brief Adds an LCM publisher system to the given DiagramBuilder.
+   * @param builder The diagram builder to add the publisher to.
+   * @param solution_port Output port for the solution vector.
+   * @param intermediates_port Output port for the intermediates vector.
+   * @param channel LCM channel name.
+   * @param lcm LCM interface pointer.
+   * @param publish_triggers Set of triggers for publishing.
+   * @param publish_period Period for periodic publishing (optional).
+   * @param publish_offset Offset for periodic publishing (optional).
+   * @return Pointer to the created LcmPublisherSystem.
+   */
   static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder(
       drake::systems::DiagramBuilder<double>& builder,
       const drake::systems::OutputPort<double>& solution_port,
@@ -40,16 +72,22 @@ class C3OutputPublisher : public drake::systems::LeafSystem<double> {
       double publish_period = 0.0, double publish_offset = 0.0);
 
  private:
+  /**
+   * @brief Calculates the lcmt_output message from the input ports.
+   * @param context The system context.
+   * @param output The output message to populate.
+   */
   void DoCalc(const drake::systems::Context<double>& context,
               c3::lcmt_output* output) const;
 
-  //  void OutputNextC3Input(const drake::systems::Context<double>& context,
-  //                         drake::systems::BasicVector<double>* u_next) const;
-
-  drake::systems::InputPortIndex c3_solution_port_;
-  drake::systems::InputPortIndex c3_intermediates_port_;
-  drake::systems::OutputPortIndex c3_output_port_;
+  drake::systems::InputPortIndex
+      c3_solution_port_; /**< Index for solution input port. */
+  drake::systems::InputPortIndex
+      c3_intermediates_port_; /**< Index for intermediates input port. */
+  drake::systems::OutputPortIndex
+      c3_output_port_; /**< Index for output port. */
 };
+
 }  // namespace publishers
 }  // namespace systems
 }  // namespace c3