HandoverCostmapBuilder.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/>.
 *
 * @author      (  )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "HandoverCostmapBuilder.h"
 
#include <algorithm>
#include <cmath>
#include <optional>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <VisionX/libraries/armem_human/util.h>
 
namespace armarx::navigation::algorithms::costmap
{
 
 
    HandoverCostmapBuilder::HandoverCostmapBuilder(const Costmap::Parameters& costmapParameters,
                                                   const Parameters& handoverParameters) :
        costmapParameters_(costmapParameters), handoverParameters_(handoverParameters)
    {
        ARMARX_CHECK_POSITIVE(handoverParameters_.handoverDistanceMin);
        ARMARX_CHECK_POSITIVE(handoverParameters_.handoverDistanceMax);
        ARMARX_CHECK_GREATER(handoverParameters_.handoverDistanceMax,
                             handoverParameters_.handoverDistanceMin);
 
        ARMARX_CHECK_LESS_EQUAL(handoverParameters_.handoverAngleMin,
                                handoverParameters_.handoverAngleMax);
 
        ARMARX_CHECK_LESS_EQUAL(handoverParameters_.handoverAngleMin,
                                handoverParameters_.handoverAnglePreference);
        ARMARX_CHECK_LESS_EQUAL(handoverParameters_.handoverAnglePreference,
                                handoverParameters_.handoverAngleMax);
 
        ARMARX_CHECK_GREATER_EQUAL(handoverParameters_.robotPathCostWeight, 0.F);
        ARMARX_CHECK_POSITIVE(handoverParameters_.robotPathMaxCostMM);
    }
 
