Change the OSC structure to make it fully compliant with the systems framework.

WORKSPACE

@@ -3,6 +3,23 @@
 
 workspace(name = "dairlib")
 
+# Support for apple toolchains on Bazel 7+
+# https://github.com/bazelbuild/apple_support/releases
+load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive")
+
+http_archive(
+    name = "build_bazel_apple_support",
+    sha256 = "c4bb2b7367c484382300aee75be598b92f847896fb31bbd22f3a2346adf66a80",
+    url = "https://github.com/bazelbuild/apple_support/releases/download/1.15.1/apple_support.1.15.1.tar.gz",
+)
+
+load(
+    "@build_bazel_apple_support//lib:repositories.bzl",
+    "apple_support_dependencies",
+)
+
+apple_support_dependencies()
+
 # dairlib can use either a local version of drake or a pegged revision
 # If the environment variable DAIRLIB_LOCAL_DRAKE_PATH is set, it will use
 # a local version, ad the specified path. Otherwise, it will get a pegged
@@ -22,8 +39,10 @@ DRAKE_CHECKSUM = "6ff298d7fbc33cb17963509f86fcd9cb6816d455b97b3fd589e1085e0548c2
 load("//:environ.bzl", "drake_repository")
 load("//:environ.bzl", "inekf_repository")
 
-drake_repository(name="drake_path")
-inekf_repository(name="inekf_path")
+drake_repository(name = "drake_path")
+
+inekf_repository(name = "inekf_path")
+
 load("@drake_path//:environ.bzl", "DAIRLIB_LOCAL_DRAKE_PATH")
 load("@inekf_path//:environ.bzl", "DAIRLIB_LOCAL_INEKF_PATH")
 

examples/Cassie/BUILD.bazel

