Integrate C3+ into push_anything branch

[xuanhien070594]

This PR integrates the new C3 algorithm, temporarily named C4, into the push_anything branch.

---

This change is 

[mposa]

Before I read further, I think we should discuss whether the new algorithm belongs as either:

• subclass of C3, as this PR has it,
• as a side-by-side replacement (a separate class that copies duplicated code, with the intent of later deprecating C3)
• with an additional abstraction layer, e.g. a parent class or interface to C3 and C4 (nextgen), where some code moves from C3 to this new parent
• some other solution, e.g. one that doesn't use a class heirarchy

Thoughts? From anyone involved in the project.

Reviewed 3 of 17 files at r1, all commit messages.
Reviewable status: 3 of 17 files reviewed, 3 unresolved discussions

---

examples/sampling_c3/start_logging.py line 11 at r1 (raw file):

def main(log_type, demo_name):

This is obviously a good change, but should require @ebianchi's review to make sure she's aware.

---

examples/sampling_c3/start_logging.py line 18 at r1 (raw file):

    if not op.isdir(logdir):
        os.makedirs(logdir, exist_ok=True)

I think if you're using exist_ok=True, then you don't need to check if the directory exists first.

---

.gitignore line 15 at r1 (raw file):

/examples/Cassie/saved_trajectories/
/MODULE.bazel.lock
/logs/

Why is this being added to .gitignore? Looks like some local files leaking from your end.

[xuanhien070594]

Some thoughts on pros/cons of each approach:

Subclass of C3 (Current PR Approach)

Pros:

• Minimal code changes required
• Minimal code duplication
• Easy to maintain: Changes to the core C3 algorithm automatically benefit all subclasses

Cons:

• Bug prone: Adding many if statements in the base C3 class when we need to access the new slack variable

Side-by-Side Replacement (Separate Class)

I did think about this option before, but ended up going with the subclass route to keep duplication to a minimum.

Pros:

• Complete independence: no more if statements; we can evolve C3NextGen without any constraints from the original C3

Cons:

• Massive code duplication
• Bug fixes and improvements (e.g, CalcCost) would need to be applied to both codebases

Additional Abstract Layer

Pros:

• Better separation: we could extract common interfaces and reduce coupling; C4 can introduce new variables without affecting the base C3 class.

Cons:

• It would require major changes to the existing C3 and its subclasses

New design pattern

I don't currently have a strong design pattern in mind, but I'm open to any suggestions.

[xuanhien070594]

examples/sampling_c3/start_logging.py line 18 at r1 (raw file):

Previously, mposa (Michael Posa) wrote…

I think if you're using exist_ok=True, then you don't need to check if the directory exists first.

Ah, yes, that's redundant. I'll remove the check.

[xuanhien070594]

.gitignore line 15 at r1 (raw file):

Previously, mposa (Michael Posa) wrote…

Why is this being added to .gitignore? Looks like some local files leaking from your end.

Thanks for catching that. Since the log script defaults to the logs folder, I need to add this to ensure Git doesn't track the log files.

[mposa]

Additional Abstract Layer

Pros:

• Better separation: we could extract common interfaces and reduce coupling; C4 can introduce new variables without affecting the base C3 class.

Cons:

• It would require major changes to the existing C3 and its subclasses

Can you expand a bit on this Con? What major changes would be needed?

[xuanhien070594]

Additional Abstract Layer

Pros:

• Better separation: we could extract common interfaces and reduce coupling; C4 can introduce new variables without affecting the base C3 class.

Cons:

• It would require major changes to the existing C3 and its subclasses

Can you expand a bit on this Con? What major changes would be needed?

If we choose this approach, both the constructor and SolveQP will need to be refactored. We'll extract shared code from these functions so it can be reused across C3 subclasses and C4. I think it is a significant internal change to the C3 class, but it won't affect downstream usage.

For example, for the SolveQP function, I'll do

std::vector<Eigen::VectorXd> BaseClass::SolveQP(
    const VectorXd& x0, const std::vector<MatrixXd>& G,
    const std::vector<VectorXd>& WD, int admm_iteration, bool is_final_solve) {

  ClearConstraints();
  AddInitialStateConstraint(x0);
  SolvePassiveLCPIfApplicable(x0);
  ClearCosts();

  for (int i = 0; i < N_; ++i) {
    const MatrixXd& G_i = G[i];
    const VectorXd& WD_i = WD[i];
    AddMatchingCosts(i, G_i, WD_i);  // implemented by C3 subclasses and C4
  }

  // Solve the QP, extract solution, return
  // ...
}

