德雷克连续碰撞检测

问题描述

我尝试在 OMPL 中使用 drake 作为碰撞检测功能，并且我已经实现了状态有效性检查器，如下所示：

bool stateValidityChecker (drake::multibody::MultibodyPlant<double> * plant,drake::systems::Context<double> * plant_context,drake::multibody::ModelInstanceIndex agent_idx,const ob::State *state) {

int robot_dof = plant->num_positions(agent_idx);
Eigen::VectorXd cfg = OMPLStatetoEigen(state,robot_dof);
plant->SetPositions(plant_context,agent_idx,cfg);
const auto &query_port = plant->get_geometry_query_input_port();
const auto &query_object = query_port.Eval<drake::geometry::QueryObject<double>>(*plant_context);
std::vector<drake::geometry::SigneddistancePair<double>> signed_distance_pairs =
        query_object.ComputeSigneddistancePairwiseClosestPoints(0.1);
bool is_collision_free = true;
for (const auto &signed_distance_pair : signed_distance_pairs) {
    // stop once any collision is detected:
    if (signed_distance_pair.distance < 1e-2) {
        // uncomment to display info on the colliding object; warning,very verbose
        const auto& inspector = query_object.inspector(); const auto&
                name_A = inspector.GetName(signed_distance_pair.id_A);
        const auto&
                name_B = inspector.GetName(signed_distance_pair.id_B);
        drake::log()->info("{} <--> {} is: {}",name_A,name_B,signed_distance_pair.distance);
        is_collision_free = false;
        return is_collision_free;
    }
}
return is_collision_free;}

但是，我想知道是否有可能的 drake API 用于作为 FCL 进行连续碰撞检测。我在 API 中找不到它。

解决方法

目前没有进行连续碰撞检测的机制。您可以为功能请求发布问题。但是，就目前而言，不打算包含它。