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/NavigatingSkillHelper.h>
#include <armarx/navigation/skills/aron/MoveRelativePlanar.aron.generated.h>
#include <armarx/navigation/skills/aron/NavigatingSkillParams.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;
        defaultParams.navigatingSkillParams = NavigatingSkillHelper::DefaultSkillDescription();
        defaultParams.navigatingSkillParams.globalPlanningAlgorithm =
            arondto::GlobalPlanningAlgorithm::Point2Point;
        defaultParams.navigatingSkillParams.p2pCheckCollisionsAlongTrajectory = false;
 
        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 NavigatingSkillHelper::Properties& baseProperties,
                                           const NavigatingSkillHelper::Services& baseSrv,
                                           const Properties& properties,
                                           const Services& srv) :
        Base(baseProperties, baseSrv, DefaultSkillDescription()),
        properties{properties},
        services{srv}
    {
    }
 
    namespace
    {
 
        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.synchronizeRobotPose(*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);
        }
    } // namespace
 
    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