Multiple MinimumDistanceConstraint constraints

Question

I'm currently applying multiple MinimumDistanceConstraint constraints. I've modified this class so that I can apply filters to consider or ignore specific pairs of geometries, which enables me to apply different distance thresholds to different pairs. So instead of defining one single MinimumDistanceConstraint, I define multiple of these with different distance thresholds and geometry pair filters.

I'm seeing very strange behavior when I define some filters vs none at all. When I define no filters, the solver (SNOPT) correctly optimizes all geometry pairs away with the same distance threshold.

Now when I apply some filters to certain geometries in set A, SNOPT suddenly returns immediately with a solution without getting all the geometries out of collision. Looking at SNOPT's output file, I see that the nonlinear penalty function returned 0 when it should not be (since certain geometry pairs are still violating their distance constraints). When I call my_min_dist_constraint->CheckSatisfied(x) on these violated constraints, I correctly get a return value of false. I've double-checked that I haven't messed up gradient calculations. So how can the constraint return false, but the solver still return that all constraints are satisfied?

I've pasted my custom MinimumDistanceConstraint below. I've verified that the logic of the filters is correct, and that they are being considered in the Distances() function. When comparing the code with drake's own implementation, you'll see that the derivative and value computation are unchanged, I am only ignoring certain geometry pairs. The result is that I will have multiple nonlinear, mininimum_value constraints of smaller vector length as opposed to a single, large constraint.

[EDIT] In the Distances function, I notice that right after calculating &distances(distance_count) from internal::CalcDistanceDerivatives(), its value is a reasonable scalar. However, when printing the entire distances at the end of the function, I get many nan values when they shouldn't be. Before calling distances.resize(distance_count), most of the values are still reasonable, so clearly the resize is causing the issue.

Here is how I would generally apply multiple distance constraints:

    for (int cons_idx=0; cons_idx<distance_filters.size(); cons_idx++) {
      auto constraint =
          std::shared_ptr<MinimumDistanceConstraintCustom>(new MinimumDistanceConstraintCustom(
              &plant_, minimum_distances.at(cons_idx), get_mutable_context(), distance_filters.at(cons_idx), {},
              influence_distance_offset));
      prog_->AddConstraint(constraint, q_);

    // Finally, one more constraint for all the collision pairs that don't fit in any of the collision groups
    auto constraint =
        std::shared_ptr<MinimumDistanceConstraintCustom>(new MinimumDistanceConstraintCustom(
            &plant_, complement_minimum_distance, get_mutable_context(), distance_filters, {},
            influence_distance_offset));
    }

Here is how MinimumDistanceConstraintCustom is defined:

#include "control/drake_library/minimum_distance_constraint.h"

#include <limits>
#include <vector>

#include <Eigen/Dense>

#include "drake/multibody/inverse_kinematics/distance_constraint_utilities.h"
#include "drake/multibody/inverse_kinematics/kinematic_evaluator_utilities.h"

#include <string>
#include <iostream>

