RotateTowards.cpp

Go to the documentation of this file.

#include "RotateTowards.h"
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/convert/deg_to_rad.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
 
#include <armarx/navigation/skills/NavigatingSkillHelper.h>
#include <armarx/navigation/skills/constants.h>
#include <armarx/navigation/util/util.h>
 
namespace armarx::navigation::skills
{
 
    RotateTowards::RotateTowards(const NavigatingSkillHelper::Properties& helperProperties,
                                 const NavigatingSkillHelper::Services& helperSrv,
                                 const Properties& props,
                                 const Services& srv) :
        Base(helperProperties, helperSrv, DefaultSkillDescription()), srv_(srv), properties_(props)
    {
    }
 
    Eigen::Isometry3f
    RotateTowards::relativeTarget(const Base::SpecializedMainInput& in)
    {
        const auto robotPoseOpt =
            srv_.robotReader.queryGlobalPose(properties_.robotName, armarx::Clock::Now());
        ARMARX_CHECK(robotPoseOpt.has_value())
            << "Could not get global pose for robot " << QUOTED(properties_.robotName);
 
        const auto& robotPose = robotPoseOpt.value();
        const auto robotPoseIsometry = Eigen::Isometry3f{robotPose};
        auto newPose = robotPoseIsometry.inverse() *
                       Eigen::Isometry3f{navigation::util::getRobotPoseAtXOrientedTowardsY(
                           robotPose.translation(), in.parameters.targetPositionGlobal)};
 
        return newPose;
    }
 
    armarx::skills::SkillDescription
    RotateTowards::DefaultSkillDescription()
    {
        ParamType defaultParams;
        defaultParams.navigatingSkillParams = NavigatingSkillHelper::DefaultSkillDescription();
        defaultParams.targetPositionGlobal = Eigen::Vector3f{0.F, 0.F, 0.F};
        defaultParams.navigatingSkillParams.globalPlanningAlgorithm =
            arondto::GlobalPlanningAlgorithm::Point2Point;
 
        return armarx::skills::SkillDescription{
            .skillId = {.skillName = constants::skill_names::RotateTowards},
            .description = "Rotate so that the robot is facing the specified global position",
            .rootProfileDefaults = defaultParams.toAron(),
            .timeout = armarx::core::time::Duration::Minutes(1),
            .parametersType = ParamType::ToAronType()};
    }
} // namespace armarx::navigation::skills