Generalized reachability to better work for non-quadrupedal robots

art_planner/include/art_planner/params.h

@@ -1,8 +1,9 @@
 #pragma once
 
-#include <string>
 #include <cmath>
 #include <memory>
+#include <string>
+#include <vector>
 
 
 
@@ -76,6 +77,7 @@ struct Params {
   } objectives;
 
   struct {
+    bool           align_torso_with_terrain{true};
     double         max_pitch_pert{10.0 / 180*M_PI};
     double         max_roll_pert{3.33 / 180*M_PI};
     bool           sample_from_distribution{true};
@@ -102,6 +104,9 @@ struct Params {
 
     struct {
 
+      std::vector<std::string> plane_symmetries{"sagittal",
+                                                "coronal"};
+
       struct {
         double         x{0.362};
         double         y{0.225};

art_planner/include/art_planner/validity_checker/validity_checker_feet.h

@@ -29,6 +29,7 @@ class ValidityCheckerFeet {
 
   float box_length_;
   float box_width_;
+  std::vector<Pose3> box_centers_;
 
   bool boxIsValidAtPose(const Pose3& pose) const;
 

art_planner/src/sampler.cpp

@@ -98,7 +98,12 @@ void SE3FromSE2Sampler::sampleUniform(ob::State* state) {
   state_pos_.get()->values[2] = map_->getHeightAtIndex(ind);
 
   // Apply small random perturbation in normal direction.
-  const Eigen::Vector3d normal_w = map_->getNormal(ind);
+  Eigen::Vector3d normal_w;
+  if (params_->sampler.align_torso_with_terrain) {
+    normal_w = map_->getNormal(ind);
+  } else {
+    normal_w = Eigen::Vector3d::UnitZ();
+  }
 
   const auto std = map_->getPlaneFitStdDev(ind);
 

art_planner/src/validity_checker/validity_checker_feet.cpp

@@ -14,6 +14,31 @@ ValidityCheckerFeet::ValidityCheckerFeet(const ParamsConstPtr& params) : params_
   box_length_ = params_->robot.feet.reach.x;
   box_width_ = params_->robot.feet.reach.y;
 
+  // Add main box, specified in robot/feet/offset.
+  box_centers_.push_back(Pose3FromXYZ(params_->robot.feet.offset.x,
+                                      params_->robot.feet.offset.y,
+                                      0.0f));
+
+  // Apply plane symmetries.
+  for (const auto& plane: params_->robot.feet.plane_symmetries) {
+    size_t n_boxes = box_centers_.size();
+    if (plane == "sagittal") {
+      // Need to iterate like this because we alter vector size during loop.
+      for (size_t i = 0; i < n_boxes; ++i) {
+        box_centers_.push_back(box_centers_[i]);
+        box_centers_.back().translation().y() *= -1;
+      }
+    } else if (plane == "coronal") {
+      // Need to iterate like this because we alter vector size during loop.
+      for (size_t i = 0; i < n_boxes; ++i) {
+        box_centers_.push_back(box_centers_[i]);
+        box_centers_.back().translation().x() *= -1;
+      }
+    } else {
+      std::cout << "Unknown plane symmetry requested: " << plane << std::endl;
+    }
+  }
+
   checker_.reset(new HeightMapBoxChecker(box_length_, box_width_, params_->robot.feet.reach.z));
 }
 
@@ -61,11 +86,11 @@ bool ValidityCheckerFeet::boxesAreValidAtPoses(const std::vector<Pose3> &poses)
 
 
 bool ValidityCheckerFeet::isValid(const Pose3& pose) const {
-  std::vector<Pose3> foot_poses({
-                     pose*Pose3FromXYZ( params_->robot.feet.offset.x,  params_->robot.feet.offset.y, 0.0f),
-                     pose*Pose3FromXYZ( params_->robot.feet.offset.x, -params_->robot.feet.offset.y, 0.0f),
-                     pose*Pose3FromXYZ(-params_->robot.feet.offset.x,  params_->robot.feet.offset.y, 0.0f),
-                     pose*Pose3FromXYZ(-params_->robot.feet.offset.x, -params_->robot.feet.offset.y, 0.0f)});
+  std::vector<Pose3> foot_poses;
+  foot_poses.reserve(box_centers_.size());
+  for (size_t i = 0; i < box_centers_.size(); ++i) {
+    foot_poses.push_back(pose * box_centers_[i]);
+  }
   return boxesAreValidAtPoses(foot_poses);
 }
 

art_planner_ros/config/params.yaml

@@ -42,7 +42,8 @@ objectives:                                         # Not active with prm_motion
         max_lat_vel: 0.1                            # [m/s]   Maximal lateral velocitiy.
         max_ang_vel: 0.5                            # [rad/s] Maximal angular velocity.
 
-sampler:        
+sampler:
+    align_torso_with_terrain: false                 # Align sampled pose with terrain normal.
     max_pitch_pert: 10                              # [degree]  Maximum pitch pertubation when sampling.
     max_roll_pert: 3.33                             # [degree]  Maximum roll pertubation when sampling.
     sample_from_distribution: true                  # Whether to use our custom sampling distribution or sample uniformly in the map.
@@ -61,11 +62,12 @@ robot:
             y: 0.0
             z: 0.04
     feet:
+        plane_symmetries: ["sagittal","coronal"]    # Body planes on which to mirror reachability box. Options: 'sagittal' - left/right, 'coronal' - front/back.
         offset:                                     # [m] Reachability box offset relative to base_frame.
-            x: 0.51                                # Sign of value does not matter.
-            y: 0.2                                # Sign of value does not matter.
+            x: 0.51
+            y: 0.2
             z: -0.475
-        reach:                                      # [m] Reachability of foot box size.
+        reach:                                      # [m] Reachability of foot (box size).
             x: 0.2
             y: 0.2
             z: 0.2

art_planner_ros/include/art_planner_ros/utils.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <string>
+#include <vector>
 
 #include <art_planner/params.h>
 #include <ros/node_handle.h>
@@ -25,13 +26,57 @@ inline T getParamWithDefaultWarning(const ros::NodeHandle& nh,
   return param;
 }
 
+// Specialize for unsigned int.
 template <>
-inline unsigned int getParamWithDefaultWarning<unsigned int>(const ros::NodeHandle& nh,
-                                                      const std::string& name,
-                                                      const unsigned int& default_val) {
+inline unsigned int getParamWithDefaultWarning<unsigned int>(
+                      const ros::NodeHandle& nh,
+                      const std::string& name,
+                      const unsigned int& default_val) {
   return getParamWithDefaultWarning(nh, name, static_cast<int>(default_val));
 }
 
+// Overload for std::vector.
+template <typename T>
+inline std::vector<T> getParamWithDefaultWarning(
+                        const ros::NodeHandle& nh,
+                        const std::string& name,
+                        const std::vector<T>& default_val) {
+  std::vector<T> param;
+
+  if (!nh.param(name, param, default_val)) {
+    std::string default_string;
+    for (size_t i = 0; i < default_val.size(); ++i) {
+      if (i > 0) default_string += std::string(", ");
+      default_string += std::to_string(default_val[i]);
+    }
+    ROS_WARN_STREAM("Could not find ROS param \"" << name <<
+                    "\", set to default: [" << default_string << "]");
+  }
+
+  return param;
+}
+
+// Specialize for std::vector of std::string.
+template <>
+inline std::vector<std::string> getParamWithDefaultWarning(
+                        const ros::NodeHandle& nh,
+                        const std::string& name,
+                        const std::vector<std::string>& default_val) {
+  std::vector<std::string> param;
+
+  if (!nh.param(name, param, default_val)) {
+    std::string default_string;
+    for (size_t i = 0; i < default_val.size(); ++i) {
+      if (i > 0) default_string += std::string(", ");
+      default_string += default_val[i];
+    }
+    ROS_WARN_STREAM("Could not find ROS param \"" << name <<
+                    "\", set to default: [" << default_string << "]");
+  }
+
+  return param;
+}
+
 
 
 ParamsPtr loadRosParameters(const ros::NodeHandle& nh);

art_planner_ros/src/utils.cpp

@@ -178,6 +178,10 @@ ParamsPtr art_planner::loadRosParameters(const ros::NodeHandle& nh) {
 
   // Sampler.
 
+  params->sampler.align_torso_with_terrain =
+      getParamWithDefaultWarning(nh,
+                                 "sampler/align_torso_with_terrain",
+                                 params->sampler.align_torso_with_terrain);
   params->sampler.max_pitch_pert =
       getParamWithDefaultWarning(nh,
                                  "sampler/max_pitch_pert",
@@ -243,6 +247,13 @@ ParamsPtr art_planner::loadRosParameters(const ros::NodeHandle& nh) {
                                  "robot/torso/offset/z",
                                  params->robot.torso.offset.z);
 
+  // Robot / Feet.
+
+  params->robot.feet.plane_symmetries =
+      getParamWithDefaultWarning(nh,
+                                 "robot/feet/plane_symmetries",
+                                 params->robot.feet.plane_symmetries);
+
   // Robot / Feet / Offset.
 
   params->robot.feet.offset.x =

art_planner_ros/src/visualizer.cpp

@@ -257,14 +257,35 @@ void Visualizer::visualizeCollisionBoxes() {
   marker.color.b = 0;
   marker.pose.position.z = params_->robot.feet.offset.z;
 
-  for (unsigned int i = 0; i < 4; ++i) {
-    marker.pose.position.x = i < 2 ? params_->robot.feet.offset.x : -params_->robot.feet.offset.x;
-    marker.pose.position.y = i % 2 ? params_->robot.feet.offset.y : -params_->robot.feet.offset.y;
-
-    array.markers.push_back(marker);
-    ++marker.id;
+  // Add foot markers to separate vector first so we can iterate over them.
+  std::vector<visualization_msgs::Marker> feet_markers;
+  marker.pose.position.x = params_->robot.feet.offset.x;
+  marker.pose.position.y = params_->robot.feet.offset.y;
+  feet_markers.push_back(marker);
+  ++marker.id;
+
+  for (const auto& plane: params_->robot.feet.plane_symmetries) {
+    size_t n_boxes = feet_markers.size();
+    if (plane == "sagittal") {
+      // Need to iterate like this because we alter vector size during loop.
+      for (size_t i = 0; i < n_boxes; ++i) {
+        feet_markers.push_back(feet_markers[i]);
+        feet_markers.back().pose.position.y *= -1;
+        feet_markers.back().id = marker.id++;
+      }
+    } else if (plane == "coronal") {
+      // Need to iterate like this because we alter vector size during loop.
+      for (size_t i = 0; i < n_boxes; ++i) {
+        feet_markers.push_back(feet_markers[i]);
+        feet_markers.back().pose.position.x *= -1;
+        feet_markers.back().id = marker.id++;
+      }
+    }
   }
 
+  // Add foot markers to output array.
+  array.markers.insert(array.markers.end(), feet_markers.begin(), feet_markers.end());
+
   collision_pub_thread_ = std::thread(std::bind(&Visualizer::visualizeCollisionBoxesThread,
                                                 this,
                                                 array));