LCOV of commit db8878a during C3 Coverage #66

Summary coverage rate:
  lines......: 92.2% (1433 of 1554 lines)
  functions..: 77.4% (130 of 168 functions)
  branches...: 52.3% (1195 of 2286 branches)

Files changed coverage rate:
                                        |Lines       |Functions  |Branches    
  Filename                              |Rate     Num|Rate    Num|Rate     Num
  ============================================================================
  core/c3.cc                            | 8.0%    276|2162%    21|    -      0
  core/c3.h                             |14.3%     21|2100%     2|    -      0
  core/c3_miqp.cc                       | 2.7%     73|    #     2|    -      0
  core/c3_options.h                     | 1.5%     67|4400%     1|    -      0
  core/c3_plus.cc                       | 7.0%     86|2467%     6|    -      0
  core/c3_plus.h                        | 200%      1| 0.0%     2|    -      0
  core/c3_qp.cc                         | 6.1%     33|4300%     2|    -      0
  core/lcs.cc                           |13.7%     51|1167%     6|    -      0
  core/lcs.h                            |17.6%     17|2667%     3|    -      0
  core/test/c3_cartpole_problem.hpp     | 2.8%     72|5300%     2|    -      0
  multibody/geom_geom_collider.cc       |12.6%     87| 545%    11|    -      0
  multibody/lcs_factory.cc              | 5.0%    303|3440%    15|    -      0
  multibody/lcs_factory.h               |33.3%      3|1200%     1|    -      0
  multibody/lcs_factory_options.h       | 7.7%     26| 800%     2|    -      0
  systems/c3_controller.cc              | 4.5%    110|3040%     5|    -      0
  systems/c3_controller_options.h       |12.5%     16|2400%     1|    -      0
  systems/framework/c3_output.h         |36.4%     11| 667%     3|    -      0
  systems/lcs_factory_system.cc         |14.7%     34|1250%     4|    -      0
  systems/lcs_factory_system.h          |50.0%      8| 0.0%     4|    -      0

bindings/pyc3/c3_multibody_py.cc line 95 at r1 (raw file):

}  // namespace pyc3
}  // namespace multibody
}  // namespace c3

Missing end line

multibody/lcs_factory.h line 57 at r5 (raw file):

class LCSFactory {
 public:
  LCSFactory(

Could you remind me why do we need extra constructor here?

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

}

int LCSFactory::GetNumContactVariables(

can this be merged into LCSFactory::GetNumContactVariables(const LCSFactoryOptions options)?

multibody/lcs_factory_options.h line 36 at r5 (raw file):

  std::optional<std::vector<int>>
      num_friction_directions_per_contact;  // Number of friction directions per
  std::optional<std::vector<double>>

is mu here same as mu of ContactPairConfig?

multibody/lcs_factory_options.h line 36 at r5 (raw file):

I mean if we have both contact_pair_configs and mu, which mu should we use?

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

class LCSFactorySystem : public drake::systems::LeafSystem<double> {
 public:
  explicit LCSFactorySystem(

Same question, why do we need an extra constructor?

examples/lcs_factory_system_example.cc line 312 at r5 (raw file):

  auto lcs_factory_system = builder.AddSystem<LCSFactorySystem>(
      plant_for_lcs, plant_for_lcs_context, *plant_autodiff,
      *plant_context_autodiff, options.lcs_factory_options);

Oh, I see the reason why you have a new constructor. You want to move contact pair information into lcs_factory_options right? I think that's very clean approach. Can we safely remove the other constructor of LCSFactory?

multibody/test/multibody_test.cc line 431 at r5 (raw file):

}  // namespace test
}  // namespace multibody
}  // namespace c3

Missing end line

multibody/geom_geom_collider.h line 267 at r5 (raw file):

   * using Drake's standard ComputeSignedDistancePairClosestPoints algorithm.
   * It works reliably for convex geometries and convex hulls of meshes, but
   * may not provide accurate results for non-convex mesh surfaces.

nit. There is no non-convex mesh in drake as it will automatically create convex hulls during URDF importing.

multibody/README.md line 1 at r5 (raw file):

# C3 Multibody Module

This instruction is very helpful.

core/c3.cc line 96 at r5 (raw file):

    lambda_sol_->push_back(Eigen::VectorXd::Zero(n_lambda_));
    u_sol_->push_back(Eigen::VectorXd::Zero(n_u_));
    z_sol_->push_back(Eigen::VectorXd::Zero(n_z_));

You're calling push_back twice on z_sol_

core/c3_options.h line 38 at r5 (raw file):

                                             // augmented cost matrix for C3+
                                             // projection
  std::optional<std::vector<double>>

int?

core/c3_plus.cc line 74 at r5 (raw file):

  int index = solve_time_ / lcs_.dt();
  double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt();
  for (int i = 0; i < N_ - 1; ++i) {

Should it be N_ instead of N_ - 1?

multibody/lcs_factory_options.h line 14 at r5 (raw file):

  std::vector<int> body_B_collision_geom_indices;
  int num_friction_directions;
  float mu;  // friction coefficient

should mu have double type?

My intention for using contact_pair_configs is to facilitate for contacts that don't change during the experiment, eg. plate-balancing. However, for some applications like push-anything, the contact pairs are recalculated using a distance metric at every timestep as I understand.

core/c3_plus.cc line 74 at r5 (raw file):

Ah, I see.

examples/lcs_factory_system_example.cc line 312 at r5 (raw file):

Agree, I suggest adding some comments mentioning that new constructor should be preferred whenever possible.

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

Thanks for explanation. The implementationint LCSFactory::GetNumContactVariables(ContactModel contact_model, int num_contacts, int num_friction_directions) can be simplified to reduce code duplication (generating a vector from num_friction_directions then call function int LCSFactory::GetNumContactVariables(ContactModel contact_model, int num_contacts, std::vector<int> num_friction_directions_per_contact)

In PushAnything, the contact pairs are fixed, but we use a dedicated function to select the active contact pair online. For example, we initially define contact pairs between the end-effector and all objects, and during execution the controller selects the pair corresponding to the object closest to the end-effector. The list of contact pairs in PushAnything is long, so I think it is reasonable to still allow users to define contact pairs programmatically.