example(plate-balancing) : Add working plate balancing example with external c3 repo

MODULE.bazel

@@ -89,10 +89,16 @@ archive_override(
     strip_prefix = "drake-{}".format(DRAKE_VERSION.lstrip("v")),
 )
 
+bazel_dep(name = "c3")
+git_override(
+    module_name = "c3",
+    remote = "https://github.com/DAIRLab/c3.git",
+    commit = "ed1cb0ed199901aeae85725502b583a2b40c9553"
+)
+
 INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69"
 INEKF_CHECKSUM = "c5a056ce00e1625e52f5a71b1d5c202acd53c1a8c7bca33da458db1e4f3f2edf"
 
-
 # Before changing the COMMIT, temporarily uncomment the next line so that Bazel
 # displays the suggested new value for the CHECKSUM.
 #INEKF_CHECKSUM = "0" * 64

bindings/pydairlib/solvers/c3_py.cc

@@ -1,7 +1,7 @@
+#include <drake/bindings/pydrake/common/sorted_pair_pybind.h>
 #include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
-#include <drake/bindings/pydrake/common/sorted_pair_pybind.h>
 
 #include "solvers/c3_miqp.h"
 #include "solvers/lcs.h"
@@ -23,11 +23,11 @@ namespace pyc3 {
 PYBIND11_MODULE(c3, m) {
   py::class_<dairlib::solvers::LCS>(m, "LCS")
       .def(py::init<const vector<MatrixXd>&, const vector<MatrixXd>&,
-                   const vector<MatrixXd>&, const vector<VectorXd>&,
-                   const vector<MatrixXd>&, const vector<MatrixXd>&,
-                   const vector<MatrixXd>&, const vector<VectorXd>&, double>(),
-          arg("A"), arg("B"), arg("D"), arg("d"), arg("E"), arg("F"), arg("H"),
-          arg("c"), arg("dt"))
+                    const vector<MatrixXd>&, const vector<VectorXd>&,
+                    const vector<MatrixXd>&, const vector<MatrixXd>&,
+                    const vector<MatrixXd>&, const vector<VectorXd>&, double>(),
+           arg("A"), arg("B"), arg("D"), arg("d"), arg("E"), arg("F"), arg("H"),
+           arg("c"), arg("dt"))
       .def(py::init<const MatrixXd&, const MatrixXd&, const MatrixXd&,
                     const MatrixXd&, const MatrixXd&, const MatrixXd&,
                     const MatrixXd&, const VectorXd&, int, double>(),
@@ -44,17 +44,10 @@ PYBIND11_MODULE(c3, m) {
       .def("Simulate", &LCS::Simulate);
 
   m.def("LinearizePlantToLCS",
-        &dairlib::solvers::LCSFactory::LinearizePlantToLCS,
-        py::arg("plant"),
-        py::arg("context"),
-        py::arg("plant_ad"),
-        py::arg("context_ad"),
-        py::arg("contact_geoms"),
-        py::arg("num_friction_directions"),
-        py::arg("mu"),
-        py::arg("dt"),
-        py::arg("N"),
-        py::arg("contact_model"));
+        &dairlib::solvers::LCSFactory::LinearizePlantToLCS, py::arg("plant"),
+        py::arg("context"), py::arg("plant_ad"), py::arg("context_ad"),
+        py::arg("contact_geoms"), py::arg("num_friction_directions"),
+        py::arg("mu"), py::arg("dt"), py::arg("N"), py::arg("contact_model"));
 
   {
     using Enum = dairlib::solvers::ContactModel;
@@ -64,13 +57,13 @@ PYBIND11_MODULE(c3, m) {
         .value("kAnitescu", Enum::kAnitescu);
   }
 
-  py::class_<dairlib::solvers::C3::CostMatrices>(m, "CostMatrices")
+  py::class_<dairlib::solvers::C3Base::CostMatrices>(m, "CostMatrices")
       .def(py::init<const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
                     const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&>(),
            arg("Q"), arg("R"), arg("G"), arg("U"));
 
   py::class_<dairlib::solvers::C3MIQP>(m, "C3MIQP")
-      .def(py::init<const LCS&, const dairlib::solvers::C3::CostMatrices&,
+      .def(py::init<const LCS&, const dairlib::solvers::C3Base::CostMatrices&,
                     const vector<VectorXd>&, const C3Options&>(),
            arg("LCS"), arg("costs"), arg("x_des"), arg("c3_options"))
       .def("Solve", &C3MIQP::Solve, arg("x0"), arg("verbose") = false)
@@ -80,10 +73,11 @@ PYBIND11_MODULE(c3, m) {
            arg("G"), arg("admm_iteration"), arg("verbose") = false)
       .def("SolveQP", &C3MIQP::SolveQP, arg("x0"), arg("G"), arg("WD"),
            arg("admm_iteration"), arg("is_final_solve"))
-      .def("SolveProjection", &C3MIQP::SolveProjection, arg("U"), arg("WZ"), arg("admm_iteration"))
+      .def("SolveProjection", &C3MIQP::SolveProjection, arg("U"), arg("WZ"),
+           arg("admm_iteration"))
       .def("AddLinearConstraint", &C3MIQP::AddLinearConstraint, arg("A"),
            arg("lower_bound"), arg("upper_bound"), arg("constraint"))
-      .def("RemoveConstraints", &C3MIQP::RemoveConstraints)
+      .def("RemoveConstraints", &C3MIQP::RemoveUserConstraints)
       .def("GetFullSolution", &C3MIQP::GetFullSolution)
       .def("GetStateSolution", &C3MIQP::GetStateSolution)
       .def("GetForceSolution", &C3MIQP::GetForceSolution)
@@ -102,8 +96,6 @@ PYBIND11_MODULE(c3, m) {
       .def_readwrite("warm_start", &C3Options::warm_start)
       .def_readwrite("use_predicted_x0", &C3Options::use_predicted_x0)
       .def_readwrite("end_on_qp_step", &C3Options::end_on_qp_step)
-      .def_readwrite("use_robust_formulation",
-                     &C3Options::use_robust_formulation)
       .def_readwrite("solve_time_filter_alpha",
                      &C3Options::solve_time_filter_alpha)
       .def_readwrite("publish_frequency", &C3Options::publish_frequency)

examples/Cassie/cassie_state_estimator.cc

@@ -1102,6 +1102,9 @@ void CassieStateEstimator::DoCalcNextUpdateTime(
         const auto& self = dynamic_cast<const CassieStateEstimator&>(system);
         return self.Update(c, s);
       };
+      auto& uu_events = events->get_mutable_unrestricted_update_events();
+      uu_events.AddEvent(UnrestrictedUpdateEvent<double>(
+          drake::systems::TriggerType::kTimed, callback));
     } else {
       *time = INFINITY;
     }

examples/Cassie/diagrams/osc_running_controller_diagram.cc

@@ -64,7 +64,8 @@ namespace controllers {
 
 OSCRunningControllerDiagram::OSCRunningControllerDiagram(
     drake::multibody::MultibodyPlant<double>& plant,
-    const string& osc_running_gains_filename, const string& osqp_settings_filename)
+    const string& osc_running_gains_filename,
+    const string& osqp_settings_filename)
     : plant_(&plant),
       pos_map(multibody::MakeNameToPositionsMap(plant)),
       vel_map(multibody::MakeNameToVelocitiesMap(plant)),
@@ -155,19 +156,20 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
   auto state_receiver = builder.AddSystem<systems::RobotOutputReceiver>(plant);
   auto command_sender = builder.AddSystem<systems::RobotCommandSender>(plant);
   auto osc = builder.AddSystem<systems::controllers::OperationalSpaceControl>(
-      plant, plant, plant_context.get(), plant_context.get(), true);
+      plant, plant_context.get(), true);
   auto radio_parser = builder.AddSystem<systems::RadioParser>();
   auto failure_aggregator =
       builder.AddSystem<systems::ControllerFailureAggregator>(
           control_channel_name_, 1);
 
   /**** OSC setup ****/
   /// REGULARIZATION COSTS
-  osc->SetAccelerationCostWeights(osc_running_gains.w_accel * osc_running_gains.W_acceleration);
+  osc->SetAccelerationCostWeights(osc_running_gains.w_accel *
+                                  osc_running_gains.W_acceleration);
   osc->SetInputSmoothingCostWeights(osc_running_gains.w_input_reg *
-      osc_running_gains.W_input_regularization);
+                                    osc_running_gains.W_input_regularization);
   osc->SetInputCostWeights(osc_running_gains.w_input *
-      osc_running_gains.W_input_regularization);
+                           osc_running_gains.W_input_regularization);
   osc->SetLambdaContactRegularizationWeight(
       osc_running_gains.w_lambda * osc_running_gains.W_lambda_c_regularization);
   osc->SetLambdaHolonomicRegularizationWeight(
@@ -179,14 +181,22 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
   // Contact information for OSC
   osc->SetContactFriction(osc_running_gains.mu);
 
-  osc->AddStateAndContactPoint(RunningFsmState::kLeftStance,
-                               &left_toe_evaluator);
-  osc->AddStateAndContactPoint(RunningFsmState::kLeftStance,
-                               &left_heel_evaluator);
-  osc->AddStateAndContactPoint(RunningFsmState::kRightStance,
-                               &right_toe_evaluator);
-  osc->AddStateAndContactPoint(RunningFsmState::kRightStance,
-                               &right_heel_evaluator);
+  osc->AddContactPoint("left_toe",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &left_toe_evaluator),
+                       {RunningFsmState::kLeftStance});
+  osc->AddContactPoint("left_heel",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &left_heel_evaluator),
+                       {RunningFsmState::kLeftStance});
+  osc->AddContactPoint("right_toe",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &right_toe_evaluator),
+                       {RunningFsmState::kRightStance});
+  osc->AddContactPoint("right_heel",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &right_heel_evaluator),
+                       {RunningFsmState::kRightStance});
 
   // Fix the springs in the dynamics
   evaluators.add_evaluator(&left_fixed_knee_spring);
