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]

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 24d08ee during C3 Coverage #61

Summary coverage rate:
  lines......: 91.3% (1614 of 1767 lines)
  functions..: 78.3% (148 of 189 functions)
  branches...: 52.3% (1294 of 2474 branches)

Files changed coverage rate:
                                                          |Lines       |Functions  |Branches    
  Filename                                                |Rate     Num|Rate    Num|Rate     Num
  ==============================================================================================
  core/c3.cc                                              | 7.7%    284|2229%    21|    -      0
  core/c3_options.h                                       | 1.5%     66|4200%     1|    -      0
  core/c3_plus.cc                                         | 7.3%     82|2267%     6|    -      0
  core/common/logging_utils.hpp                           |20.0%      5| 200%     1|    -      0
  multibody/geom_geom_collider.cc                         |12.9%     93| 550%    12|    -      0
  multibody/lcs_factory.cc                                | 4.5%    309|3686%    14|    -      0
  multibody/lcs_factory.h                                 |33.3%      6| 600%     2|    -      0
  systems/c3_controller.cc                                | 4.5%    110|3040%     5|    -      0
  systems/c3_controller.h                                 |50.0%     10| 0.0%     5|    -      0
  systems/c3_controller_options.h                         |12.5%     16|2400%     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                           |15.2%     33|1200%     4|    -      0
  systems/lcs_factory_system.h                            |50.0%      8| 0.0%     4|    -      0