ProxemicZoneCreator.cpp

Go to the documentation of this file.

#include "ProxemicZoneCreator.h"
 
#include <math.h>
 
#include <cmath>
 
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/types.h>
 
namespace armarx::navigation::human
{
 
    ProxemicZones
    ProxemicZoneCreator::createProxemicZones(const Human& human)
    {
        const Eigen::Matrix2f global_R_human = human.pose.linear();
        const Eigen::Vector2f global_V_offset = global_R_human * params.offset;
 
        core::Pose2D pose = human.pose;
        pose.translation() += global_V_offset;
 
        // intimate zone
        ProxemicZone intimate{
            .pose = pose,
            .shape = {.a = params.intimateRadius, .b = params.intimateRadius},
            .penalty = intimatePenalty,
            .weight = params.intimateWeight,
            .homotopicRelevance = true,
        };
 
        // personal zone
        float velocityNorm = human.linearVelocity.norm();
        // stretch with velocity in m/s as factor
        float movementStretch = 1 + velocityNorm * params.movementInfluence / 1000; // [mm] to [m]
        if (velocityNorm != 0)
        {
            // y pointing forward
            float movementDirection =
                std::atan2(human.linearVelocity.y(), human.linearVelocity.x()) - M_PI_2;
            pose.linear() = Eigen::Rotation2Df(movementDirection).toRotationMatrix();
            pose.translation() +=
                human.linearVelocity / velocityNorm * params.personalRadius * (movementStretch - 1);
        }
 
        ProxemicZone personal{
            .pose = pose,
            .shape = {.a = params.personalRadius, .b = movementStretch * params.personalRadius},
            .penalty = personalPenalty,
            .weight = params.personalWeight,
            .homotopicRelevance = false,
        };
 
        return {intimate, personal};
    }
 
 
} // namespace armarx::navigation::human