[mposa]

Does this require more than just moving the shared code to the common interface?

[xuanhien070594]

Does this require more than just moving the shared code to the common interface?

I think moving the shared code to the common interface should be enough.

[mposa]

Then from your Pro/Con list, that would seem to be the best. Perhaps you can take a deeper dive and consider whether this would actually lead to more readable and maintainable code? If/else statements to switch between configurations is acceptable at times, but once they have to appear in many different places (for the same configuration switch), it starts to get pretty unwieldy.

[xuanhien070594]

Then from your Pro/Con list, that would seem to be the best. Perhaps you can take a deeper dive and consider whether this would actually lead to more readable and maintainable code? If/else statements to switch between configurations is acceptable at times, but once they have to appear in many different places (for the same configuration switch), it starts to get pretty unwieldy.

Agree, I'll give it a try.

[haorany8]

A small problem in Sampling_based_c3_controller.cc in line 861 and line 853. Eigen::VectorXd u = zs[i].tail(n_u_); should be changed to Eigen::VectorXd u = zs[i].segment(n_x_+n_lambda,n_u_);

[mposa]

Reviewed 1 of 17 files at r1, 1 of 5 files at r2, 30 of 32 files at r3, all commit messages.
Reviewable status: 32 of 34 files reviewed, 8 unresolved discussions (waiting on @ebianchi, @PDKy, @xuanhien070594, and @yufeiyg)

---

a discussion (no related file):
Most importantly, it doesn't seem like you moved common code to a shared interface or abstract class. This PR largely seems:

1. Rename C3 to BaseC3
2. Move some of the C3 code into methods
3. Overwrite those methods in C3+

I thought the plan would be to:

1. Move everything in C3 that's shared with C3+ (not overwritten) into C3Base
2. Have C3 extend C3Base, keeping it's code that's not shared
3. Have C3+ extend C3Base

The advantage of this approach is for when someone tries to read the C3+ code, they know where to look. Right now, they have to figure out what code in BaseC3 is and isn't ovewritten.

Nit: Should we really be using the "+" symbol in file names?

---

examples/sampling_c3/parameter_headers/sampling_c3_options.h line 86 at r3 (raw file):

  // Only applicable for C3+
  std::vector<std::vector<double>> g_eta_slack_position_list;

See previous comment--should these be their own YAML file? Or do we want to leave a full reorganization of the YAML files until another date, and accept some ugliness for the moment.

---

examples/sampling_c3/anything/parameters/sampling_c3_options.yaml line 146 at r3 (raw file):

u_lambda: []

# Only applicable for C3+

I feel like C3+ should have it's own options file and yaml parser, forcing these 4e

---

examples/sampling_c3/jacktoy/parameters/sampling_c3_options.yaml line 276 at r3 (raw file):

u_eta_n_position_list: []
u_eta_t_position_list: []
u_eta_position_list: []

Missing new line at end of file

---

solvers/base_c3.h line 18 at r3 (raw file):

namespace solvers {

class BaseC3 {

Suggest naming it C3Base, instead of BaseC3.

---

solvers/base_c3.h line 32 at r3 (raw file):

  };