@@ -196,8 +206,9 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
   evaluators.add_evaluator(&left_loop);
   evaluators.add_evaluator(&right_loop);
 
-
-  osc->AddKinematicConstraint(&evaluators);
+  osc->AddKinematicConstraint(
+      std::unique_ptr<const multibody::KinematicEvaluatorSet<double>>(
+          &evaluators));
 
   /**** Tracking Data *****/
 
@@ -208,8 +219,8 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
       osc_running_gains.vel_scale_trans_lateral);
 
   auto pelvis_trans_traj_generator =
-      builder.AddSystem<PelvisTransTrajGenerator>(
-          plant, plant_context.get(), feet_contact_points);
+      builder.AddSystem<PelvisTransTrajGenerator>(plant, plant_context.get(),
+                                                  feet_contact_points);
   pelvis_trans_traj_generator->SetSLIPParams(
       osc_running_gains.rest_length, osc_running_gains.rest_length_offset);
   auto l_foot_traj_generator = builder.AddSystem<FootTrajGenerator>(
@@ -261,8 +272,8 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
   right_foot_tracking_data->AddStateAndPointToTrack(
       RunningFsmState::kLeftFlight, "toe_right");
 
-  left_foot_tracking_data->AddStateAndPointToTrack(
-      RunningFsmState::kLeftFlight, "toe_left");
+  left_foot_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight,
+                                                   "toe_left");
   right_foot_tracking_data->AddStateAndPointToTrack(
       RunningFsmState::kRightFlight, "toe_right");
 
