Merge with master branch

Author: yminchen

.cirrus.yml

@@ -16,6 +16,24 @@ build_task:
       --local_ram_resources=24000
       --local_cpu_resources=8
       //...
+build_focal_task:
+  timeout_in: 120m
+  container:
+    dockerfile: install/focal/Dockerfile
+    cpu: 8
+    memory: 24
+  test_script:
+    - bazel build
+      --define=WITH_SNOPT=OFF
+      --local_ram_resources=24000
+      --local_cpu_resources=8
+      --jobs=8 
+      //...
+    - bazel test
+      --define=WITH_SNOPT=OFF
+      --local_ram_resources=24000
+      --local_cpu_resources=8
+      //...
 build_with_ros_task:
   timeout_in: 120m
   container:

README.md

@@ -2,7 +2,9 @@
 Warning! This is very much "development-level" code and is provided as-is. APIs are likely to be unstable and, while we hope for the documentation to be thorough and accurate, we make no guarantees.
 
 ## Current Continuous Integration Status
-* `master` branch build and unit tests: [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib)
+* `master` branch build and unit tests (Ubuntu 18.04): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib)
+* `master` branch build and unit tests (Ubuntu 20.04): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build_focal&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib)
+* `master` branch build and unit tests (Ubuntu 18.04 with ROS): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build_with_ros&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib)
 * Experimental build against Drake's `master` branch: [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=drake_master_build&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib)
 ## Complete Build Instructions
 

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 = "1cab4e23d5d41690a3c43a0af80cb62f7eaacf3f"
+DRAKE_COMMIT = "2f16f87df02d7c6ef436fb87437e59a3df840bb5"
 
-DRAKE_CHECKSUM = "da63499c336e690afa41db030c7586e8c2c2ae53ca0a81acf8f94d162a6a2b0e"
+DRAKE_CHECKSUM = "fb65e3f2949a7d50075afa658ec0043b6c8a32f8f692628032c53a18176eff76"
 # Before changing the COMMIT, temporarily uncomment the next line so that Bazel
 # displays the suggested new value for the CHECKSUM.
 # DRAKE_CHECKSUM = "0" * 64

examples/Cassie/BUILD.bazel

@@ -27,13 +27,13 @@ cc_library(
     srcs = ["cassie_utils.cc"],
     hdrs = ["cassie_utils.h"],
     deps = [
+        ":cassie_encoder",
         ":cassie_urdf",
         ":sim_cassie_sensor_aggregator",
         "//common",
         "//multibody:utils",
         "//multibody/kinematic",
         "//systems:robot_lcm_systems",
-        ":cassie_encoder",
         "@drake//:drake_shared_library",
     ],
 )