  using CalcZSizeFunc = std::function<int(const LCS&)>;

This method needs documentation. Perhaps it will be come clear when reading the implementations, but this API strikes me as very odd.

---

solvers/c3_options.h line 177 at r3 (raw file):

    }
    g_vector.insert(g_vector.end(), g_u.begin(), g_u.end());
    if (projection_type == "C3+") {

Along with the YAML files, this is definitely the wrong way to do it, but maybe we're willing to defer this.

[xuanhien070594]

Reviewable status: 7 of 34 files reviewed, 8 unresolved discussions (waiting on @ebianchi, @Meow404, @mposa, @PDKy, and @yufeiyg)

---

examples/sampling_c3/anything/parameters/sampling_c3_options.yaml line 146 at r3 (raw file):

Previously, mposa (Michael Posa) wrote…

I feel like C3+ should have it's own options file and yaml parser, forcing these 4e

We decided to defer the full refactor of the options file to a later time.

---

examples/sampling_c3/jacktoy/parameters/sampling_c3_options.yaml line 276 at r3 (raw file):

Previously, mposa (Michael Posa) wrote…

Missing new line at end of file

Done

---

examples/sampling_c3/parameter_headers/sampling_c3_options.h line 86 at r3 (raw file):

Previously, mposa (Michael Posa) wrote…

See previous comment--should these be their own YAML file? Or do we want to leave a full reorganization of the YAML files until another date, and accept some ugliness for the moment.

We decided to do that to a later time

---

solvers/base_c3.h line 18 at r3 (raw file):

Previously, mposa (Michael Posa) wrote…

Suggest naming it C3Base, instead of BaseC3.

Done

---

solvers/base_c3.h line 32 at r3 (raw file):

Previously, mposa (Michael Posa) wrote…

This method needs documentation. Perhaps it will be come clear when reading the implementations, but this API strikes me as very odd.

Done. The odd function has been removed and replaced with a new constructor that takes z_size as an argument.

[mposa]

Reviewed 27 of 27 files at r4, all commit messages.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on @ebianchi, @Meow404, @PDKy, @xuanhien070594, and @yufeiyg)

---

solvers/c3_plus.cc line 107 at r4 (raw file):

  // Handle complementarity constraints for each lambda-eta pair
  for (int i = 0; i < m_; ++i) {

See my comment in the header, but wouldn't this code work? It's fully vectorized, though I suppose speed here doesn't really matter--but I think having fewer steps does help readability.

Eigen::Ref<VectorXd> lambda_c =  delta_c.segment(n_, m_);
Eigen::Ref<VectorXd> eta_c=  delta_c.segment(n_ + m_ + k_, m_);

//Set the smaller of lambda and eta to zero
Eigen::Array<bool, Eigen::Dynamic, 1> eta_larger =lambda_c.array() <  std::sqrt(w_eta / w_lambda) * eta_c.array();
delta_proj.segment(n_, m_)= eta_larger.select(VectorXd::Zero(m_), lambda_c);
delta_proj.segment(n_ + m_ + k_, m_)=eta_larger.select(eta_c, VectorXd::Zero(m_));

// Clip lambda and eta at 0
delta_proj.segment(n_, m_) = delta_proj.segment(n_, m_).cwiseMax(0);
delta_proj.segment(n_ + m_ + k_, m_) = delta_proj.segment(n_ + m_ + k_, m_).cwiseMax(0);

---

solvers/base_c3.h line 40 at r4 (raw file):

  ///                      - C3MIQP / C3QP: z = [x, u, lambda]
  ///                      - C3Plus:        z = [x, u, lambda, eta]
  C3Base(const LCS& LCS, const CostMatrices& costs,

Should this constructor be protected? Users should never use it, right? If so, it should also be clearly documented that this is for internal use only.

---

solvers/c3_plus.h line 46 at r4 (raw file):

// 3. λ₀ > 0 and η₀ <= 0, then λ = λ₀ and η = 0
// 4. λ₀ > 0, η₀ > 0, and η₀ > sqrt(w_λ/w_η) * λ₀, then λ = 0 and η = η₀
// 5. λ₀ > 0, η₀ > 0, and η₀ <= sqrt(w_λ/w_η) * λ₀, then λ = λ₀ and η = 0

I'm not sure, but would this be an easier way to describe (and code) the projections?

1. if eta > sqrt(w_lambda/w_eta), then lambda=0, else eta=0
2. [lambda, eta] = max(0, [lambda, eta])

I feel like the 5 steps above are unnecessarily complicated.

---

solvers/c3_plus.h line 77 at r4 (raw file):

}  // namespace solvers
}  // namespace dairlib

missing newline

[xuanhien070594]

Reviewable status: 25 of 39 files reviewed, 6 unresolved discussions (waiting on @ebianchi, @Meow404, @mposa, @PDKy, and @yufeiyg)

---

solvers/c3_plus.h line 46 at r4 (raw file):

Previously, mposa (Michael Posa) wrote…

I'm not sure, but would this be an easier way to describe (and code) the projections?

1. if eta > sqrt(w_lambda/w_eta), then lambda=0, else eta=0
2. [lambda, eta] = max(0, [lambda, eta])

I feel like the 5 steps above are unnecessarily complicated.

Agree, it feels easier to follow.

---

solvers/c3_plus.h line 77 at r4 (raw file):

Previously, mposa (Michael Posa) wrote…

