Adding only c3 specific files - part 1 of 3 of c3_merge

[yangwill]

Splitting up c3 merge into 3 PRs to make reviewing more manageable.

This is the first PR that adds just the c3 specific files without any of the systems framework.

Next two PRs will be the following:

1. General systems associated with c3 and minor reformatting changes. Includes c3 controller, lcs factory, and relevant visualization features.
2. Examples, specifically with franka

---

This change is 

[Brian-Acosta]

Reviewed 23 of 27 files at r1, all commit messages.
Reviewable status: 23 of 27 files reviewed, 18 unresolved discussions (waiting on @ebianchi and @yangwill)

---

multibody/geom_geom_collider.h line 66 at r1 (raw file):

          drake::multibody::JacobianWrtVariable::kV);

  std::pair<drake::VectorX<double>, drake::VectorX<double>> CalcWitnessPoints(

would be good to document this function, especially what order the witness points are in, if any.

---

solvers/c3.h line 20 at r1 (raw file):

 public:
  struct CostMatrices {
    CostMatrices() = default;

Do we actually need a default constructor?

---

solvers/c3.h line 42 at r1 (raw file):

  /// @param w A pointer to the scaled dual variable solution
  /// @return The first control action to take, u[0]
  void Solve(const Eigen::VectorXd& x0);

A suggestion for the API - Ideally the only non-const public member function is something like Solve(x0, lcs, x_goal). This prevents bugs like users forgetting to call UpdateXYZ(), and you can ensure that after calling Solve() everything accessed via getters is up-to-date.

---

solvers/c3.h line 71 at r1 (raw file):

  /// allow users to add constraints (adds for all timesteps)
  /// @param A, lower_bound, upper_bound lower_bound <= A^T x <= upper_bound

Comment says constraint is A^T but code just uses A

---

solvers/c3.h line 112 at r1 (raw file):

  bool warm_start_;
  const int N_;
  const int n_;  // n_x

can these just be renamed instead of these comments?

---

solvers/c3.h line 124 at r1 (raw file):

  std::vector<Eigen::MatrixXd> F_;
  std::vector<Eigen::MatrixXd> H_;
  std::vector<Eigen::VectorXd> c_;

Why not just store an LCS object? Would have the added benefit of making it clear what all of these matrices are

---

solvers/c3.h line 130 at r1 (raw file):

  Eigen::VectorXd w_;
  double AnDn_ = 1.0;
  const std::vector<Eigen::MatrixXd> Q_;

Similarly, why not not just store a CostMatrices object?

---

solvers/c3.h line 160 at r1 (raw file):

  std::vector<std::shared_ptr<drake::solvers::QuadraticCost>> input_costs_;

  // Solutions

consider a container struct for the solution

---

solvers/c3.cc line 168 at r1 (raw file):

}

void C3::UpdateLCS(const LCS& lcs) {

Should check that LCS dimensions match C3 dimensions

---

solvers/c3.cc line 297 at r1 (raw file):

}

vector<VectorXd> C3::SolveQP(const VectorXd& x0, const vector<MatrixXd>& G,

Has any profiling been done to see how much overhead removing and adding all these costs/constraints is adding?

---

solvers/c3.cc line 412 at r1 (raw file):

  return deltaProj;
}

My previous comment about API design applies here too. you could add an argument like
const std::vector<std::tuple<Eigen::MatrixXd, Eigen::VectorXd, Eigen::VectorXd>>& input_constraint_A_lb_ub to Solve, and so on for state and force constraints.

---

solvers/c3_miqp.cc line 56 at r1 (raw file):

  for (int i = 0; i < n_ + m_ + k_; i++) {
    delta_k[i] = model.addVar(-100.0, 100.0, 0.0, GRB_CONTINUOUS);

nit - magic numbers

---

solvers/c3_miqp.cc line 72 at r1 (raw file):

  model.setObjective(obj, GRB_MINIMIZE);

  int M = 1000;  // big M variable

nit - consider making bigM part of C3options

---

solvers/c3_miqp_no_gurobi.cc line 14 at r1 (raw file):

    : C3(LCS, costs, xdesired, options) {
  throw std::runtime_error(
      "The Gurobi bindings were not compiled.  You'll need to use a different "

nit - consider telling the user where to look for instructions compiling with gurobi.

---

solvers/c3_options.h line 22 at r1 (raw file):

  double publish_frequency;

//  std::vector<double> world_x_limits;

Remove if unused

---

solvers/c3_options.h line 38 at r1 (raw file):

  double w_U;

  std::vector<double> q_vector;

Should document all of these cost terms

---

solvers/c3_qp.cc line 48 at r1 (raw file):

  double alpha = 0.01;
  double scaling = 1000;

nit - magic numbers

---

solvers/c3_qp.cc line 83 at r1 (raw file):

  solver_options.SetOption(OsqpSolver::id(), "scaled_termination", 1);
  solver_options.SetOption(OsqpSolver::id(), "linsys_solver", 0);
  prog.SetSolverOptions(solver_options);

nit - could this be moved to a static inline set_solver_options(SolverOptions& solver_options) function in the c3_qp.h to make all these options visible from the header?

[yangwill]

Reviewable status: 14 of 27 files reviewed, 15 unresolved discussions (waiting on @Brian-Acosta and @ebianchi)

---

multibody/geom_geom_collider.h line 66 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

would be good to document this function, especially what order the witness points are in, if any.

Done.

---

solvers/c3.h line 20 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Do we actually need a default constructor?

good point, removing it

---

solvers/c3.h line 71 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Comment says constraint is A^T but code just uses A

fixed

---

solvers/c3.h line 112 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

can these just be renamed instead of these comments?

the variable names were chosen to match the paper, i'll switch the variable name and the comment so that the readers of the paper can still match it to the code

---

solvers/c3.h line 124 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Why not just store an LCS object? Would have the added benefit of making it clear what all of these matrices are

good point, done

---

solvers/c3.h line 130 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Similarly, why not not just store a CostMatrices object?

good point, done

---

solvers/c3.h line 160 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

consider a container struct for the solution

z_sol already contains the full solution and we have getters for the individual parts

---

solvers/c3.cc line 168 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Should check that LCS dimensions match C3 dimensions

Done.

---

solvers/c3.cc line 297 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Has any profiling been done to see how much overhead removing and adding all these costs/constraints is adding?

removed this, changing to UpdateConstraint

---

solvers/c3.cc line 412 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

My previous comment about API design applies here too. you could add an argument like
const std::vector<std::tuple<Eigen::MatrixXd, Eigen::VectorXd, Eigen::VectorXd>>& input_constraint_A_lb_ub to Solve, and so on for state and force constraints.

user constraints probably shouldn't be updated every controller call, so I'm leaving this as is.

---

solvers/c3_miqp.cc line 72 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

nit - consider making bigM part of C3options

done

---

solvers/c3_options.h line 22 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Remove if unused

Done.

---

solvers/c3_options.h line 38 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Should document all of these cost terms

Done.

---

solvers/c3_qp.cc line 48 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

nit - magic numbers

Yeah, i think c3_qp is still a WIP, so i'm just leaving these as is.

---

solvers/c3_qp.cc line 83 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

nit - could this be moved to a static inline set_solver_options(SolverOptions& solver_options) function in the c3_qp.h to make all these options visible from the header?

Also a WIP so we'll probably put these in a default .yaml settings eventually, but not necessary in this PR

[Brian-Acosta]

Reviewed 2 of 27 files at r1, 11 of 11 files at r2, all commit messages.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @ebianchi and @yangwill)

---

solvers/c3.cc line 297 at r1 (raw file):

Previously, yangwill (William Yang) wrote…

removed this, changing to UpdateConstraint

I'm not seeing that you changed it but I think it's probably minimal overhead compared to the rest of C3, so it can stay like this for now. Long term I think calling UpdateCoefficients makes the intent more clear.

---

solvers/c3_qp.cc line 72 at r2 (raw file):

  VectorXd cost_linear = -delta_c.transpose() * New_U;

  //  prog.AddQuadraticCost(New_U, cost_linear, {xn_, ln_, un_}, 1);

what's with these comments?

---

solvers/lcs_factory.cc line 66 at r2 (raw file):

  // If this ldlt is slow, there are alternate formulations which avoid it
  AutoDiffVecXd vdot_no_contact =
      M.ldlt().solve(tau_g + Bu + f_app.generalized_forces() - C);

This seems to be double-counting gravity (#298 (comment))

[yangwill]

Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @Brian-Acosta and @ebianchi)

---

solvers/c3.cc line 297 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

I'm not seeing that you changed it but I think it's probably minimal overhead compared to the rest of C3, so it can stay like this for now. Long term I think calling UpdateCoefficients makes the intent more clear.

oops, i replaced the functionality with initial_constraint_ (see line 209) but never removed the corresponding lines for constraints_.

---

solvers/c3_qp.cc line 72 at r2 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

what's with these comments?

i wasn't involved with the qp projection formulation so i'm not sure. I removed the commented out lines as they seem to be straight duplicates though.

---

solvers/lcs_factory.cc line 66 at r2 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

This seems to be double-counting gravity (#298 (comment))

hmm this seems like a pretty big bug, are we able to test this in any way? My inclination would be to remove the generalized_forces and keep tau_g because as we observed in the OSC, the generalized_forces includes damping which in practice is difficult to get correct.