@@ -274,17 +285,17 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
       "right_hip_traj", osc_running_gains.K_p_swing_foot,
       osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, plant,
       plant);
-  left_hip_tracking_data->AddStateAndPointToTrack(
-      RunningFsmState::kRightStance, "pelvis");
-  right_hip_tracking_data->AddStateAndPointToTrack(
-      RunningFsmState::kLeftStance, "pelvis");
-  right_hip_tracking_data->AddStateAndPointToTrack(
-      RunningFsmState::kLeftFlight, "pelvis");
-  left_hip_tracking_data->AddStateAndPointToTrack(
-      RunningFsmState::kRightFlight, "pelvis");
-
-  left_hip_tracking_data->AddStateAndPointToTrack(
-      RunningFsmState::kLeftFlight, "pelvis");
+  left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightStance,
+                                                  "pelvis");
+  right_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftStance,
+                                                   "pelvis");
+  right_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight,
+                                                   "pelvis");
+  left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightFlight,
+                                                  "pelvis");
+
+  left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight,
+                                                  "pelvis");
   right_hip_tracking_data->AddStateAndPointToTrack(
       RunningFsmState::kRightFlight, "pelvis");
 

examples/Cassie/diagrams/osc_walking_controller_diagram.cc

@@ -92,8 +92,8 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
       left_right_foot({left_toe_origin, right_toe_origin}),
       left_foot_points({left_heel, left_toe}),
       right_foot_points({right_heel, right_toe}),
-      view_frame_(
-          std::make_shared<multibody::WorldYawViewFrame<double>>(plant.GetBodyByName("pelvis"))),
+      view_frame_(std::make_shared<multibody::WorldYawViewFrame<double>>(
+          plant.GetBodyByName("pelvis"))),
       left_toe_evaluator(multibody::WorldPointEvaluator(
           plant, left_toe.first, left_toe.second, *view_frame_,
           Matrix3d::Identity(), Vector3d::Zero(), {1, 2})),
@@ -198,16 +198,17 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
   swing_ft_accel_gain_multiplier_samples[2](2, 2) *= 0;
   swing_ft_accel_gain_multiplier_samples[3](2, 2) *= 0;
   auto swing_ft_accel_gain_multiplier_gain_multiplier =
