test(c3): Add unit tests for C3 package

[Meow404]

• Added some helper functions to C3 for testing purposes
• Added unit tests for C3 package
• Added E2E Cartpole test
• File refactoring

---

This change is 

[xuanhien070594]

Reviewable status: 0 of 8 files reviewed, 3 unresolved discussions

---

external/gmock.BUILD line 1 at r1 (raw file):

cc_library(

Pls remove this file as it is redundant.

---

WORKSPACE line 14 at r1 (raw file):

# Maybe download Drake.
load("@bazel_tools//tools/build_defs/repo:git.bzl", "new_git_repository")

Pls remove this line.

---

core/BUILD.bazel line 61 at r1 (raw file):

        "@gtest//:main",
    ],
    env_inherit = [

Pls remove env_inherit

[Meow404]

Reviewable status: 0 of 8 files reviewed, 3 unresolved discussions (waiting on @xuanhien070594)

---

WORKSPACE line 14 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Pls remove this line.

Done

---

core/BUILD.bazel line 61 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Pls remove env_inherit

This does not work locally for me without env_inherit. My best guess for this is Bazel creates an isolated environment for tests and the test errors out without access to this variables.

---

external/gmock.BUILD line 1 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Pls remove this file as it is redundant.

Done

[xuanhien070594]

Reviewable status: 0 of 8 files reviewed, 3 unresolved discussions (waiting on @Meow404)

---

core/BUILD.bazel line 61 at r1 (raw file):

Previously, Meow404 (Thomas Stephen Felix) wrote…

This does not work locally for me without env_inherit. My best guess for this is Bazel creates an isolated environment for tests and the test errors out without access to this variables.

I've tried to build this branch without env_inherit and it worked. Can you try again with a fresh build (run bazel clean expunge before run bazel build ...)?

[Meow404]

Reviewable status: 0 of 8 files reviewed, 3 unresolved discussions (waiting on @xuanhien070594)

---

core/BUILD.bazel line 61 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

I've tried to build this branch without env_inherit and it worked. Can you try again with a fresh build (run bazel clean expunge before run bazel build ...)?

It builds successfully. I have an issue running bazel test --test_output=all //core:c3_test

[xuanhien070594]

core/BUILD.bazel line 61 at r1 (raw file):

Previously, Meow404 (Thomas Stephen Felix) wrote…

It builds successfully. I have an issue running bazel test --test_output=all //core:c3_test

You're right. Running bazel test will create sandbox and isolated environment, so we should keep these lines.

[mposa]

Reviewed 3 of 8 files at r1, 1 of 2 files at r2, all commit messages.
Reviewable status: 4 of 8 files reviewed, 7 unresolved discussions (waiting on @Meow404 and @xuanhien070594)

---

core/c3.h line 61 at r2 (raw file):

  /*!
   * Get a copy of the list of the dynamic constraints

This method needs more clear documentation on the returned constraint object. What is the format of the dynamics constraints? (decision variables, how it's expressed, and the role of the vector as the returned object)

---

core/c3.h line 74 at r2 (raw file):

   * Get the current cost associated with the target reference trajectory
   */
  const std::vector<drake::solvers::QuadraticCost*>& GetTargetCost();

This method needs more clear documentation on the returned constraint object. What is the format of the cost object? Specifically, the decision variables.

---

core/test/c3_test.cc line 15 at r2 (raw file):

using std::vector;

using c3::C3Options;

Can you add a bit of high-level documentation to the file, about what this test checks for (and maybe what it doesn't)?

---

core/test/c3_test.cc line 267 at r2 (raw file):

}

// Test if user can update the target stateLCY system

Maybe I missed it, but what is the target stateLCY system? Or is this a typo?

[Meow404]

Reviewable status: 3 of 8 files reviewed, 7 unresolved discussions (waiting on @mposa and @xuanhien070594)

---

core/c3.h line 61 at r2 (raw file):

Previously, mposa (Michael Posa) wrote…

This method needs more clear documentation on the returned constraint object. What is the format of the dynamics constraints? (decision variables, how it's expressed, and the role of the vector as the returned object)

Done.

---

core/c3.h line 74 at r2 (raw file):

Previously, mposa (Michael Posa) wrote…

This method needs more clear documentation on the returned constraint object. What is the format of the cost object? Specifically, the decision variables.

Done.

---

core/test/c3_test.cc line 15 at r2 (raw file):

Previously, mposa (Michael Posa) wrote…

Can you add a bit of high-level documentation to the file, about what this test checks for (and maybe what it doesn't)?

Done.

---

core/test/c3_test.cc line 267 at r2 (raw file):

Previously, mposa (Michael Posa) wrote…

Maybe I missed it, but what is the target stateLCY system? Or is this a typo?

Done.

[xuanhien070594]

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

   *              | λᵢ |
   *              |xᵢ₊₁|
   * (Aᵢ, Dᵢ, Bᵢ, dᵢ) are defined by the LCS (Linear Complimentary System). 

The orders are wrong. They should be [x_i, \lambda_i, u_i, x_{i+1}] and [A_i, B_i, D_i, -1]

[xuanhien070594]

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

   *              |xᵢ₊₁|
   * (Aᵢ, Dᵢ, Bᵢ, dᵢ) are defined by the LCS (Linear Complimentary System). 
   * (xᵢ, uᵢ, λᵢ) are optimization variables for state, action and force respectively

We should follow the correct order in the paper (x_i, \lambda_i, u_i).

[xuanhien070594]

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

   * lb ≼ Aλᵢ ≼ ub
   * xᵢ     : State variable at the ith timestep
   * uᵢ     : Action variable at the ith timestep

Nit. We should use Input instead of Action.

[xuanhien070594]

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

#include <vector>
#include <span>

This is unused. Pls remove it.

[xuanhien070594]

Reviewed 4 of 8 files at r1, 2 of 2 files at r2, 2 of 2 files at r3, all commit messages.
Reviewable status: all files reviewed, 11 unresolved discussions (waiting on @Meow404 and @mposa)

[Meow404]

Reviewable status: 7 of 8 files reviewed, 10 unresolved discussions (waiting on @mposa and @xuanhien070594)

---

core/BUILD.bazel line 61 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

You're right. Running bazel test will create sandbox and isolated environment, so we should keep these lines.

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

This is unused. Pls remove it.

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

The orders are wrong. They should be [x_i, \lambda_i, u_i, x_{i+1}] and [A_i, B_i, D_i, -1]

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

We should follow the correct order in the paper (x_i, \lambda_i, u_i).

Done.

[xuanhien070594]

Reviewed 1 of 1 files at r4, all commit messages.
Reviewable status: all files reviewed, 10 unresolved discussions (waiting on @mposa)