Pre-publication code review

README.md

@@ -54,26 +54,8 @@ These dependencies are necessary for some advanced visualization and process man
 #### LCM and libbot
 Install a local copy of `lcm` and `libbot2` using `sudo apt install lcm libbot2`. The prerequisites installation (option 1.a) should add the proper apt repo for these.
 
-#### ROS
-To integrate with ROS (tested on ROS Noetic with 20.04), the following steps are required.
-1. Install ROS http://wiki.ros.org/ROS/Installation
-2. Do not forget to setup your environment. For instance, add these lines to `~/.bashrc`
-```
-export ROS_MASTER_URI=http://localhost:11311
-source /opt/ros/noetic/setup.bash 
-```
-3. Install additional dependencies
-```
-sudo apt install python3-rosinstall-generator python-catkin-tools python3-vcstool
-```
-4. Build the ROS workspace using catkin. From `dairlib/`,
-```
-sudo ./tools/workspace/ros/compile_ros_workspace.sh
-```
-5. Set the environment variable `DAIRLIB_WITH_ROS` to `ON`. For instance, add to `~/.bashrc`
-```
-export DAIRLIB_WITH_ROS=ON
-```
+#### Install procman 
+Install procman from https://github.com/ashuang/procman
 
 #### Invariant-EKF
 State Estimation for Cassie is done using contact-aided invariant-EKF. `invariant-ekf` is an external repository forked from Ross Hartley's repository of the same name. By default, a pegged version of this forked repository is used i.e. the `bazel` branch of DAIR lab's fork of `invariant-ekf` is automatically downloaded and used. However, to make changes to the files, the [DAIR Lab's fork of invariant-ekf](https://github.com/DAIRLab/invariant-ekf/tree/bazel "DAIR Lab's fork of invariant-ekf") can be cloned as a local repository.
@@ -96,14 +78,3 @@ Build what you want via Bazel. From `dairlib`,  `bazel build ...` will build the
 ## Included Modules
 A list of included modules
 
-### DIRCON
-A modern Drake implementation of the DIRCON constrained trajectory optimization algorithm. Currently under construction. See `/examples/PlanarWalker/run_gait_dircon.cc` for a simple example of the hybrid DIRCON algorithm. The more complete example set (from the paper) currently exists on an older version of Drake https://github.com/mposa/drake/tree/hybrid-merge
-
-Based off the publication
-
-Michael Posa, Scott Kuindersma, Russ Tedrake. "Optimization and Stabilization of Trajectories for Constrained Dynamical Systems." Proceedings of the International Conference on Robotics and Automation (ICRA), 2016. 
-
-Available online at https://posa.seas.upenn.edu/wp-content/uploads/Posa16a.pdf
-
-## Docker (experimental)
-Docker support is currently experimental. See `install/bionic/Dockerfile` for an Ubuntu Dockerfile. Docker is being used in conjuction with Cirrus Continuous Integration, and should be better supported in the future.

WORKSPACE

@@ -11,9 +11,9 @@ workspace(name = "dairlib")
 #  export DAIRLIB_LOCAL_DRAKE_PATH=/home/user/workspace/drake
 
 # Choose a revision of Drake to use.
-DRAKE_COMMIT = "v1.22.0"
+DRAKE_COMMIT = "v1.33.0"
 
-DRAKE_CHECKSUM = "78cf62c177c41f8415ade172c1e6eb270db619f07c4b043d5148e1f35be8da09"
+DRAKE_CHECKSUM = "2713ad1ea53ed7b9cfe1ba751eeeab7dc173408753ead21b7bafb0436b92dcf3"
 # Before changing the COMMIT, temporarily uncomment the next line so that Bazel
 # displays the suggested new value for the CHECKSUM.
 #DRAKE_CHECKSUM = "0" * 64

bindings/pydairlib/solvers/c3_py.cc

@@ -73,11 +73,11 @@ PYBIND11_MODULE(c3, m) {
       .def(py::init<const LCS&, const dairlib::solvers::C3::CostMatrices&,
                     const vector<VectorXd>&, const C3Options&>(),
            arg("LCS"), arg("costs"), arg("x_des"), arg("c3_options"))
-      .def("Solve", &C3MIQP::Solve, arg("x0"))
+      .def("Solve", &C3MIQP::Solve, arg("x0"), arg("verbose") = false)
       .def("UpdateTarget", &C3MIQP::UpdateTarget, arg("x0"))
       .def("UpdateLCS", &C3MIQP::UpdateLCS, arg("lcs"))
       .def("ADMMStep", &C3MIQP::ADMMStep, arg("x0"), arg("delta"), arg("w"),
-           arg("G"), arg("admm_iteration"))
+           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"))

examples/sampling_c3/BUILD.bazel

@@ -0,0 +1,271 @@
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "urdfs",
+    data = glob([
+        "urdf/**",
+    ]),
+)
+
+filegroup(
+    name = "all_example_params_yamls",
+    srcs = glob(["*/parameters/*.yaml"]),
+    visibility = ["//visibility:public"],
+)
+
+filegroup(
+    name = "shared_params_yamls",
+    srcs = glob(["shared_parameters/*.yaml"]),
+    visibility = ["//visibility:public"],
+)
+
+cc_binary(
+    name = "franka_sim",
+    srcs = ["franka_sim.cc"],
+    data = [
+        ":urdfs",
+        "//examples/sampling_c3:all_example_params_yamls",
+        "@drake_models//:franka_description",
+    ],
+    deps = [
+        "//examples/sampling_c3/parameter_headers:franka_lcm_channels",
+        "//examples/sampling_c3/parameter_headers:franka_sim_params",
+        "//common",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/framework:lcm_driven_loop",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+cc_binary(
+    name = "franka_osc_controller",
+    srcs = ["franka_osc_controller.cc"],
+    data = [
+        ":urdfs",
+        "//examples/sampling_c3:all_example_params_yamls",
+        "@drake_models//:franka_description",
+    ],
+    deps = [
+        "//examples/sampling_c3/parameter_headers:franka_lcm_channels",
+        "//examples/sampling_c3/parameter_headers:franka_osc_controller_params",
+        "//common",
+        "//systems:franka_systems",
+        "//lcm:lcm_trajectory_saver",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/controllers:gravity_compensator",
+        "//systems/controllers/osc:operational_space_control",
+        "//systems/framework:lcm_driven_loop",
+        "//systems/trajectory_optimization:lcm_trajectory_systems",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+cc_binary(
+    name = "franka_joint_osc_controller",
+    srcs = ["franka_joint_osc_controller.cc"],
+    data = [
+        ":urdfs",
+        "//examples/sampling_c3:all_example_params_yamls",
+        "@drake_models//:franka_description",
+    ],
+    deps = [
+        "//examples/sampling_c3/parameter_headers:franka_lcm_channels",
+        "//examples/sampling_c3/parameter_headers:franka_osc_controller_params",
+        "//common",
+        "//systems:franka_systems",
+        ":joint_trajectory_generator",
+        "//lcm:lcm_trajectory_saver",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/controllers:gravity_compensator",
+        "//systems/controllers/osc:operational_space_control",
+        "//systems/framework:lcm_driven_loop",
+        "//systems/primitives:radio_parser",
+        "//systems/trajectory_optimization:lcm_trajectory_systems",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+
+
+cc_library(
+    name = "joint_trajectory_generator",
+    srcs = ["joint_trajectory_generator.cc"],
+    hdrs = ["joint_trajectory_generator.h"],
+    deps = [
+        "//systems/framework:vector",
+    ],
+)
+cc_binary(
+    name = "franka_visualizer",
+    srcs = ["franka_visualizer.cc"],
+    data = [
+        ":urdfs",
+        "//examples/sampling_c3:all_example_params_yamls",
+        "@drake_models//:franka_description",
+    ],
+    deps = [
+        ":c3_mode_visualizer",
+        "//solvers:c3",
+        "//examples/sampling_c3/parameter_headers:franka_lcm_channels",
+        "//examples/sampling_c3/parameter_headers:franka_sim_params",
+        "//examples/sampling_c3/parameter_headers:franka_c3_controller_params",
+        "//systems/controllers:sampling_params",
+        "//common",
+        "//multibody:utils",
+        "//multibody:visualization_utils",
+        "//systems/sender_systems:sender_systems",
+        "//systems:franka_systems",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/primitives",
+        "//systems/trajectory_optimization:lcm_trajectory_systems",
+        "//systems/visualization:lcm_visualization_systems",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+cc_binary(
+    name = "franka_bridge_driver_out",
+    srcs = ["franka_bridge_driver_out.cc"],
+    data = [
+        ":urdfs",
+        "@drake_models//:franka_description",
+        "//examples/sampling_c3:all_example_params_yamls",
+        "//examples/sampling_c3:shared_params_yamls",
+    ],
+    deps = [
+        "//examples/sampling_c3/parameter_headers:franka_lcm_channels",
+        "//examples/sampling_c3/parameter_headers:franka_drake_lcm_driver_channels",
+        "//examples/sampling_c3/parameter_headers:franka_sim_params",
+        "//common",
+        ":franka_state_translator",
+        "//multibody:utils",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/framework:lcm_driven_loop",
+        "//systems/primitives",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+cc_binary(
+    name = "franka_bridge_driver_in",
+    srcs = ["franka_bridge_driver_in.cc"],
+    data = [
+        ":urdfs",
+        "@drake_models//:franka_description",
+        "//examples/sampling_c3:all_example_params_yamls",
+        "//examples/sampling_c3:shared_params_yamls",
+    ],
+    deps = [
+        "//examples/sampling_c3/parameter_headers:franka_lcm_channels",
+        "//examples/sampling_c3/parameter_headers:franka_drake_lcm_driver_channels",
+        "//examples/sampling_c3/parameter_headers:franka_sim_params",
+        "//common",
+        ":franka_state_translator",
+        "//multibody:utils",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/framework:lcm_driven_loop",
+        "//systems/primitives",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+cc_binary(
+    name = "franka_c3_controller",
+    srcs = ["franka_c3_controller.cc"],
+    data = [
+        ":urdfs",
+        "@drake_models//:franka_description",
+        "//examples/sampling_c3:all_example_params_yamls",
+        "//examples/sampling_c3:shared_params_yamls",
+    ],
+    deps = [
+        "//examples/sampling_c3/parameter_headers:franka_lcm_channels",
+        "//examples/sampling_c3/parameter_headers:franka_sim_params",
+        "//examples/sampling_c3/parameter_headers:trajectory_params",
+        "//systems/controllers:sampling_params",
+        "//common",
+        "//systems:franka_systems",
+        "//systems/sender_systems:sample_buffer_sender",
+        "//systems/sender_systems:sender_systems",
+        ":control_target_generator",
+        "//lcm:lcm_trajectory_saver",
+        "//systems:robot_lcm_systems",
+        "//systems:system_utils",
+        "//systems/controllers:sampling_c3_controller",
+        "//systems/framework:lcm_driven_loop",
+        "//systems/trajectory_optimization:c3_output_systems",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+
+cc_library(
+    name = "c3_mode_visualizer",
+    srcs = ["c3_mode_visualizer.cc"],
+    hdrs = ["c3_mode_visualizer.h"],
+    deps = [
+        "//lcmtypes:lcmt_robot",
+        "//lcm:lcm_trajectory_saver",
+        "//systems/framework:vector",
+        "@drake//:drake_shared_library",
+        "@lcm",
+    ],
+)
+
+cc_library(
+    name = "franka_state_translator",
+    srcs = [
+        "franka_state_translator.cc",
+    ],
+    hdrs = [
+        "franka_state_translator.h",
+    ],
+    deps = [
+        "//lcmtypes:lcmt_robot",
+        "//systems/framework:vector",
+        "@drake//:drake_shared_library",
+    ],
+)
+
+
+cc_library(
+    name = "control_target_generator",
+    srcs = ["control_target_generator.cc"],
+    hdrs = ["control_target_generator.h"],
+    deps = [
+        "//lcmtypes:lcmt_robot",
+        "//multibody:utils",
+        "//systems/framework:vector",
+        "@drake//:drake_shared_library",
+        "//solvers:c3",
+        "//examples/sampling_c3/parameter_headers:franka_c3_controller_params",
+        "//systems/controllers:sampling_params",
+        "//lcm:lcm_trajectory_saver",
+        "@lcm",
+    ],
+)
+
+py_binary(
+    name = "xbox_script",
+    srcs = ["xbox_script.py"],
+    data = [
+        "@lcm//:lcm-python",
+    ],
+    main = "xbox_script.py",
+    deps = [
+        "//lcmtypes:lcmtypes_robot_py",
+    ],
+)