自定义MinimumDistanceConstraint约束下SNOPT求解异常问题

我基于MinimumDistanceConstraint扩展实现了支持过滤器的版本，可筛选特定几何对，为不同几何对设置不同距离阈值，替代原单一约束逻辑。

无过滤器时，求解器SNOPT能正常将所有几何对优化至设定阈值；但为集合A中部分几何添加过滤器后，SNOPT直接返回解，却存在几何对仍处于碰撞状态的问题。查看SNOPT输出发现非线性惩罚函数返回0（本不应返回0），调用my_min_dist_constraint->CheckSatisfied(x)能正确返回false，梯度计算已验证无误。此外，在Distances()函数中发现，调用internal::CalcDistanceDerivatives()后数值正常，但执行distances.resize(distance_count)后出现大量NaN值。

约束应用代码 #

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));
}

自定义MinimumDistanceConstraintCustom实现代码 #

#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

内容的提问来源于stack exchange，提问作者alvin