namespace drake {
namespace multibody {
using internal::RefFromPtrOrThrow;
using namespace std;


template <typename T, typename S>
VectorX<S> Distances(const MultibodyPlant<T>& plant,
                     systems::Context<T>* context,
                     const Eigen::Ref<const VectorX<S>>& q,
                     double influence_distance,
                     double minimum_distance,
                     const std::unordered_set<GeomPair, min_dist_pair_hash>& considered_pairs) {
  internal::UpdateContextConfiguration(context, plant, q);
  const auto& query_port = plant.get_geometry_query_input_port();
  if (!query_port.HasValue(*context)) {
    throw std::invalid_argument(
        "MinimumDistanceConstraintCustom: Cannot get a valid geometry::QueryObject. "
        "Either the plant geometry_query_input_port() is not properly "
        "connected to the SceneGraph's output port, or the plant_context_ is "
        "incorrect. Please refer to AddMultibodyPlantSceneGraph on connecting "
        "MultibodyPlant to SceneGraph.");
  }
  const auto& query_object =
      query_port.template Eval<geometry::QueryObject<T>>(*context);
  const geometry::SceneGraphInspector<T>& inspector =
          query_object.inspector();

  // Get all candidate collision geometry pairs
  // NOTE: querying all distances, then selecting is faster than looping and querying distance for each individual pair
  const std::vector<geometry::SignedDistancePair<T>> signed_distance_pairs =
      query_object.ComputeSignedDistancePairwiseClosestPoints(
          influence_distance);
  VectorX<S> distances(signed_distance_pairs.size());
  int distance_count{0};
  cout << "Distances! " << endl;
  for (const auto& signed_distance_pair : signed_distance_pairs) {
    const GeomPair p1 {signed_distance_pair.id_A, signed_distance_pair.id_B};
    const GeomPair p2 {signed_distance_pair.id_B, signed_distance_pair.id_A};
    bool is_considered = (
      (considered_pairs.find(p1) != considered_pairs.end()) || 
      (considered_pairs.find(p2) != considered_pairs.end())
    );
    if (is_considered && signed_distance_pair.distance < influence_distance) {
      // cout << "considering " << inspector.GetName(signed_distance_pair.id_A) << "(" << signed_distance_pair.id_A << ")" << ", ";
      // cout << inspector.GetName(signed_distance_pair.id_B) << "(" << signed_distance_pair.id_B << ")";
      // cout << ": dist " << signed_distance_pair.distance << " vs influence dist " << influence_distance << " vs minimum_distance " << minimum_distance << endl;
      const geometry::FrameId frame_A_id =
          inspector.GetFrameId(signed_distance_pair.id_A);
      const geometry::FrameId frame_B_id =
          inspector.GetFrameId(signed_distance_pair.id_B);
      const Frame<T>& frameA =
          plant.GetBodyFromFrameId(frame_A_id)->body_frame();
      const Frame<T>& frameB =
          plant.GetBodyFromFrameId(frame_B_id)->body_frame();
      internal::CalcDistanceDerivatives(
          plant, *context, frameA, frameB,
          // GetPoseInFrame() returns RigidTransform<double> -- we can't
          // multiply across heterogeneous scalar types; so we cast the double
          // to T.
          inspector.GetPoseInFrame(signed_distance_pair.id_A)
                  .template cast<T>() *
              signed_distance_pair.p_ACa,
          signed_distance_pair.distance, signed_distance_pair.nhat_BA_W, q,
          &distances(distance_count++));
    }
  }
  distances.resize(distance_count);
  return distances;
}

template <typename T>
void MinimumDistanceConstraintCustom::Initialize(
    const MultibodyPlant<T>& plant, systems::Context<T>* plant_context,
    double minimum_distance, double influence_distance_offset,
    MinimumDistancePenaltyFunction penalty_function,
    const std::vector<MinimumDistanceConstraintFilter> filters, const bool complement_all_filters,
    std::unordered_set<GeomPair, min_dist_pair_hash>& considered_pairs) {
  CheckPlantIsConnectedToSceneGraph(plant, *plant_context);
  if (!std::isfinite(influence_distance_offset)) {
    throw std::invalid_argument(
        "MinimumDistanceConstraintCustom: influence_distance_offset must be finite.");
  }
  if (influence_distance_offset <= 0) {
    throw std::invalid_argument(
        "MinimumDistanceConstraintCustom: influence_distance_offset must be "
        "positive.");
  }
  const auto& query_port = plant.get_geometry_query_input_port();
  // Maximum number of SignedDistancePairs returned by calls to
  // ComputeSignedDistancePairwiseClosestPoints().
  const auto all_collision_candidates = 
    query_port.template Eval<geometry::QueryObject<T>>(*plant_context)
          .inspector()
          .GetCollisionCandidates();
  if (filters.size() > 0) {
    for (const GeomPair& geom_pair : all_collision_candidates) {
      if (min_dist_should_include(geom_pair, filters, complement_all_filters)) {
        considered_pairs.insert(geom_pair);
      }
    }
  } else {
    std::copy(all_collision_candidates.begin(),
            all_collision_candidates.end(),
            std::inserter(considered_pairs, considered_pairs.begin()));
  }

  minimum_value_constraint_ = std::make_unique<solvers::MinimumValueConstraint>(
      this->num_vars(), minimum_distance, influence_distance_offset,
      all_collision_candidates.size(),
      [&plant, plant_context, &considered_pairs, minimum_distance](const auto& x, double influence_distance) {
        return Distances<T, AutoDiffXd>(plant, plant_context, x,
                                        influence_distance, minimum_distance, considered_pairs);
      },
      [&plant, plant_context, &considered_pairs, minimum_distance](const auto& x, double influence_distance) {
        return Distances<T, double>(plant, plant_context, x,
                                    influence_distance, minimum_distance, considered_pairs);
      });
  this->set_bounds(minimum_value_constraint_->lower_bound(),
                   minimum_value_constraint_->upper_bound());
  if (penalty_function) {
    minimum_value_constraint_->set_penalty_function(penalty_function);
  }
}

MinimumDistanceConstraintCustom::MinimumDistanceConstraintCustom(
    const multibody::MultibodyPlant<double>* const plant,
    double minimum_distance, systems::Context<double>* plant_context,
    MinimumDistancePenaltyFunction penalty_function,
    double influence_distance_offset)
    : solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
                          Vector1d(0), Vector1d(0)),
      plant_double_{plant},
      plant_context_double_{plant_context},
      plant_autodiff_{nullptr},
      plant_context_autodiff_{nullptr} {
  Initialize(*plant_double_, plant_context_double_, minimum_distance,
             influence_distance_offset, penalty_function, 
             {}, 
             false, /*Doesn't matter*/
             considered_pairs_);
}

MinimumDistanceConstraintCustom::MinimumDistanceConstraintCustom(
    const multibody::MultibodyPlant<AutoDiffXd>* const plant,
    double minimum_distance, systems::Context<AutoDiffXd>* plant_context,
    MinimumDistancePenaltyFunction penalty_function,
    double influence_distance_offset)
    : solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
                          Vector1d(0), Vector1d(0)),
      plant_double_{nullptr},
      plant_context_double_{nullptr},
      plant_autodiff_{plant},
      plant_context_autodiff_{plant_context} {
  Initialize(*plant_autodiff_, plant_context_autodiff_, minimum_distance,
             influence_distance_offset, penalty_function, 
             {}, 
             false, /*Doesn't matter*/
             considered_pairs_);
}

