From 070b4c4cc86ae93ce79bf75277fe048edae11153 Mon Sep 17 00:00:00 2001 From: Lorenz Wellhausen Date: Fri, 3 Mar 2023 20:04:48 +0100 Subject: [PATCH] Generalized reachability to better work for non-quadrupedal robot configurations --- art_planner/include/art_planner/params.h | 7 ++- .../validity_checker/validity_checker_feet.h | 1 + art_planner/src/sampler.cpp | 7 ++- .../validity_checker_feet.cpp | 35 +++++++++++-- art_planner_ros/config/params.yaml | 10 ++-- .../include/art_planner_ros/utils.h | 51 +++++++++++++++++-- art_planner_ros/src/utils.cpp | 11 ++++ art_planner_ros/src/visualizer.cpp | 33 +++++++++--- 8 files changed, 135 insertions(+), 20 deletions(-) diff --git a/art_planner/include/art_planner/params.h b/art_planner/include/art_planner/params.h index 24c7d9c..baa0f2f 100644 --- a/art_planner/include/art_planner/params.h +++ b/art_planner/include/art_planner/params.h @@ -1,8 +1,9 @@ #pragma once -#include #include #include +#include +#include @@ -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 plane_symmetries{"sagittal", + "coronal"}; + struct { double x{0.362}; double y{0.225}; diff --git a/art_planner/include/art_planner/validity_checker/validity_checker_feet.h b/art_planner/include/art_planner/validity_checker/validity_checker_feet.h index 498a28d..a3cada2 100644 --- a/art_planner/include/art_planner/validity_checker/validity_checker_feet.h +++ b/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 box_centers_; bool boxIsValidAtPose(const Pose3& pose) const; diff --git a/art_planner/src/sampler.cpp b/art_planner/src/sampler.cpp index b843c59..f5c7353 100644 --- a/art_planner/src/sampler.cpp +++ b/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); diff --git a/art_planner/src/validity_checker/validity_checker_feet.cpp b/art_planner/src/validity_checker/validity_checker_feet.cpp index 9b402c4..213b5a7 100644 --- a/art_planner/src/validity_checker/validity_checker_feet.cpp +++ b/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 &poses) bool ValidityCheckerFeet::isValid(const Pose3& pose) const { - std::vector 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 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); } diff --git a/art_planner_ros/config/params.yaml b/art_planner_ros/config/params.yaml index 9974a74..1c5a4f6 100644 --- a/art_planner_ros/config/params.yaml +++ b/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 diff --git a/art_planner_ros/include/art_planner_ros/utils.h b/art_planner_ros/include/art_planner_ros/utils.h index d3c62cf..9c686d6 100644 --- a/art_planner_ros/include/art_planner_ros/utils.h +++ b/art_planner_ros/include/art_planner_ros/utils.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -25,13 +26,57 @@ inline T getParamWithDefaultWarning(const ros::NodeHandle& nh, return param; } +// Specialize for unsigned int. template <> -inline unsigned int getParamWithDefaultWarning(const ros::NodeHandle& nh, - const std::string& name, - const unsigned int& default_val) { +inline unsigned int getParamWithDefaultWarning( + const ros::NodeHandle& nh, + const std::string& name, + const unsigned int& default_val) { return getParamWithDefaultWarning(nh, name, static_cast(default_val)); } +// Overload for std::vector. +template +inline std::vector getParamWithDefaultWarning( + const ros::NodeHandle& nh, + const std::string& name, + const std::vector& default_val) { + std::vector 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 getParamWithDefaultWarning( + const ros::NodeHandle& nh, + const std::string& name, + const std::vector& default_val) { + std::vector 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); diff --git a/art_planner_ros/src/utils.cpp b/art_planner_ros/src/utils.cpp index a0dd9f7..7e02795 100644 --- a/art_planner_ros/src/utils.cpp +++ b/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 = diff --git a/art_planner_ros/src/visualizer.cpp b/art_planner_ros/src/visualizer.cpp index dee1a64..5c98f87 100644 --- a/art_planner_ros/src/visualizer.cpp +++ b/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 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));