Sampling.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
 
#include "Sampling.h"
 
#include <cmath>
#include <limits>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::navigation::evaluation
{
    namespace
    {
        core::Pose
        poseAt(const Eigen::Vector2f& position, const float yaw)
        {
            core::Pose pose = core::Pose::Identity();
            pose.translation() = Eigen::Vector3f{position.x(), position.y(), 0.F};
            pose.linear() = Eigen::AngleAxisf{yaw, Eigen::Vector3f::UnitZ()}.toRotationMatrix();
            return pose;
        }
 
        /// All cell centres whose clearance falls in [lo, hi]. Collected once and sampled from
        /// by index: rejection sampling on a band this narrow would spend most of its attempts
        /// missing, and the band is the whole point of the near-obstacle category.
        std::vector<Eigen::Vector2f>
        collectCells(const algorithms::Costmap& costmap, const float lo, const float hi)
        {
            std::vector<Eigen::Vector2f> cells;
 
            for (int ix = 0; ix < costmap.getGrid().rows(); ix++)
            {
                for (int iy = 0; iy < costmap.getGrid().cols(); iy++)
                {
                    const algorithms::Costmap::Index index{ix, iy};
                    if (not costmap.isValid(index))
                    {
                        continue;
                    }
 
                    const float value = costmap.getGrid()(ix, iy);
                    if (value >= lo and value <= hi)
                    {
                        cells.push_back(costmap.toPositionGlobal(index));
                    }
                }
            }
 
            return cells;
        }
    } // namespace
 
    std::string
    toString(const RequestKind kind)
    {
        switch (kind)
        {
            case RequestKind::Uniform:
                return "uniform";
            case RequestKind::NearObstacle:
                return "near_obstacle";
            case RequestKind::Location:
                return "location";
            case RequestKind::CorridorExit:
                return "corridor_exit";
        }
 
        return "unknown";
    }
 
    std::vector<Request>
    sampleRequests(const RequestKind kind,
                   const int count,
                   const algorithms::Costmap& costmap,
                   const RoomScene& room,
                   const SamplingParameters& parameters,
                   std::mt19937& rng)
    {
        std::vector<Request> requests;
        requests.reserve(static_cast<std::size_t>(count));
 
        std::uniform_real_distribution<float> yawDistribution{-M_PIf32, M_PIf32};
 
        if (kind == RequestKind::Location)
        {
            // Several named locations are manipulation poses set right at a worktop, so the
            // robot's own footprint puts them inside an obstacle in the *planning* costmap.
            // Those are requests for the recovery path, not for the planner proper, and
            // mixing them in would report recovery behaviour as a planning failure rate.
            std::vector<NamedPose> reachable;
            for (const NamedPose& location : room.locations)
            {
                const Eigen::Vector2f position = location.pose.translation().head<2>();
                const auto clearance = costmap.value(position);
 
                if (clearance.has_value() and clearance.value() > 0.F)
                {
                    reachable.push_back(location);
                }
                else
                {
                    ARMARX_WARNING << "Location `" << location.name
                                   << "` is inside an obstacle for a robot of this radius; "
                                      "excluded from the location category.";
                }
            }
 
            if (reachable.size() < 2)
            {
                ARMARX_WARNING << "Fewer than two reachable named locations; skipping this "
                                  "category.";
                return requests;
            }
 
            std::uniform_int_distribution<std::size_t> pick{0, reachable.size() - 1};
 
            for (int index = 0; index < count; index++)
            {
                std::size_t from = pick(rng);
                std::size_t to = pick(rng);
                for (int attempt = 0; attempt < parameters.maxAttempts and to == from; attempt++)
                {
                    to = pick(rng);
                }
 
                // The graph's own poses, orientation included -- resampling the yaw would
                // discard the approach direction that makes these requests realistic.
                requests.push_back({.kind = kind,
                                    .index = index,
                                    .start = reachable.at(from).pose,
                                    .goal = reachable.at(to).pose,
                                    .startName = reachable.at(from).name,
                                    .goalName = reachable.at(to).name});
            }
 
            return requests;
        }
 
        if (kind == RequestKind::CorridorExit)
        {
            if (not room.corridor.has_value())
            {
                ARMARX_WARNING << "No corridor in this scene; skipping the corridor-exit "
                                  "category.";
                return requests;
            }
 
            const auto& corridor = room.corridor.value();
 
            // Anything inside the corridor the robot actually fits in, and anything outside it
            // that is clear. The corridor's own cells are what makes this hard: in a 900 mm
            // channel a 350 mm robot has 100 mm to either side, so the start is pinned with
            // almost no room.
            std::vector<Eigen::Vector2f> inside;
            std::vector<Eigen::Vector2f> outside;
 
            for (int ix = 0; ix < costmap.getGrid().rows(); ix++)
            {
                for (int iy = 0; iy < costmap.getGrid().cols(); iy++)
                {
                    const algorithms::Costmap::Index index{ix, iy};
                    if (not costmap.isValid(index) or costmap.getGrid()(ix, iy) <= 0.F)
                    {
                        continue;
                    }
 
                    const Eigen::Vector2f p = costmap.toPositionGlobal(index);
 
                    if (contains(corridor, p))
                    {
                        inside.push_back(p);
                    }
                    else if (p.y() > corridor.max.y() + 500.F)
                    {
                        // Clear of the mouth, so the request is a genuine exit rather than a
                        // step across the threshold.
                        outside.push_back(p);
                    }
                }
            }
 
            if (inside.empty() or outside.empty())
            {
                ARMARX_WARNING << "Corridor holds " << inside.size() << " free cell(s) and "
                               << outside.size()
                               << " outside it; a robot of this radius does not fit. Skipping "
                                  "the corridor-exit category.";
                return requests;
            }
 
            ARMARX_INFO << "corridor_exit: " << inside.size() << " cells inside the corridor, "
                        << outside.size() << " outside.";
 
            std::uniform_int_distribution<std::size_t> pickInside{0, inside.size() - 1};
            std::uniform_int_distribution<std::size_t> pickOutside{0, outside.size() - 1};
 
            for (int index = 0; index < count; index++)
            {
                const Eigen::Vector2f start = inside.at(pickInside(rng));
                const Eigen::Vector2f goal = outside.at(pickOutside(rng));
 
                // Start heading points along the corridor, as it would if the robot had driven
                // in: a start facing a wall would make this a test of the orientation stage.
                const float startYaw = std::atan2(goal.y() - start.y(), goal.x() - start.x());
 
                requests.push_back({.kind = kind,
                                    .index = index,
                                    .start = poseAt(start, startYaw),
                                    .goal = poseAt(goal, yawDistribution(rng)),
                                    .startName = {},
                                    .goalName = {}});
            }
 
            return requests;
        }
 
        const float lo = (kind == RequestKind::NearObstacle) ? parameters.nearObstacleMinClearance
                                                             : parameters.nearObstacleMaxClearance;
        const float hi = (kind == RequestKind::NearObstacle) ? parameters.nearObstacleMaxClearance
                                                             : std::numeric_limits<float>::max();
 
        const std::vector<Eigen::Vector2f> cells = collectCells(costmap, lo, hi);
 
        if (cells.size() < 2)
        {
            ARMARX_WARNING << "Only " << cells.size() << " cell(s) in the clearance band [" << lo
                           << ", " << hi << "] mm; skipping the " << toString(kind) << " category.";
            return requests;
        }
 
        ARMARX_INFO << toString(kind) << ": sampling from " << cells.size()
                    << " cells with clearance in [" << lo << ", " << hi << "] mm.";
 
        std::uniform_int_distribution<std::size_t> pick{0, cells.size() - 1};
 
        for (int index = 0; index < count; index++)
        {
            bool found = false;
 
            for (int attempt = 0; attempt < parameters.maxAttempts and not found; attempt++)
            {
                const Eigen::Vector2f start = cells.at(pick(rng));
                const Eigen::Vector2f goal = cells.at(pick(rng));
 
                if ((goal - start).norm() < parameters.minSeparation)
                {
                    continue;
                }
 
                requests.push_back({.kind = kind,
                                    .index = index,
                                    .start = poseAt(start, yawDistribution(rng)),
                                    .goal = poseAt(goal, yawDistribution(rng)),
                                    .startName = {},
                                    .goalName = {}});
                found = true;
            }
 
            if (not found)
            {
                ARMARX_WARNING << "Gave up drawing a " << toString(kind) << " request at least "
                               << parameters.minSeparation << " mm long after "
                               << parameters.maxAttempts << " attempts.";
                break;
            }
        }
 
        return requests;
    }
 
} // namespace armarx::navigation::evaluation