feat(plate-balancing) : Support plate-balancing example in dairlib

[Meow404]

Fixes : #11
Support for : DAIRLab/dairlib#376

• Waiting on feat(lcs-factory-system) : Add lcs factory system to C3 repo with examples #9 to be merged
• Waiting on build(c3): Update version for drake and bazel #14 to be merged (soft requirement : to make updates for new drake and bazel version)
• Waiting on feat(core): add c3 plus #22 to be merged

This change is 

[xuanhien070594]

core/configs/solve_options_default.hpp line 3 at r1 (raw file):

#pragma once
#include <string>
const std::string default_solver_options =

I'm confused about why we use yaml string here instead of normal yaml.

[xuanhien070594]

systems/publishers/output_publisher.h line 46 at r1 (raw file):

              c3::lcmt_output* output) const;

  //  void OutputNextC3Input(const drake::systems::Context<double>& context,

Pls remove commented codes.

[xuanhien070594]

systems/publishers/force_publisher.h line 16 at r1 (raw file):

namespace systems {
namespace publishers {
class ContactForcePublisher : public drake::systems::LeafSystem<double> {

Missing documentation

[xuanhien070594]

systems/publishers/force_publisher.cc line 16 at r1 (raw file):

namespace systems {
namespace publishers {
ContactForcePublisher::ContactForcePublisher() {

We'll need some documentation for input ports and output ports.

[xuanhien070594]

systems/publishers/output_publisher.h line 17 at r2 (raw file):

namespace publishers {
/// Converts a OutputVector object to LCM type lcmt_robot_output
class C3OutputPublisher : public drake::systems::LeafSystem<double> {

Pls add documentation.

[xuanhien070594]

systems/publishers/force_publisher.cc line 46 at r2 (raw file):

  MatrixXd J_c = contact_info->first;
  int contact_force_start = solution->lambda_sol_.rows() - J_c.rows();
  bool using_stewart_and_trinkle_model = contact_force_start > 0;

Using the difference in dimension of lambda and number of force basis to determine the contact model is generally not a safe way. Could we store the type of contact model in the C3Solution?

[Meow404]

FYI, building with different drake versions doesnt seem to be an issue so far between dairlib and c3.

[Meow404]

Reviewable status: 0 of 19 files reviewed, 6 unresolved discussions (waiting on @xuanhien070594)

---

systems/publishers/force_publisher.h line 16 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Missing documentation

Done.

---

systems/publishers/force_publisher.cc line 16 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

We'll need some documentation for input ports and output ports.

Done.

---

systems/publishers/output_publisher.h line 46 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Pls remove commented codes.

Done.

---

systems/publishers/output_publisher.h line 17 at r2 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Pls add documentation.

Done.

[xuanhien070594]

lcmtypes/lcmt_forces.lcm line 1 at r3 (raw file):

package c3;

Should the name be lcmt_contact_forces.lcm?

[xuanhien070594]

multibody/geom_geom_collider.cc line 197 at r5 (raw file):

  if (num_friction_directions < 1) {
    // Planar contact: basis is constructed from the contact and planar normals.
    return ComputePlanarForceBasis(-query_result.signed_distance_pair.nhat_BA_W,

Should we add few lines in documentation to tell the user which forces we are calculating (i.e forces that object A acts on object B or object B acts on object A)?

[xuanhien070594]

multibody/geom_geom_collider.cc line 197 at r5 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Should we add few lines in documentation to tell the user which forces we are calculating (i.e forces that object A acts on object B or object B acts on object A)?

Since you're using minus sign here, it seems that we're calculating the forces that object A acting on object B.

[xuanhien070594]

multibody/lcs_factory.cc line 377 at r5 (raw file):

  for (int i = 0; i < n_contacts_; i++) {
    multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]);

Can we reuse the contact results from LCSFactory::ComputeContactJacobian? I think we might want to store a list of GeomGeomCollider inside LCSFactory class, so we can quickly get the contact result without recalculation. What do you think?

[xuanhien070594]

multibody/lcs_factory.cc line 417 at r5 (raw file):

    DRAKE_ASSERT(n_friction_directions_ > 0);
    for (int i = 0; i < tangential_contact_descriptions.size(); i++) {
      int normal_index = i / (2 * n_friction_directions_);

I think it is worth to mention somewhere in the documentation of LCSFactory about how we stack the contact forces.

For example, Stewart-Trinkle currently stacks them as [slack_1, slack_2, ..., slack N, normal_1, normal_2, ..., normal_N, tangential_1_1, ..., tangential_1_4, ... tangential_N_1, ..., tangential_N_4]

[xuanhien070594]

multibody/test/lcs_factory_test.cc line 185 at r5 (raw file):

}

// TEST_P(LCSFactoryPivotingTest, UpdateStateAndInput) {

Those tests are commented out. We can remove them if they are no longer needed.

[xuanhien070594]

systems/lcs_factory_system.h line 91 at r5 (raw file):

   */
  const drake::systems::OutputPort<double>&
  get_output_port_lcs_contact_description() const {

Nit. get_output_port_lcs_contact_descriptions

[xuanhien070594]

systems/lcs_factory_system.h line 128 at r5 (raw file):

  drake::systems::InputPortIndex lcs_inputs_input_port_;
  drake::systems::OutputPortIndex lcs_port_;
  drake::systems::OutputPortIndex lcs_contact_output_port_;

Nit. lcs_contact_descriptions_output_port_

[xuanhien070594]

systems/lcmt_generators/contact_force_generator.cc line 63 at r5 (raw file):

    if (contact_info->at(i).is_slack)
      // If the contact is slack, set the force to zero.

I think we're not setting the force to zero right? We just ignore it.

[xuanhien070594]

systems/lcmt_generators/c3_trajectory_generator_config.h line 12 at r5 (raw file):

struct TrajectoryDescription {
  struct index_range {
    int start;  // Start index of the range

Could we add some comments to clarify why do we need start and end?

My understanding is that we need those to extract a portion of state vector.

[xuanhien070594]

Reviewed 18 of 19 files at r1, 1 of 4 files at r3, 7 of 14 files at r4, 22 of 22 files at r5, all commit messages.
Reviewable status: all files reviewed, 11 unresolved discussions (waiting on @Meow404)

[github-actions]

LCOV of commit ed1cb0e during C3 Coverage #34

Summary coverage rate:
  lines......: 89.9% (1498 of 1667 lines)
  functions..: 76.9% (140 of 182 functions)
  branches...: 51.3% (1183 of 2306 branches)

Files changed coverage rate:
                                                          |Lines       |Functions  |Branches    
  Filename                                                |Rate     Num|Rate    Num|Rate     Num
  ==============================================================================================
  core/c3.cc                                              | 7.3%    287|2410%    20|    -      0
  core/c3_options.h                                       | 1.6%     64|4200%     1|    -      0
  core/c3_plus.cc                                         | 8.9%     56|2550%     4|    -      0
  core/common/logging_utils.hpp                           |20.0%      5| 200%     1|    -      0
  multibody/geom_geom_collider.cc                         |16.9%     71| 600%    11|    -      0
  multibody/lcs_factory.cc                                | 4.5%    266|3167%    12|    -      0
  multibody/lcs_factory.h                                 |33.3%      6| 600%     2|    -      0
  systems/c3_controller.cc                                | 4.7%    106|3040%     5|    -      0
  systems/c3_controller.h                                 |50.0%     10| 0.0%     5|    -      0
  systems/c3_controller_options.h                         |14.3%     14|2200%     1|    -      0
  systems/framework/c3_output.cc                          | 4.3%     46|3300%     2|    -      0
  systems/framework/c3_output.h                           |26.3%     19| 750%     4|    -      0
  systems/lcmt_generators/c3_output_generator.cc          |21.4%     14|1000%     2|    -      0
  systems/lcmt_generators/c3_output_generator.h           |50.0%      6| 0.0%     3|    -      0
  systems/lcmt_generators/c3_trajectory_generator.cc      | 6.5%     46|2900%     2|    -      0
  systems/lcmt_generators/c3_trajectory_generator.h       |50.0%      4| 0.0%     2|    -      0
  systems/lcmt_generators/c3_trajectory_generator_config.h| 0.0%      1|    -     0|    -      0
  systems/lcmt_generators/contact_force_generator.cc      |10.0%     30|2100%     2|    -      0
  systems/lcmt_generators/contact_force_generator.h       |50.0%      6| 0.0%     3|    -      0
  systems/lcs_factory_system.cc                           |10.0%     30|1867%     3|    -      0
  systems/lcs_factory_system.h                            |50.0%      8| 0.0%     4|    -      0

⛔ The code coverage is too low: 89.86. Expected at least 90.