MoveRelativePlanar.cpp

Go to the documentation of this file.

#include "MoveRelativePlanar.h"
 
#include <optional>
#include <sstream>
#include <string>
 
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
 
#include <armarx/navigation/skills/aron/MoveRelativePlanar.aron.generated.h>
#include <armarx/navigation/skills/constants.h>
 
namespace armarx::navigation::skills
{
 
    armarx::skills::SkillDescription
    MoveRelativePlanar::DefaultSkillDescription()
    {
        ParamType defaultParams;
        defaultParams.translationCartesian = std::nullopt;
        defaultParams.translationPolar = std::nullopt;
        defaultParams.rotationRadians = std::nullopt;
        defaultParams.rotationDegrees = std::nullopt;
 
        static const std::string nl = "\n\n";
 
        std::stringstream desc;
        desc << "**Move the robot relatively to its current pose in the horizontal plane.**";
 
        desc << nl
             << "The parameters `translationCartesian` and `translationPolar` specify the "
                "translation of the robot. (If both are given, `translationCartesian` takes "
                "precedence over `translationPolar`.)";
 
        desc << nl
             << "`translationCartesian` specifies the relative translation in Cartesian"
                " coordinates (x, y). The corresponding coordinate system is specified by its "
                "`frameID`. If `frameID` is not given, the robot's root coordinate system is used."
                "To define a translation in the global frame, set `frame` to `Global` (and leave "
                "`agent` empty).";
 
        desc << nl
             << "`translationPolar` specifies the relative translation in polar coordinates"
                " (direction, distance), where"
             << nl << "- `direction = 0` moves the robot forward," << nl
             << "- `direction > 0` moves the robot to the left," << nl
             << "- `direction < 0` moves the robot to the right." << nl
             << " (Generally, `forward` is defined as the +y axis of the robot root frame.)";
 
        desc << nl
             << "`rotationRadians` and `rotationDegrees` specify the rotation around the +z axis"
                " (up). "
             << nl
             << "(If both are given, `rotationRadians` takes precedence over `rotationDegrees`.)"
             << nl << "- `directionDegrees = 0` does not turn the robot," << nl
             << "- `directionDegrees > 0` turns the robot to the left," << nl
             << "- `directionDegrees < 0` turns the robot to the right.";
 
        return armarx::skills::SkillDescription{
            .skillId = {.skillName = constants::skill_names::MoveRelativePlanar},
            .description = desc.str(),
            .rootProfileDefaults = defaultParams.toAron(),
            .timeout = armarx::core::time::Duration::Minutes(1),
            .parametersType = ParamType::ToAronType(),
        };
    }
 
    MoveRelativePlanar::MoveRelativePlanar(const Properties& properties, const Services& srv) :
        Base(
            Base::Services{
                .iceNavigator = srv.iceNavigator,
                .memoryNameSystem = srv.memoryNameSystem,
            },
            DefaultSkillDescription()),
        properties{properties},
        services{.robotReader = srv.robotReader}
    {
    }
 
    Eigen::Vector3f
    makeTranslation(const arondto::FramedCartesian2D& cartesian,
                    armem::robot_state::VirtualRobotReader& robotReader,
                    const std::string& defaultRobotName)
    {
        Eigen::Vector3f translation;
        translation << cartesian.translationCartesian.cast<float>(), 0;
 
        // Target translation must be given in root frame of robot.
        if (cartesian.frameID.has_value())
        {
            armarx::arondto::FrameID frameID = cartesian.frameID.value();
 
            if (frameID.agent.empty())
            {
                // If frame == armarx::GlobalFrame, the agent is ignored, so we can set it anyway.
                frameID.agent = defaultRobotName;
            }
 
            VirtualRobot::RobotPtr robot = robotReader.getRobot(frameID.agent);
            ARMARX_CHECK_NOT_NULL(robot)
                << "Robot " << QUOTED(frameID.agent) << " not found in robot reader";
 
            ARMARX_CHECK(robotReader.synchronizeRobot(*robot, armarx::Clock::Now()))
                << "Failed to synchronize robot " << QUOTED(frameID.agent);
 
            if (frameID.frame.empty())
            {
                frameID.frame = robot->getRootNode()->getName();
            }
 
            armarx::FramedDirection dir(translation, frameID.frame, frameID.agent);
 
            return dir.toRootFrame(robot)->toEigen();
        }
        else
        {
            // If not given, we fall back to the robot root frame => no transformation necessary.
            return translation;
        }
    }
 
    Eigen::Vector3f
    makeTranslation(const arondto::Polar& polar,
                    const Eigen::Vector3f forward,
                    const Eigen::Vector3f up)
    {
        const Eigen::AngleAxisf rotation(simox::math::deg_to_rad(polar.directionDegrees), up);
        const Eigen::Vector3f translation = polar.distanceMillimeters * (rotation * forward);
 
        return translation;
    }
 
    Eigen::AngleAxisf
    makeRotation(float rotationRadians, const Eigen::Vector3f up)
    {
        return Eigen::AngleAxisf(rotationRadians, up);
    }
 
    Eigen::Isometry3f
    MoveRelativePlanar::relativeTarget(const Base::SpecializedMainInput& in)
    {
        // Note: In theory, it depends on the robot.
        static const Eigen::Vector3f forward = Eigen::Vector3f::UnitY();
        static const Eigen::Vector3f up = Eigen::Vector3f::UnitZ();
 
        const ParamType& parameters = in.parameters;
 
        Eigen::Isometry3f relativeTarget = Eigen::Isometry3f::Identity();
 
        // Translation.
        if (parameters.translationCartesian.has_value())
        {
            relativeTarget.translation() = makeTranslation(parameters.translationCartesian.value(),
                                                           services.robotReader,
                                                           properties.robotName);
        }
        else if (parameters.translationPolar.has_value())
        {
            relativeTarget.translation() =
                makeTranslation(parameters.translationPolar.value(), forward, up);
        }
 
        // Rotation.
        std::optional<float> rotationRadians = parameters.rotationRadians;
        if (not rotationRadians.has_value() and parameters.rotationDegrees.has_value())
        {
            rotationRadians = simox::math::deg_to_rad(parameters.rotationDegrees.value());
        }
        if (rotationRadians.has_value())
        {
            relativeTarget.linear() =
                relativeTarget.linear() * makeRotation(rotationRadians.value(), up);
        }
 
        return relativeTarget;
    }
 
} // namespace armarx::navigation::skills