feat(c3-controller) : c3 drake controller leaf system

[Meow404]

TODO :

• Working C3 controller test + LCS simulator
• Cleanup C3 controller code and slight restructure
  • Add a method to set user constraints in C3 controller
  • Allow C3 controller to set custom cost matrices
  • Clean up and restructure C3 options to make it more readable
  • Update unit tests
  • Clean up C3 test and C3 controller test
• Add python bindings
• Create issues for any pending problems or potential improvements
  • [feature] Refactor C3Options for Improved Structure and Maintainability #8
  • [Bug] osqp_.solve Silent Failures Leading to Stale z_sol_ Values #6

This change is 

[xuanhien070594]

systems/test/res/c3_controller_test_diagram.ps line 0 at r2 (raw file):
Please remove this if we don't use it anywhere.

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 19 at r2 (raw file):

// A system that integrates a Cartpole model with Drake's SceneGraph for
// visualization.
class CartpoleGeometry : public drake::systems::LeafSystem<double> {

I think the class name should be CartpoleWithSoftWallsVisualizer or CartpoleVisualizer

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 34 at r2 (raw file):

    // Declare input and output ports.
    state_input_port_ = this->DeclareVectorInputPort(
                                "state", drake::systems::BasicVector<double>(4))

Remove magic number. You can replace it by calling plant.num_multibody_states()

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 36 at r2 (raw file):

                                "state", drake::systems::BasicVector<double>(4))
                            .get_index();
    scene_graph_state_output_port_ =

Using scene_graph_state_output_port_ seems a bit confusing, maybe just call it geometry_pose_output_port_

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 43 at r2 (raw file):

  // Returns the input port for the Cartpole state.
  const drake::systems::InputPort<double>& get_input_port_state() const {

Nit. We can change the name of this function to get_state_input_port()

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 48 at r2 (raw file):

  // Returns the output port for the SceneGraph state.
  const drake::systems::OutputPort<double>& get_output_port_scene_graph_state()

Nit. Change the function name to get_geometry_pose_output_port

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 60 at r2 (raw file):

      const drake::systems::OutputPort<double>& state_port,
      double time_step = 0.0) {
    // Create the CartpoleGeometry system.

It might be safer to add check DRAKE_DEMAND(builder != nullptr && scene_graph != nullptr);

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 61 at r2 (raw file):

      double time_step = 0.0) {
    // Create the CartpoleGeometry system.
    CartpoleGeometry* geometry =

Nit. Consider to use auto for better readability

[xuanhien070594]

systems/test/c3_controller_test.cc line 109 at r2 (raw file):

// Utility function to save and visualize the system diagram.
void DrawAndSaveDiagramGraph(const drake::systems::Diagram<double>& diagram,

Move this function to a separate source file, we can call it system_utils.cc

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 24 at r2 (raw file):

  // SceneGraph.
  CartpoleGeometry(SceneGraph<double>* scene_graph, double time_step = 0.0) {
    plant = new MultibodyPlant<double>(time_step);

Please avoid using raw pointer, it might cause memory leak if we don't handle proper destruction. Can we just use stack allocation here plant = MultibodyPlant<double>(time_step);

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 15 at r2 (raw file):

namespace c3 {
namespace test {

We should put test namespace under systems namespace

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 83 at r2 (raw file):

  }

  MultibodyPlant<double>* plant;  // Pointer to the MultibodyPlant.

Related to one of above comments, we can just use std::unique_ptr<drake::multibody::MultibodyPlant<double>> plant;

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 24 at r2 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Please avoid using raw pointer, it might cause memory leak if we don't handle proper destruction. Can we just use stack allocation here plant = MultibodyPlant<double>(time_step);

Or using smart pointer plant = std::make_unique<MultibodyPlant<double>>(time_step);

[xuanhien070594]

systems/test/cartpole_geometry.hpp line 1 at r2 (raw file):

#include <drake/multibody/parsing/parser.h>

Is it class necessary? Since this class is only used for testing, it might be better to directly add MultibodyPositionToGeometryPose system in the c3_controller_test.cc

[xuanhien070594]

systems/test/c3_controller_test.cc line 200 at r2 (raw file):

  auto diagram_ = builder.Build();

  // DrawAndSaveDiagramGraph(*diagram_,

Please remove. Or add flag to enable this feature if necessary. You can use gflags.

[xuanhien070594]

systems/test/c3_controller_test.cc line 144 at r2 (raw file):

  drake::systems::ZeroOrderHold<double>* state_zero_order_hold =
      builder.AddSystem<drake::systems::ZeroOrderHold<double>>(
          1 / c3_cartpole_problem.options.publish_frequency, 4);

Magic number

[xuanhien070594]

systems/test/c3_controller_test.cc line 135 at r2 (raw file):

  // Initialize the C3 cartpole problem.
  C3CartpoleProblem c3_cartpole_problem = C3CartpoleProblem();

Nit. Use auto to improve readability.

[xuanhien070594]

systems/test/c3_controller_test.cc line 135 at r2 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Nit. Use auto to improve readability.

Same comments for LCSSimulator* and drake::systems::ZeroOrderHold*

[xuanhien070594]

systems/test/c3_controller_test.cc line 63 at r2 (raw file):

  // Compute the action from the C3 solution.
  void GetC3Action(const drake::systems::Context<double>& context,

Use control inputs or inputs instead of action for consistency.

[xuanhien070594]

systems/c3_controller.cc line 25 at r2 (raw file):

C3Controller::C3Controller(
    const drake::multibody::MultibodyPlant<double>& plant,
    const C3::CostMatrices& costs, C3ControllerOptions c3_options)

You should name it c3_controller_options or controller_options

[xuanhien070594]

systems/c3_controller.cc line 27 at r2 (raw file):

    const C3::CostMatrices& costs, C3ControllerOptions c3_options)
    : plant_(plant),
      c3_options_(std::move(c3_options)),

Does C3ControllerOptions have move constructor? If not, then using std::move here is redundant as it'll fall back to the copy constructor

[xuanhien070594]

systems/c3_controller.h line 70 at r2 (raw file):

   * @param options The solver options to configure.
   */
  void SetOsqpSolverOptions(const drake::solvers::SolverOptions& options);

Do we need this function? I believe the C3 class has this function already.

[xuanhien070594]

bindings/test/c3_systems_py_test.py line 24 at r2 (raw file):

)

class CartpoleGeometry(LeafSystem):

I think some classes here are already implemented in C++.

[xuanhien070594]

core/c3.cc line 400 at r2 (raw file):

  // can be disabled with DRAKE_ASSERT_IS_DISARMED is defined
  DRAKE_ASSERT(result.is_success());

Use DRAKE_DEMAND as this only works in debug mode.

[xuanhien070594]

core/c3_options.h line 170 at r2 (raw file):

}

struct C3ControllerOptions : public C3Options {

Can SolverOptions be migrated into this class?

[xuanhien070594]

core/lcs.h line 155 at r2 (raw file):

  int k_;

  bool is_placeholder_ = false;

Any reason that we need this flag?

[xuanhien070594]

Reviewed 14 of 38 files at r1, 8 of 14 files at r2, all commit messages.
Reviewable status: 22 of 38 files reviewed, 23 unresolved discussions (waiting on @Meow404)