feat(core): add c3 plus

[Meow404]

This change is 

[github-actions]

LCOV of commit af38053 during C3 Coverage #63

Summary coverage rate:
  lines......: 92.1% (1436 of 1560 lines)
  functions..: 77.4% (130 of 168 functions)
  branches...: 52.1% (1189 of 2280 branches)

Files changed coverage rate:
                                        |Lines       |Functions  |Branches    
  Filename                              |Rate     Num|Rate    Num|Rate     Num
  ============================================================================
  core/c3.cc                            | 8.0%    275|2181%    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|2200%     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              | 4.9%    307|3467%    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]

core/c3.h line 296 at r3 (raw file):

      bool is_final_solve = false);

  virtual void AddAugmentedCost(const std::vector<Eigen::MatrixXd>& G,

We might need some docs here.

[xuanhien070594]

core/c3_plus.cc line 131 at r3 (raw file):

    }
  } else {
    C3::AddAugmentedCost(G, WD, delta, is_final_solve);

If I'm not wrong, here, when it is not the last QP solve, we'll add augmented costs for the whole vector z, right? Note that in C3+, we don't have the augmented terms for states and inputs. I think it might affect the results of C3+.
In the else statement, you can explicitly add costs only for lambda and eta.

[xuanhien070594]

core/c3_plus.cc line 109 at r3 (raw file):

    augmented_costs_.clear();

    std::vector<Eigen::MatrixXd> GFinal = G;

We can add some comments here about why we need to scale up the G in the final solve.
Here is an explanation I took from our paper "This encourages some contact forces to closely match their values from the previous projection step, which satisfies the complementarity constraints and results in more dynamically feasible forces. While this could theoretically be applied to all contacts, in practice it is too restrictive and may prevent the QP solver from finding a solution."

[Meow404]

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

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

Missing end line

Done

---

core/c3.h line 296 at r3 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

We might need some docs here.

Done

---

core/c3_plus.cc line 109 at r3 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

We can add some comments here about why we need to scale up the G in the final solve.
Here is an explanation I took from our paper "This encourages some contact forces to closely match their values from the previous projection step, which satisfies the complementarity constraints and results in more dynamically feasible forces. While this could theoretically be applied to all contacts, in practice it is too restrictive and may prevent the QP solver from finding a solution."

Done

---

core/c3_plus.cc line 131 at r3 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

If I'm not wrong, here, when it is not the last QP solve, we'll add augmented costs for the whole vector z, right? Note that in C3+, we don't have the augmented terms for states and inputs. I think it might affect the results of C3+.
In the else statement, you can explicitly add costs only for lambda and eta.

Done

[Meow404]

gentle ping @xuanhien070594