Better implementations of DoMaybeGetFeasiblePoint for some subclasses…

… of ConvexSet. (#19824) fixup private doxygen
RobotLocomotion · Jul 23, 2023 · 3fdd74c · 3fdd74c
1 parent 23e3325
commit 3fdd74c
Show file tree

Hide file tree

Showing 8 changed files with 61 additions and 0 deletions.
diff --git a/geometry/optimization/cartesian_product.cc b/geometry/optimization/cartesian_product.cc
@@ -135,7 +135,24 @@ std::optional<VectorXd> CartesianProduct::DoMaybeGetPoint() const {
       return std::nullopt;
     }
   }
+  return CartesianProduct::StackAndMaybeTransform(points);
+}
+
+std::optional<Eigen::VectorXd> CartesianProduct::DoMaybeGetFeasiblePoint()
+    const {
+  std::vector<VectorXd> points;
+  for (const auto& s : sets_) {
+    if (std::optional<VectorXd> point = s->MaybeGetFeasiblePoint()) {
+      points.push_back(std::move(*point));
+    } else {
+      return std::nullopt;
+    }
+  }
+  return CartesianProduct::StackAndMaybeTransform(points);
+}
 
+VectorXd CartesianProduct::StackAndMaybeTransform(
+    const std::vector<Eigen::VectorXd>& points) const {
   // Stack the points.
   const int y_ambient_dimension = A_ ? A_->rows() : ambient_dimension();
   int start = 0;

diff --git a/geometry/optimization/cartesian_product.h b/geometry/optimization/cartesian_product.h
@@ -87,6 +87,15 @@ class CartesianProduct final : public ConvexSet {
 
   std::optional<Eigen::VectorXd> DoMaybeGetPoint() const final;
 
+  std::optional<Eigen::VectorXd> DoMaybeGetFeasiblePoint() const final;
+
+  /* Given a list of vectors, one from each constituent set of this
+  CartesianProduct, this stacks them into a single vector. Then, if this
+  CartesianProduct has an associated transformation (in the form of an A_
+  matrix and b_ vector), it applies that transformation. */
+  Eigen::VectorXd StackAndMaybeTransform(
+      const std::vector<Eigen::VectorXd>& points) const;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

diff --git a/geometry/optimization/hyperellipsoid.cc b/geometry/optimization/hyperellipsoid.cc
@@ -193,6 +193,10 @@ bool Hyperellipsoid::DoIsEmpty() const {
   return false;
 }
 
+std::optional<Eigen::VectorXd> Hyperellipsoid::DoMaybeGetFeasiblePoint() const {
+  return center_;
+}
+
 bool Hyperellipsoid::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                                   double tol) const {
   DRAKE_DEMAND(A_.cols() == x.size());

diff --git a/geometry/optimization/hyperellipsoid.h b/geometry/optimization/hyperellipsoid.h
@@ -117,6 +117,9 @@ class Hyperellipsoid final : public ConvexSet {
 
   // N.B. No need to override DoMaybeGetPoint here.
 
+  /** Returns the center, which is always feasible. */
+  std::optional<Eigen::VectorXd> DoMaybeGetFeasiblePoint() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

diff --git a/geometry/optimization/minkowski_sum.cc b/geometry/optimization/minkowski_sum.cc
@@ -132,6 +132,22 @@ std::optional<VectorXd> MinkowskiSum::DoMaybeGetPoint() const {
   return result;
 }
 
+std::optional<VectorXd> MinkowskiSum::DoMaybeGetFeasiblePoint() const {
+  std::optional<VectorXd> result;
+  for (const auto& s : sets_) {
+    if (std::optional<VectorXd> point = s->MaybeGetFeasiblePoint()) {
+      if (result.has_value()) {
+        *result += *point;
+      } else {
+        result = std::move(point);
+      }
+    } else {
+      return std::nullopt;
+    }
+  }
+  return result;
+}
+
 bool MinkowskiSum::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                                 double) const {
   // TODO(russt): Figure out if there is a general way to communicate tol

diff --git a/geometry/optimization/minkowski_sum.h b/geometry/optimization/minkowski_sum.h
@@ -72,6 +72,8 @@ class MinkowskiSum final : public ConvexSet {
 
   std::optional<Eigen::VectorXd> DoMaybeGetPoint() const final;
 
+  std::optional<Eigen::VectorXd> DoMaybeGetFeasiblePoint() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc
@@ -351,6 +351,14 @@ std::optional<VectorXd> VPolytope::DoMaybeGetPoint() const {
   return std::nullopt;
 }
 
+std::optional<VectorXd> VPolytope::DoMaybeGetFeasiblePoint() const {
+  if (IsEmpty()) {
+    return std::nullopt;
+  } else {
+    return vertices_.col(0);
+  }
+}
+
 bool VPolytope::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                              double tol) const {
   if (vertices_.cols() == 0) {

diff --git a/geometry/optimization/vpolytope.h b/geometry/optimization/vpolytope.h
@@ -102,6 +102,8 @@ class VPolytope final : public ConvexSet {
 
   std::optional<Eigen::VectorXd> DoMaybeGetPoint() const final;
 
+  std::optional<Eigen::VectorXd> DoMaybeGetFeasiblePoint() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;