@@ -66,7 +66,7 @@ cc_library(
     hdrs = ["cassie_state_estimator_settings.h"],
     deps = [
         "@drake//:drake_shared_library",
-    ]
+    ],
 )
 
 cc_library(
@@ -75,9 +75,9 @@ cc_library(
     hdrs = ["cassie_state_estimator.h"],
     deps = [
         ":cassie_utils",
+        "//examples/Cassie:cassie_state_estimator_settings",
         "//examples/Cassie/datatypes:cassie_names",
         "//examples/Cassie/datatypes:cassie_out_t",
-        "//examples/Cassie:cassie_state_estimator_settings",
         "//multibody:utils",
         "//multibody/kinematic",
         "//systems/framework:vector",
@@ -185,9 +185,9 @@ cc_binary(
         "//examples/Cassie/systems:cassie_encoder",
         "//solvers:optimization_utils",
         "//systems:robot_lcm_systems",
+        "//systems:system_utils",
         "//systems/framework:geared_motor",
         "//systems/primitives",
-        "//systems:system_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -249,9 +249,9 @@ cc_binary(
         "//lcmtypes:lcmt_robot",
         "//multibody:multibody_solvers",
         "//systems:robot_lcm_systems",
+        "//systems:system_utils",
         "//systems/framework:lcm_driven_loop",
         "//systems/primitives",
-        "//systems:system_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -269,9 +269,9 @@ cc_binary(
         "//examples/Cassie/systems:input_supervisor",
         "//lcmtypes:lcmt_robot",
         "//systems:robot_lcm_systems",
+        "//systems:system_utils",
         "//systems/controllers:linear_controller",
         "//systems/controllers:pd_config_lcm",
-        "//systems:system_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -296,8 +296,8 @@ cc_binary(
     deps = [
         "//lcmtypes:lcmt_robot",
         "@drake//lcm",
-        "@lcm",
         "@gflags",
+        "@lcm",
     ],
 )
 
@@ -307,8 +307,8 @@ cc_binary(
     deps = [
         "//lcmtypes:lcmt_robot",
         "@drake//lcm",
-        "@lcm",
         "@gflags",
+        "@lcm",
     ],
 )
 
@@ -350,18 +350,18 @@ cc_binary(
     deps = [
         ":cassie_urdf",
         ":cassie_utils",
-        "//examples/Cassie/osc",
         "//examples/Cassie/contact_scheduler:all",
-        "//examples/Cassie/systems:cassie_out_to_radio",
+        "//examples/Cassie/osc",
         "//examples/Cassie/osc_jump",
         "//examples/Cassie/osc_run",
+        "//examples/Cassie/systems:cassie_out_to_radio",
         "//lcm:trajectory_saver",
         "//multibody:utils",
         "//systems:robot_lcm_systems",
         "//systems:system_utils",
-        "//systems/filters:floating_base_velocity_filter",
-        "//systems/controllers/osc:osc_tracking_datas",
         "//systems/controllers:controllers_all",
+        "//systems/controllers/osc:osc_tracking_datas",
+        "//systems/filters:floating_base_velocity_filter",
         "//systems/framework:lcm_driven_loop",
         "//systems/primitives",
         "@drake//:drake_shared_library",
@@ -372,6 +372,7 @@ cc_binary(
 cc_binary(
     name = "run_osc_walking_controller",
     srcs = ["run_osc_walking_controller.cc"],
+    tags = ["manual"],
     deps = [
         ":cassie_urdf",
         ":cassie_utils",
@@ -382,11 +383,11 @@ cc_binary(
         "//multibody:view_frame",
         "//multibody/kinematic",
         "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/controllers:controllers_all",
         "//systems/controllers:fsm_event_time",
         "//systems/framework:lcm_driven_loop",
         "//systems/primitives",
-        "//systems/controllers:controllers_all",
-        "//systems:system_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -399,17 +400,17 @@ cc_binary(
         ":cassie_urdf",
         ":cassie_utils",
         "//examples/Cassie/osc",
-        "//examples/impact_invariant_control:impact_aware_time_based_fsm",
         "//examples/Cassie/systems:cassie_out_to_radio",
         "//examples/Cassie/systems:simulator_drift",
+        "//examples/impact_invariant_control:impact_aware_time_based_fsm",
         "//multibody:utils",
         "//multibody:view_frame",
         "//multibody/kinematic",
         "//solvers:solver_options_io",
         "//systems:robot_lcm_systems",
         "//systems:system_utils",
-        "//systems/controllers:fsm_event_time",
         "//systems/controllers:controllers_all",
+        "//systems/controllers:fsm_event_time",
         "//systems/filters:floating_base_velocity_filter",
         "//systems/framework:lcm_driven_loop",
         "//systems/primitives",
@@ -429,11 +430,11 @@ cc_binary(
         "//multibody:utils",
         "//multibody/kinematic",
         "//systems:robot_lcm_systems",
+        "//systems:system_utils",
         "//systems/controllers/osc:operational_space_control",
         "//systems/controllers/osc:osc_gains",
         "//systems/framework:lcm_driven_loop",
         "//systems/primitives",
-        "//systems:system_utils",
         "@drake//:drake_shared_library",
         "@gflags",
     ],

examples/Cassie/cassie_fixed_point_solver.cc

@@ -88,14 +88,14 @@ void CassieFixedPointSolver(
   program.AddJointLimitConstraints(q);
 
   // Fix floating base
-  program.AddConstraint(q(positions_map.at("base_qw")) == 1);
-  program.AddConstraint(q(positions_map.at("base_qx")) == 0);
-  program.AddConstraint(q(positions_map.at("base_qy")) == 0);
-  program.AddConstraint(q(positions_map.at("base_qz")) == 0);
+  program.AddConstraint(q(positions_map.at("pelvis_qw")) == 1);
+  program.AddConstraint(q(positions_map.at("pelvis_qx")) == 0);
+  program.AddConstraint(q(positions_map.at("pelvis_qy")) == 0);
+  program.AddConstraint(q(positions_map.at("pelvis_qz")) == 0);
 
-  program.AddConstraint(q(positions_map.at("base_x")) == 0);
-  program.AddConstraint(q(positions_map.at("base_y")) == 0);
-  program.AddConstraint(q(positions_map.at("base_z")) == height);
+  program.AddConstraint(q(positions_map.at("pelvis_x")) == 0);
+  program.AddConstraint(q(positions_map.at("pelvis_y")) == 0);
+  program.AddConstraint(q(positions_map.at("pelvis_z")) == height);
 
   // Add symmetry constraints, and zero roll/pitch on the hip
   program.AddConstraint(q(positions_map.at("knee_left")) ==
@@ -143,7 +143,7 @@ void CassieFixedPointSolver(
   // Set initial guess/cost for q using a vaguely neutral position
   Eigen::VectorXd q_guess = Eigen::VectorXd::Zero(plant.num_positions());
   q_guess(0) = 1;  // quaternion
-  q_guess(positions_map.at("base_z")) = height;
+  q_guess(positions_map.at("pelvis_z")) = height;
   q_guess(positions_map.at("hip_pitch_left")) = 1;
   q_guess(positions_map.at("knee_left")) = -2;
   q_guess(positions_map.at("ankle_joint_left")) = 2;
@@ -334,27 +334,27 @@ void CassieInitStateSolver(
   int n_v = plant.num_velocities();
 
   // Fix floating base
-  program.AddBoundingBoxConstraint(1, 1, q(positions_map.at("base_qw")));
-  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qx")));
-  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qy")));
-  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qz")));
+  program.AddBoundingBoxConstraint(1, 1, q(positions_map.at("pelvis_qw")));
+  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qx")));
+  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qy")));
+  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qz")));
 
-  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_x")));
-  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_y")));
+  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_x")));
+  program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_y")));
   program.AddBoundingBoxConstraint(height, height,
-                                   q(positions_map.at("base_z")));
+                                   q(positions_map.at("pelvis_z")));
 
   program.AddBoundingBoxConstraint(-10, 10, v);
 
-  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wx")));
-  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wy")));
-  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wz")));
+  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wx")));
+  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wy")));
+  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wz")));
 
   program.AddBoundingBoxConstraint(pelvis_xy_vel(0), pelvis_xy_vel(0),
-                                   v(vel_map.at("base_vx")));
+                                   v(vel_map.at("pelvis_vx")));
   program.AddBoundingBoxConstraint(pelvis_xy_vel(1), pelvis_xy_vel(1),
-                                   v(vel_map.at("base_vy")));
-  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_vz")));
+                                   v(vel_map.at("pelvis_vy")));
+  program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_vz")));
 
   // Add symmetry constraints, and zero roll/pitch on the hip
   program.AddConstraint(q(positions_map.at("knee_left")) ==

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/networking/cassie_udp_publisher.cc

@@ -42,8 +42,8 @@ CassieUDPPublisher::CassieUDPPublisher(const std::string& address,
   server_address_.sin_family = AF_INET;  // IPv4
   server_address_.sin_port = htons(port);
 
-  // Declare a forced publish so that any time Publish(.) is called on this
-  // system (or a Diagram containing it), a message is emitted.
+  // Declare a forced publish so that any time ForcedPublish(.) is called on
+  // this system (or a Diagram containing it), a message is emitted.
   if (publish_triggers.find(TriggerType::kForced) != publish_triggers.end()) {
     this->DeclareForcedPublishEvent(
         &CassieUDPPublisher::PublishInputAsUDPMessage);

examples/Cassie/osc/heading_traj_generator.cc

@@ -1,9 +1,7 @@
 #include "examples/Cassie/osc/heading_traj_generator.h"
 
 #include <math.h>
-
-#include <string>
-
+#include "drake/common/trajectories/piecewise_quaternion.h"
 #include "multibody/multibody_utils.h"
 
 using std::string;
@@ -21,6 +19,7 @@ using drake::systems::Context;
 using drake::systems::LeafSystem;
 
 using drake::trajectories::PiecewisePolynomial;
+using drake::trajectories::PiecewiseQuaternionSlerp;
 
 namespace dairlib {
 namespace cassie {
@@ -43,7 +42,7 @@ HeadingTrajGenerator::HeadingTrajGenerator(
       this->DeclareVectorInputPort("pelvis_yaw", BasicVector<double>(1))
           .get_index();
   // Provide an instance to allocate the memory first (for the output)
-  PiecewisePolynomial<double> pp(VectorXd(0));
+  PiecewiseQuaternionSlerp<double> pp;
   drake::trajectories::Trajectory<double>& traj_inst = pp;
   this->DeclareAbstractOutputPort("pelvis_quat", traj_inst,
                                   &HeadingTrajGenerator::CalcHeadingTraj);
@@ -100,19 +99,17 @@ void HeadingTrajGenerator::CalcHeadingTraj(
   Quaterniond relative_quat(cos(des_delta_yaw / 2), 0, 0,
                             sin(des_delta_yaw / 2));
   Quaterniond final_quat = relative_quat * init_quat;
-  Eigen::Vector4d pelvis_rotation_f;
-  pelvis_rotation_f << final_quat.w(), final_quat.vec();
 
   const std::vector<double> breaks = {context.get_time(),
                                       context.get_time() + dt};
-  std::vector<MatrixXd> knots(breaks.size(), MatrixXd::Zero(4, 1));
-  knots[0] = pelvis_rotation_i;
-  knots[1] = pelvis_rotation_f;
-  const auto pp = PiecewisePolynomial<double>::FirstOrderHold(breaks, knots);
+  std::vector<Quaterniond> knots(breaks.size(), Quaterniond());
+  knots[0] = init_quat;
+  knots[1] = final_quat;
+  const auto pp = PiecewiseQuaternionSlerp(breaks, knots);
 
   // Assign traj
   auto* pp_traj =
-      (PiecewisePolynomial<double>*)dynamic_cast<PiecewisePolynomial<double>*>(
+      (PiecewiseQuaternionSlerp<double>*)dynamic_cast<PiecewiseQuaternionSlerp<double>*>(
           traj);
   *pp_traj = pp;
 }

examples/Cassie/osc/standing_pelvis_orientation_traj.cc

@@ -3,12 +3,14 @@
 #include <dairlib/lcmt_cassie_out.hpp>
 
 #include "drake/common/trajectories/piecewise_polynomial.h"
+#include "drake/common/trajectories/piecewise_quaternion.h"
 #include "drake/common/trajectories/trajectory.h"
 #include "drake/math/wrap_to.h"
 
 using dairlib::systems::OutputVector;
 using drake::systems::BasicVector;
 using drake::trajectories::PiecewisePolynomial;
+using drake::trajectories::PiecewiseQuaternionSlerp;
 using drake::trajectories::Trajectory;
 using Eigen::MatrixXd;
 using Eigen::Vector3d;
@@ -39,7 +41,7 @@ StandingPelvisOrientationTraj::StandingPelvisOrientationTraj(
       this->DeclareAbstractInputPort("radio_out",
                                      drake::Value<dairlib::lcmt_radio_out>{})
           .get_index();
-  PiecewisePolynomial<double> empty_pp_traj(Eigen::VectorXd(0));
+  PiecewiseQuaternionSlerp<double> empty_pp_traj;
   Trajectory<double>& traj_inst = empty_pp_traj;
   this->set_name(traj_name);
   this->DeclareAbstractOutputPort(traj_name, traj_inst,
@@ -57,7 +59,7 @@ void StandingPelvisOrientationTraj::CalcTraj(
   VectorXd q = robot_output->GetPositions();
   plant_.SetPositions(context_, q);
   auto* casted_traj =
-      (PiecewisePolynomial<double>*)dynamic_cast<PiecewisePolynomial<double>*>(
+      (PiecewiseQuaternionSlerp<double>*)dynamic_cast<PiecewiseQuaternionSlerp<double>*>(
           traj);
   Vector3d pt_0;
   Vector3d pt_1;
@@ -83,11 +85,12 @@ void StandingPelvisOrientationTraj::CalcTraj(
           0.5 * (atan2(l_foot(1), l_foot(0)) + atan2(r_foot(1), r_foot(0))),
           -M_PI, M_PI) +
           radio_out->channel[3];
-
+  const std::vector<double> breaks = {context.get_time(),
+                                      context.get_time() + 1.0};
   auto rot_mat =
       drake::math::RotationMatrix<double>(drake::math::RollPitchYaw(rpy));
 
-  *casted_traj = PiecewisePolynomial<double>(rot_mat.ToQuaternionAsVector4());
+  *casted_traj = PiecewiseQuaternionSlerp<double>(breaks, {rot_mat.ToQuaternion(), rot_mat.ToQuaternion()});
 }
 
 }  // namespace dairlib::cassie::osc