@@ -226,6 +226,7 @@ cc_binary(
     name = "multibody_sim",
     srcs = ["multibody_sim.cc"],
     deps = [
+        ":cassie_encoder",
         ":cassie_fixed_point_solver",
         ":cassie_urdf",
         ":cassie_utils",
@@ -234,7 +235,6 @@ cc_binary(
         "//systems:robot_lcm_systems",
         "//systems/framework:vector",
         "//systems/primitives",
-        ":cassie_encoder",
         "@drake//:drake_shared_library",
         "@gflags",
     ],
@@ -256,6 +256,23 @@ cc_binary(
     ],
 )
 
+cc_binary(
+    name = "multibody_sim_w_ground_incline",
+    srcs = ["multibody_sim_w_ground_incline.cc"],
+    deps = [
+        ":cassie_encoder",
+        ":cassie_fixed_point_solver",
+        ":cassie_urdf",
+        ":cassie_utils",
+        "//solvers:optimization_utils",
+        "//systems:robot_lcm_systems",
+        "//systems/framework:vector",
+        "//systems/primitives",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
 cc_binary(
     name = "parse_log_test",
     srcs = ["test/parse_log_test.cc"],

examples/Cassie/cassie_fixed_point_solver.cc

@@ -31,13 +31,11 @@ void CassieFixedPointSolver(
     VectorXd* lambda_result, std::string visualize_model_urdf,
     double ground_incline, VectorXd* all_sol) {
   // Get the rotational matrix
-  Eigen::AngleAxisd rollAngle(ground_incline, Eigen::Vector3d::UnitZ());
-  Eigen::AngleAxisd yawAngle(0, Eigen::Vector3d::UnitY());
-  Eigen::AngleAxisd pitchAngle(0, Eigen::Vector3d::UnitX());
-  Eigen::Quaternion<double> quat = rollAngle * yawAngle * pitchAngle;
-  Eigen::Matrix3d rotationMatrix = quat.matrix();
-  // Get normal direction
-  Eigen::Vector3d ground_normal(sin(ground_incline), 0, cos(ground_incline));
+  Eigen::AngleAxisd roll_angle(ground_incline, Eigen::Vector3d::UnitZ());
+  Eigen::AngleAxisd yaw_angle(0, Eigen::Vector3d::UnitY());
+  Eigen::AngleAxisd pitch_angle(0, Eigen::Vector3d::UnitX());
+  Eigen::Quaternion<double> quat = roll_angle * yaw_angle * pitch_angle;
+  Eigen::Matrix3d rotation_mat = quat.matrix();
 
   multibody::KinematicEvaluatorSet<double> evaluators(plant);
 
@@ -48,28 +46,30 @@ void CassieFixedPointSolver(
   evaluators.add_evaluator(&right_loop);
 
   // Add contact points
+  std::vector<int> yz_active_idx = {1, 2};
+  std::vector<int> z_active_idx = {2};
   auto left_toe = LeftToeFront(plant);
-  auto left_toe_evaluator = multibody::WorldPointEvaluator(plant,
-      left_toe.first, left_toe.second, rotationMatrix,
-      Eigen::Vector3d(0, toe_spread, 0), {1, 2});
+  auto left_toe_evaluator = multibody::WorldPointEvaluator(
+      plant, left_toe.first, left_toe.second, rotation_mat,
+      Eigen::Vector3d(0, toe_spread, 0), yz_active_idx);
   evaluators.add_evaluator(&left_toe_evaluator);
 
   auto left_heel = LeftToeRear(plant);
-  auto left_heel_evaluator = multibody::WorldPointEvaluator(plant,
-      left_heel.first, left_heel.second, ground_normal,
-      Eigen::Vector3d::Zero(), false);
+  auto left_heel_evaluator = multibody::WorldPointEvaluator(
+      plant, left_heel.first, left_heel.second, rotation_mat,
+      Eigen::Vector3d::Zero(), z_active_idx);
   evaluators.add_evaluator(&left_heel_evaluator);
 
   auto right_toe = RightToeFront(plant);
-  auto right_toe_evaluator = multibody::WorldPointEvaluator(plant,
-      right_toe.first, right_toe.second, rotationMatrix,
-      Eigen::Vector3d(0, -toe_spread, 0), {1, 2});
+  auto right_toe_evaluator = multibody::WorldPointEvaluator(
+      plant, right_toe.first, right_toe.second, rotation_mat,
+      Eigen::Vector3d(0, -toe_spread, 0), yz_active_idx);
   evaluators.add_evaluator(&right_toe_evaluator);
 
   auto right_heel = RightToeRear(plant);
-  auto right_heel_evaluator = multibody::WorldPointEvaluator(plant,
-      right_heel.first, right_heel.second, ground_normal,
-      Eigen::Vector3d::Zero(), false);
+  auto right_heel_evaluator = multibody::WorldPointEvaluator(
+      plant, right_heel.first, right_heel.second, rotation_mat,
+      Eigen::Vector3d::Zero(), z_active_idx);
   evaluators.add_evaluator(&right_heel_evaluator);
 
   auto program = multibody::MultibodyProgram(plant);

examples/Cassie/cassie_state_estimator.cc

@@ -6,7 +6,6 @@
 #include <fstream>
 #include <utility>
 
-#include "drake/math/orthonormal_basis.h"
 #include "drake/solvers/equality_constrained_qp_solver.h"
 #include "drake/solvers/mathematical_program.h"
 #include "drake/solvers/solve.h"

examples/Cassie/director_scripts/pd_panel.py

@@ -38,20 +38,7 @@
     "toe_left",
     "toe_right"]
 
-# joint_default = [0,0,0,0,0,0,0,0,0,0]
-# kp_default = [20,20,20,20,20,20,20,20,2,2]
-# kd_default = [5,5,5,5,5,5,5,5,1,1]
-
-# joint_default = [0,0,0,0,0,0,0,0,0,0]
-# kp_default = [0,0,0,0,0,0,0,0,0,0]
-# kd_default = [0,0,0,0,0,0,0,0,0,0]
-
-# Set of gains with which Cassie can stand:
-# joint_default = [0.2,-.2,0,0,0.35,0.35,-0.9,-0.9,-1.8,-1.8]
-# kp_default = [i for i in [20,20,10,10,20,20,100,100,10,10]]
-# kd_default = [i for i in [1,1,1,1,1,1,2,2,1,1]]
-
-# Set of gains with which COM is within support polygon when we lower the hoist 
+# Set of gains with which COM is within support polygon when we lower the hoist
 joint_default = [-0.01,.01,0,0,0.55,0.55,-1.5,-1.5,-1.8,-1.8]
 kp_default = [i for i in [80,80,50,50,50,50,50,50,10,10]]
 kd_default = [i for i in [1,1,1,1,1,1,2,2,1,1]]
@@ -207,8 +194,6 @@ def publish_clicked(self):
         for i in range(100):
             # ramp up the gains for 5 seconds
             msg.timestamp = int(time.time() * 1e6)
-            # msg.kp = list(self.kp_ + i / 99.0 * np.array(self.values[len(joint_names):2*len(joint_names)] - self.kp_))
-            # msg.kd = list(self.kd_ + i / 99.0 * np.array(self.values[2*len(joint_names):3*len(joint_names)] - self.kd_))
             msg.kp = [b + i / 99.0 * (a - b) for a, b in zip(self.values[len(joint_names):2*len(joint_names)], self.kp_)]
             msg.kd = [b + i / 99.0 * (a - b) for a, b in zip(self.values[2*len(joint_names):3*len(joint_names)], self.kd_)]
 

examples/Cassie/dispatcher_robot_out.cc

@@ -45,6 +45,7 @@ DEFINE_bool(broadcast_robot_state, false, "broadcast to planner thread");
 DEFINE_string(address, "127.0.0.1", "IPv4 address to receive on.");
 DEFINE_int64(port, 25001, "Port to receive on.");
 DEFINE_double(pub_rate, 0.1, "Network LCM pubishing period (s).");
+DEFINE_double(fast_network_pub_rate, 0.01, "Network LCM pubishing period (s).");
 DEFINE_bool(simulation, false,
             "Simulated or real robot (default=false, real robot)");
 DEFINE_bool(test_with_ground_truth_state, false,
@@ -214,7 +215,6 @@ int do_main(int argc, char* argv[]) {
   // connect cassie_out publisher
   builder.Connect(*output_sender, *output_pub);
 
-
   // Connect appropriate input receiver for simulation
   systems::CassieOutputReceiver* input_receiver = nullptr;
   if (FLAGS_simulation) {
@@ -243,19 +243,20 @@ int do_main(int argc, char* argv[]) {
   auto robot_output_sender =
       builder.AddSystem<systems::RobotOutputSender>(plant, true, true);
 
-  if(FLAGS_floating_base){
-  // Create and connect contact estimation publisher.
-  auto contact_pub =
-      builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_contact>(
-          "CASSIE_CONTACT_DISPATCHER", &lcm_local, {TriggerType::kForced}));
-  builder.Connect(state_estimator->get_contact_output_port(),
-                  contact_pub->get_input_port());
-  //TODO(yangwill): Consider filtering contact estimation
-  auto gm_contact_pub =
-      builder.AddSystem(LcmPublisherSystem::Make<drake::lcmt_contact_results_for_viz>(
-          "CASSIE_GM_CONTACT_DISPATCHER", &lcm_local, {TriggerType::kForced}));
-  builder.Connect(state_estimator->get_gm_contact_output_port(),
-                  gm_contact_pub->get_input_port());
+  if (FLAGS_floating_base) {
+    // Create and connect contact estimation publisher.
+    auto contact_pub =
+        builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_contact>(
+            "CASSIE_CONTACT_DISPATCHER", &lcm_local, {TriggerType::kForced}));
+    builder.Connect(state_estimator->get_contact_output_port(),
+                    contact_pub->get_input_port());
+    // TODO(yangwill): Consider filtering contact estimation
+    auto gm_contact_pub = builder.AddSystem(
+        LcmPublisherSystem::Make<drake::lcmt_contact_results_for_viz>(
+            "CASSIE_GM_CONTACT_DISPATCHER", &lcm_local,
+            {TriggerType::kForced}));
+    builder.Connect(state_estimator->get_gm_contact_output_port(),
+                    gm_contact_pub->get_input_port());
   }
 
   // Pass through to drop all but positions and velocities
@@ -306,7 +307,7 @@ int do_main(int argc, char* argv[]) {
     auto net_state_pub =
         builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_robot_output>(
             "NETWORK_CASSIE_STATE_DISPATCHER", &lcm_network,
-            {TriggerType::kPeriodic}, 0.01));
+            {TriggerType::kPeriodic}, FLAGS_fast_network_pub_rate));
     builder.Connect(*robot_output_sender, *net_state_pub);
 
     // Option 3 -- find a way to only publish to network after receiving message