-      std::make_shared<PiecewisePolynomial<double>>(PiecewisePolynomial<double>::FirstOrderHold(
-          swing_ft_accel_gain_multiplier_breaks,
-          swing_ft_accel_gain_multiplier_samples));
+      std::make_shared<PiecewisePolynomial<double>>(
+          PiecewisePolynomial<double>::FirstOrderHold(
+              swing_ft_accel_gain_multiplier_breaks,
+              swing_ft_accel_gain_multiplier_samples));
 
   /**** Initialize all the leaf systems ****/
 
   auto state_receiver = builder.AddSystem<systems::RobotOutputReceiver>(plant);
   auto command_sender = builder.AddSystem<systems::RobotCommandSender>(plant);
   auto osc = builder.AddSystem<systems::controllers::OperationalSpaceControl>(
-      plant, plant, plant_context.get(), plant_context.get(), true);
+      plant, plant_context.get(), true);
   auto fsm = builder.AddSystem<systems::TimeBasedFiniteStateMachine>(
       plant, fsm_states, state_durations);
   auto liftoff_event_time =
@@ -234,28 +235,26 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
   // Contact information for OSC
   osc->SetContactFriction(osc_walking_gains.mu);
 
-  osc->AddStateAndContactPoint(left_stance_state, &left_toe_evaluator);
-  osc->AddStateAndContactPoint(left_stance_state, &left_heel_evaluator);
-  osc->AddStateAndContactPoint(right_stance_state, &right_toe_evaluator);
-  osc->AddStateAndContactPoint(right_stance_state, &right_heel_evaluator);
-  if (has_double_stance) {
-    osc->AddStateAndContactPoint(post_left_double_support_state,
-                                 &left_toe_evaluator);
-    osc->AddStateAndContactPoint(post_left_double_support_state,
-                                 &left_heel_evaluator);
-    osc->AddStateAndContactPoint(post_left_double_support_state,
-                                 &right_toe_evaluator);
-    osc->AddStateAndContactPoint(post_left_double_support_state,
-                                 &right_heel_evaluator);
-    osc->AddStateAndContactPoint(post_right_double_support_state,
-                                 &left_toe_evaluator);
-    osc->AddStateAndContactPoint(post_right_double_support_state,
-                                 &left_heel_evaluator);
-    osc->AddStateAndContactPoint(post_right_double_support_state,
-                                 &right_toe_evaluator);
-    osc->AddStateAndContactPoint(post_right_double_support_state,
-                                 &right_heel_evaluator);
-  }
+  osc->AddContactPoint("left_toe",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &left_toe_evaluator),
+                       {left_stance_state, post_left_double_support_state,
+                        post_right_double_support_state});
+  osc->AddContactPoint("left_heel",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &left_heel_evaluator),
+                       {left_stance_state, post_left_double_support_state,
+                        post_right_double_support_state});
+  osc->AddContactPoint("right_toe",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &right_toe_evaluator),
+                       {right_stance_state, post_left_double_support_state,
+                        post_right_double_support_state});
+  osc->AddContactPoint("right_heel",
+                       std::unique_ptr<multibody::WorldPointEvaluator<double>>(
+                           &right_heel_evaluator),
+                       {right_stance_state, post_left_double_support_state,
+                        post_right_double_support_state});
 
   evaluators.add_evaluator(&left_loop);
   evaluators.add_evaluator(&right_loop);
@@ -266,7 +265,8 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
   evaluators.add_evaluator(&left_fixed_ankle_spring);
   evaluators.add_evaluator(&right_fixed_ankle_spring);
 
-  osc->AddKinematicConstraint(&evaluators);
+  osc->AddKinematicConstraint(
+      std::unique_ptr<multibody::KinematicEvaluatorSet<double>>(&evaluators));
 
   /**** Trajectory Generators ****/
 
@@ -346,8 +346,10 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
       "swing_ft_traj", osc_walking_gains.K_p_swing_foot,
       osc_walking_gains.K_d_swing_foot, osc_walking_gains.W_swing_foot, plant,
       plant);
-  swing_ft_traj_global_->AddStateAndPointToTrack(left_stance_state, "toe_right");
-  swing_ft_traj_global_->AddStateAndPointToTrack(right_stance_state, "toe_left");
+  swing_ft_traj_global_->AddStateAndPointToTrack(left_stance_state,
+                                                 "toe_right");
+  swing_ft_traj_global_->AddStateAndPointToTrack(right_stance_state,
+                                                 "toe_left");
   swing_ft_traj_local_->SetTimeVaryingPDGainMultiplier(
       swing_ft_gain_multiplier_gain_multiplier);
   swing_ft_traj_local_->SetTimerVaryingFeedForwardAccelMultiplier(

examples/Cassie/networking/cassie_udp_publisher.cc

@@ -92,7 +92,7 @@ std::string CassieUDPPublisher::make_name(const std::string& address,
 // period or publish period = 0.0 was passed to the constructor).
 drake::systems::EventStatus CassieUDPPublisher::PublishInputAsUDPMessage(
     const drake::systems::Context<double>& context) const {
-  SPDLOG_TRACE(drake::log(), "Publishing UDP {} message", address_);
+  SPDLOG_TRACE("Publishing UDP {} message", address_);
 
   // Converts the input into message bytes.
   const drake::AbstractValue* const input_value =