feat(core): add c3 plus

[Meow404]

This change is 

[github-actions]

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

[xuanhien070594]

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

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

Missing end line

[xuanhien070594]

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?

[xuanhien070594]

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

}

int LCSFactory::GetNumContactVariables(

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

[xuanhien070594]

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?

[xuanhien070594]

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

Previously, xuanhien070594 (Hien Bui) wrote…

is mu here same as mu of ContactPairConfig?

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

[xuanhien070594]

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?

[xuanhien070594]

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?

[xuanhien070594]

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

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

Missing end line

[xuanhien070594]

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.

[xuanhien070594]

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

# C3 Multibody Module

This instruction is very helpful.

[xuanhien070594]

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_

[xuanhien070594]

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

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

int?

[xuanhien070594]

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?

[xuanhien070594]

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?

[xuanhien070594]

@xuanhien070594 reviewed 35 files and all commit messages, and resolved 3 discussions.
Reviewable status: all files reviewed, 13 unresolved discussions (waiting on @Meow404).

[Meow404]

@Meow404 made 11 comments.
Reviewable status: 30 of 35 files reviewed, 13 unresolved discussions (waiting on @xuanhien070594).

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

You're calling push_back twice on z_sol_

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

int?

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

Should it be N_ instead of N_ - 1?

my reasoning is since were using i and i+1, if use N, at the N - 1 we will weighted average N - 1 and N, which would be out of bouds for an array of size N.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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?

It's possible, but I kept it as is to not disturb current implementations too much and give people the flexibility to add contact pairs manually if they want (via code).

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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

Ahh nice to know. I've removed the statement about non-convex meshes.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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

I think LCSFactory::GetNumContactVariables(const LCSFactoryOptions options) covers a good number of usecases we have. I prefer keeping it gives the flexibility to the user to not having to generate an LCSFactoryOptions variable which contains much more information than what is required.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

should mu have double type?

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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

contact_pair_configs are currently optional, and if provided, it will overwrite the mu provided in lcs_factory.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

This instruction is very helpful.

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

Missing end line

Done.

[Meow404]

@Meow404 made 1 comment.
Reviewable status: 30 of 35 files reviewed, 13 unresolved discussions (waiting on @xuanhien070594).

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

Same question, why do we need an extra constructor?

as said above

[Meow404]

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.

[xuanhien070594]

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

Previously, Meow404 (Thomas Stephen Felix) wrote…

my reasoning is since were using i and i+1, if use N, at the N - 1 we will weighted average N - 1 and N, which would be out of bouds for an array of size N.

Ah, I see.

[xuanhien070594]

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

Previously, Meow404 (Thomas Stephen Felix) wrote…

It's possible, but I kept it as is to not disturb current implementations too much and give people the flexibility to add contact pairs manually if they want (via code).

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

[xuanhien070594]

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

Previously, Meow404 (Thomas Stephen Felix) wrote…

I think LCSFactory::GetNumContactVariables(const LCSFactoryOptions options) covers a good number of usecases we have. I prefer keeping it gives the flexibility to the user to not having to generate an LCSFactoryOptions variable which contains much more information than what is required.

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)

[xuanhien070594]

@xuanhien070594 reviewed 5 files and all commit messages, and resolved 11 discussions.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @Meow404).

[xuanhien070594]

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.

[Meow404]

@Meow404 made 2 comments.
Reviewable status: 33 of 35 files reviewed, 2 unresolved discussions (waiting on @xuanhien070594).

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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)

Done.

[xuanhien070594]

Please check the failed CI.

@xuanhien070594 reviewed 2 files and all commit messages, made 1 comment, and resolved 2 discussions.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @Meow404).