missing newline

Done

---

solvers/c3_plus.cc line 107 at r4 (raw file):

Previously, mposa (Michael Posa) wrote…

See my comment in the header, but wouldn't this code work? It's fully vectorized, though I suppose speed here doesn't really matter--but I think having fewer steps does help readability.

Eigen::Ref<VectorXd> lambda_c =  delta_c.segment(n_, m_);
Eigen::Ref<VectorXd> eta_c=  delta_c.segment(n_ + m_ + k_, m_);

//Set the smaller of lambda and eta to zero
Eigen::Array<bool, Eigen::Dynamic, 1> eta_larger =lambda_c.array() <  std::sqrt(w_eta / w_lambda) * eta_c.array();
delta_proj.segment(n_, m_)= eta_larger.select(VectorXd::Zero(m_), lambda_c);
delta_proj.segment(n_ + m_ + k_, m_)=eta_larger.select(eta_c, VectorXd::Zero(m_));

// Clip lambda and eta at 0
delta_proj.segment(n_, m_) = delta_proj.segment(n_, m_).cwiseMax(0);
delta_proj.segment(n_ + m_ + k_, m_) = delta_proj.segment(n_ + m_ + k_, m_).cwiseMax(0);

Done. Thanks for the suggestions. I switched to fully vectorized version of C3+ projection step.

[mposa]

Reviewed 13 of 14 files at r5, all commit messages.
Reviewable status: 38 of 39 files reviewed, 6 unresolved discussions (waiting on @ebianchi, @Meow404, @PDKy, @xuanhien070594, and @yufeiyg)

---

solvers/c3_plus.cc line 107 at r4 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Done. Thanks for the suggestions. I switched to fully vectorized version of C3+ projection step.

Any reason you're using VectorXd instead of Eigen::Ref? VectorXd forces a copy. I admit that efficiency doesn't matter much here, but am curious if you see a reason to do it this way.

This goes for w_eta_vec, w_lambda_vec, lambda_c, eta_c`

---

solvers/c3_plus.cc line 108 at r5 (raw file):

  // Extract the weight vectors for lambda and eta from the diagonal of the cost
  // matrix U.
  // Use absolute values to ensure numerical safety when taking square roots,

Wouldn't we rather have an error than just let someone supply negative weights? Is the abs actually saving us, or just hiding user errors?

---

examples/sampling_c3/jacktoy/franka_hardware_jack.pmd line 11 at r5 (raw file):

    }
    cmd "logger" {
        exec = "python3 examples/sampling_c3/start_logging.py hw jacktoy /mnt/data2/jacktoy/logs/";

Should we really be committing scripts which log to specific locations that likely won't exist? This goes for all of these pmd files

[xuanhien070594]

Reviewable status: 38 of 39 files reviewed, 6 unresolved discussions (waiting on @ebianchi, @Meow404, @mposa, @PDKy, and @yufeiyg)

---

examples/sampling_c3/jacktoy/franka_hardware_jack.pmd line 11 at r5 (raw file):

Previously, mposa (Michael Posa) wrote…

Should we really be committing scripts which log to specific locations that likely won't exist? This goes for all of these pmd files

These locations have been used in our sampling-based C3 project, so I set them as the default.

---

solvers/c3_plus.cc line 107 at r4 (raw file):

Previously, mposa (Michael Posa) wrote…

Any reason you're using VectorXd instead of Eigen::Ref? VectorXd forces a copy. I admit that efficiency doesn't matter much here, but am curious if you see a reason to do it this way.

This goes for w_eta_vec, w_lambda_vec, lambda_c, eta_c`

The .diagonal() operation returns a temporary expression, which cannot be bound to an Eigen::Ref, leading to a compilation error. Also, the compiler does not allow mixing Eigen::Ref and Eigen::VectorXd in operations like the computation of eta_larger. That's why I can't use Eigen::Ref.

---

solvers/c3_plus.cc line 108 at r5 (raw file):

Previously, mposa (Michael Posa) wrote…

Wouldn't we rather have an error than just let someone supply negative weights? Is the abs actually saving us, or just hiding user errors?

Done, now, it'll throw error if users provide negative weights.

[mposa]

Reviewable status: 38 of 39 files reviewed, 6 unresolved discussions (waiting on @ebianchi, @Meow404, @PDKy, @xuanhien070594, and @yufeiyg)