MinimumDistanceConstraintCustom::MinimumDistanceConstraintCustom(
    const multibody::MultibodyPlant<double>* const plant,
    double minimum_distance, systems::Context<double>* plant_context, 
    const MinimumDistanceConstraintFilter filter,
    MinimumDistancePenaltyFunction penalty_function,
    double influence_distance_offset)
    : solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
                          Vector1d(0), Vector1d(0)),
      plant_double_{plant},
      plant_context_double_{plant_context},
      plant_autodiff_{nullptr},
      plant_context_autodiff_{nullptr} {
  Initialize(*plant_double_, plant_context_double_, minimum_distance,
             influence_distance_offset, penalty_function, {filter}, false, considered_pairs_);
}

MinimumDistanceConstraintCustom::MinimumDistanceConstraintCustom(
    const multibody::MultibodyPlant<double>* const plant,
    double minimum_distance, systems::Context<double>* plant_context, 
    const std::vector<MinimumDistanceConstraintFilter> complement_filters,
    MinimumDistancePenaltyFunction penalty_function,
    double influence_distance_offset)
    : solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
                          Vector1d(0), Vector1d(0)),
      plant_double_{plant},
      plant_context_double_{plant_context},
      plant_autodiff_{nullptr},
      plant_context_autodiff_{nullptr} {
  Initialize(*plant_double_, plant_context_double_, minimum_distance,
             influence_distance_offset, penalty_function,
             complement_filters, true, considered_pairs_);
}

template <typename T>
void MinimumDistanceConstraintCustom::DoEvalGeneric(
    const Eigen::Ref<const VectorX<T>>& x, VectorX<T>* y) const {
  minimum_value_constraint_->Eval(x, y);
}

void MinimumDistanceConstraintCustom::DoEval(
    const Eigen::Ref<const Eigen::VectorXd>& x, Eigen::VectorXd* y) const {
  DoEvalGeneric(x, y);
}

void MinimumDistanceConstraintCustom::DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
                                       AutoDiffVecXd* y) const {
  DoEvalGeneric(x, y);
}
}  // namespace multibody
}  // namespace drake

Answer 1

SNOPT suddenly returns immediately with a solution without getting all the geometries out of collision.

First I would suggest to evaluate your constraint at this solution. You could do it through

std::cout << "constraint value: " << result.EvalBinding(constraint).transpose() << "\n";
std::cout << "constraint lower bound: " << constraint.evaluator()->lower_bound().transpose() << "\nconstraint upper bound: " << constraint.evaluator()>upper_bound().transpose() << "\n";

I think you will see that the constraint value is indeed between the lower bound and the upper bound at the solution. This suggest that the problem is in how you evaluate your constraint.

From a quick glance of your code, I believe the issue is in this line

distances.resize(distance_count);

in your Distance function. Check Eigen's documentation on resize function. Specifically it says

The resize() method is a no-operation if the actual matrix size doesn't change; otherwise it is destructive: the values of the coefficients may change.

Since you change the size of distances, after calling resize your distances vector is not meaningful.

I would suggest to use conservativeResize() function instead of resize.