问Drake连续碰撞检测

EN

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