    std::optional<Costmap>
    HandoverCostmapBuilder::create(const HandoverCostmapBuilder::SceneInformation& sceneInfo)
    {
        const std::optional<core::Pose2D> global_T_human_opt =
            sceneInfo.humanPose2DOverride.has_value()
                ? sceneInfo.humanPose2DOverride
                : armem::human::calculatePose(sceneInfo.humanPose);
 
        if (not global_T_human_opt.has_value())
        {
            // cannot compute human pose
            return std::nullopt;
        }
 
        // obtain scene information from human position
        const auto sceneBounds = [this, global_T_human = global_T_human_opt.value()]
        {
            // scene bounds
            const Eigen::Vector2f human_T_left{-handoverParameters_.humanSceneBoundOffsetSideLeft,
                                               0};
            const Eigen::Vector2f human_T_right{handoverParameters_.humanSceneBoundOffsetSideRight,
                                                0};
            const Eigen::Vector2f human_T_front{0, handoverParameters_.humanSceneBoundOffsetFront};
 
            const Eigen::Vector2f human_T_back{0, -handoverParameters_.humanSceneBoundOffsetBehind};
 
            SceneBounds initialSceneBounds;
 
            const auto sceneBounds = computeSceneBounds({global_T_human * human_T_left,
                                                         global_T_human * human_T_right,
                                                         global_T_human * human_T_front,
                                                         global_T_human * human_T_back},
                                                        initialSceneBounds,
                                                        200);
 
            return sceneBounds;
        }();
 
        // helper function to project a value to [-1, 1].
        // Values outside the min/max range are clamped.
        // min maps to -1.0, max maps to 1.0 while preference maps to 0.0
        const auto projectToInterval =
            [](const float value, const float min, const float max, const float preference) -> float
        {
            ARMARX_CHECK_GREATER(max, min);
            if (value <= min)
            {
                return -1.0f;
            }
            if (value >= max)
            {
                return 1.0f;
            }
            if (value == preference)
            {
                return 0.0f;
            }
            if (value < preference)
            {
                return -((preference - value) / (preference - min));
            }
            // value > preference
            return (value - preference) / (max - preference);
        };
 
        const auto costFn = [this, &projectToInterval](float distanceToObstacle,
                                                       float distanceToHuman,
                                                       float angleToHuman) -> std::optional<float>
        {
            // check bounds
            if (distanceToHuman < handoverParameters_.handoverDistanceMin ||
                distanceToHuman > handoverParameters_.handoverDistanceMax)
            {
                return std::nullopt;
            }
 
            if (angleToHuman < handoverParameters_.handoverAngleMin ||
                angleToHuman > handoverParameters_.handoverAngleMax)
            {
                return std::nullopt;
            }
 
 
            // distance cost (scaled to [-1, 1])
            const float xDistance =
                projectToInterval(distanceToHuman,
                                  handoverParameters_.handoverDistanceMin,
                                  handoverParameters_.handoverDistanceMax,
                                  handoverParameters_.handoverDistancePreference);
            const float xAngle = projectToInterval(angleToHuman,
                                                   handoverParameters_.handoverAngleMin,
                                                   handoverParameters_.handoverAngleMax,
                                                   handoverParameters_.handoverAnglePreference);
 
            // compute factors that scale cost based on distance and angle
            // both factors are in [0, +inf); 0 means optimal, +inf means worst
            const double fDistance = std::abs(std::tan(xDistance * M_PI_2));
            const double fAngle = std::abs(std::tan(xAngle * M_PI_2));
 
            // in order to avoid multiplication by zero, we add an offset of 1. This avoids
            // zero-cost areas that cannot be distinguished.
 
            // base cost is the product of both factors
            const double f = (fDistance + 1) * (fAngle + 1);
 
            // slight additive penalty for candidates too close to an obstacle, instead
            // of scaling the whole cost by the (raw) distance to the nearest obstacle
            const double obstaclePenalty =
                (distanceToObstacle < handoverParameters_.obstacleProximityThreshold)
                    ? handoverParameters_.obstacleProximityPenalty
                    : 0.0;
 
            return static_cast<float>(f + obstaclePenalty);
        };
 
 
        // create costmap
        auto grid = CostmapBuilder::createUniformGrid(sceneBounds, costmapParameters_);
 
        Costmap costmap(grid, costmapParameters_, sceneBounds);
        // the loop below writes through `getMutableMask()`, which requires the mask
        // to already hold a value (otherwise it is a disengaged std::optional). Every
        // cell visited by the loop overwrites its mask entry unconditionally, so the
        // mask's initial content here is irrelevant, only its presence and size matter.
        CostmapBuilder::updateMask(costmap);
 
        costmap.forEachCell(
            [&costmap, &sceneInfo, global_T_human = global_T_human_opt.value(), &costFn](
                const Costmap::Index& idx)
            {
                // obtain global position of the cell
                const auto global_P_cell = costmap.toPositionGlobal(idx);
 
                // initialize based on distance map
 
                const auto vertex = sceneInfo.distanceMap.toVertexOrInvalid(global_P_cell);
 
                // initialize as invalid
                {
                    costmap.getMutableGrid()(idx.x(), idx.y()) = 0.0f;
                    costmap.getMutableMask()->operator()(idx.x(), idx.y()) = false;
                }
 
                if (!vertex.has_value())
                {
                    // outside distance map or invalid
                    return;
                }
 
                // Use isValid()+direct grid access instead of value(), which logs an
                // ARMARX_IMPORTANT message on every masked-out cell. Since the human-
                // centered local scene bounds routinely extend beyond the valid region
                // of the (larger, static) distance map, that path is hit very frequently
                // here and would otherwise spam the log.
                if (!sceneInfo.distanceMap.isValid(vertex->index))
                {
                    // masked out / invalid distance
                    return;
                }
 
                // Now we know that the cell is valid. Hence, we can compute its cost.
                {
 
                    const float distanceToObstacle =
                        sceneInfo.distanceMap.getGrid()(vertex->index.x(), vertex->index.y());
                    const Eigen::Vector2f human_P_cell = global_T_human.inverse() * global_P_cell;
 
                    const float distanceToHuman = human_P_cell.norm();
 
                    const float angleToHuman = std::atan2(human_P_cell.y(), human_P_cell.x());
 
 
                    const auto costOpt = costFn(distanceToObstacle, distanceToHuman, angleToHuman);
 
                    if (not costOpt.has_value())
                    {
                        // invalid cost
                        return;
                    }
 
                    costmap.getMutableGrid()(idx.x(), idx.y()) = costOpt.value();
                    costmap.getMutableMask()->operator()(idx.x(), idx.y()) = true;
                }
            });
 
        // If a robot position is provided, run SPFA from the robot on the static
        // distance map. The resulting path cost is added to every reachable
        // candidate, which favors placements that are closer to the robot and
        // reachable without passing through narrow spaces (SPFA's edge cost
        // penalizes proximity to obstacles). Unreachable candidates are masked out.
        if (sceneInfo.robotPosition.has_value())
        {
            const auto robotVertexOpt =
                sceneInfo.distanceMap.toVertexOrInvalid(sceneInfo.robotPosition.value());
            if (not robotVertexOpt.has_value())
            {
                ARMARX_WARNING << "Robot position is outside the distance map; ignoring robot "
                                  "path cost and reachability.";
            }
            else
            {
                spfa::ShortestPathFasterAlgorithm spfa(sceneInfo.distanceMap,
                                                        handoverParameters_.robotPathParams);
                const auto spfaResult = spfa.spfa(sceneInfo.robotPosition.value());
 
                costmap.forEachCell(
                    [&](const Costmap::Index& idx)
                    {
                        if (not costmap.isValid(idx))
                        {
                            return;
                        }
 
                        const Eigen::Vector2f globalP = costmap.toPositionGlobal(idx);
                        const auto vertexOpt = sceneInfo.distanceMap.toVertexOrInvalid(globalP);
                        if (not vertexOpt.has_value())
                        {
                            costmap.getMutableMask()->operator()(idx.x(), idx.y()) = false;
                            return;
                        }
 
                        const Eigen::Vector2i dmIdx = vertexOpt->index;
                        if (not sceneInfo.distanceMap.isValid(dmIdx) or
                            not spfaResult.reachable(dmIdx.x(), dmIdx.y()))
                        {
                            costmap.getMutableMask()->operator()(idx.x(), idx.y()) = false;
                            return;
                        }
 
                        const float pathCostMM = spfaResult.distances(dmIdx.x(), dmIdx.y());
                        const float normalizedCost =
                            std::min(pathCostMM / handoverParameters_.robotPathMaxCostMM, 1.F);
 
                        costmap.getMutableGrid()(idx.x(), idx.y()) +=
                            handoverParameters_.robotPathCostWeight * normalizedCost;
                    });
            }
        }
 
        return costmap;
    }
 
    std::optional<core::Pose2D>
    HandoverCostmapBuilder::getOptimalHandoverPose(const SceneInformation& sceneInfo)
    {
        const auto costmapOpt = create(sceneInfo);
 
        if (not costmapOpt.has_value())
        {
            return std::nullopt;
        }
 
        const auto& costmap = costmapOpt.value();
        const auto optimum = costmap.optimum();
 
        if (optimum.index.isConstant(-1))
        {
            // no valid optimum found
            return std::nullopt;
        }
 
        // compute orientation towards human
        const auto global_P_human = armem::human::computeMeanPosition(sceneInfo.humanPose);
 
        if (not global_P_human.has_value())
        {
            // cannot compute human pose
            return std::nullopt;
        }
 
        const auto global_P_handover = optimum.position;
 
        const Eigen::Vector2f direction =
            (global_P_human->head<2>() - global_P_handover).normalized();
 
        const float yaw = std::atan2(direction.y(), direction.x());
 
        return core::Pose2D{Eigen::Translation2f{global_P_handover.x(), global_P_handover.y()} *
                            Eigen::Rotation2Df{yaw}};
    }
} // namespace armarx::navigation::algorithms::costmap