德雷克连续碰撞检测

Question

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

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

但是，我想知道是否有可能作为FCL进行连续碰撞检测的drake API。我在 API.

中找不到它

Answer 1

目前没有进行连续碰撞检测的机制。您可以 post 提出功能请求问题。但是，就目前而言，我们无意将其包括在内。

德雷克连续碰撞检测

Drake Continuous Collision Detection

drake