MovePlatformToPose.cpp

Go to the documentation of this file.

#include "MovePlatformToPose.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "util/PlatformUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        platform_control::arondto::MovePlatformToPoseAcceptedType
        GetDefaultParameterization()
        {
            platform_control::arondto::MovePlatformToPoseAcceptedType ret;
            ret.orientationalAccuracy = 0.1;
            ret.positionalAccuracy = 100;
            return ret;
        }
    } // namespace
 
    SkillDescription MovePlatformToPose::Description = skills::SkillDescription{
        "MovePlatformToPosition",
        "Move a platform unit to target pos.",
        {},
        armarx::Duration::MilliSeconds(30000),
        platform_control::arondto::MovePlatformToPoseAcceptedType::ToAronType(),
        GetDefaultParameterization().toAron()};
 
    MovePlatformToPose::MovePlatformToPose(armem::client::MemoryNameSystem& mns,
                                           armarx::viz::Client& arviz,
                                           PlatformControlSkillContext& context) :
        PlatformControlSkill(mns, arviz, Description.skillName, context),
        mixin::RobotReadingSkillMixin(mns),
        PeriodicSimpleSpecializedSkill<ArgType>(Description,
                                                armarx::core::time::Frequency::Hertz(10))
    {
    }
 
    Skill::InitResult
    MovePlatformToPose::init(const SpecializedInitInput& in)
    {
        // setup memory reader
        robotReader.connect();
 
        return {.status = TerminatedSkillStatus::Succeeded};
    }
 
    Skill::ExitResult
    MovePlatformToPose::exit(const SpecializedExitInput& in)
    {
        stopPlatformMovement();
        clearLayer();
 
        return {.status = TerminatedSkillStatus::Succeeded};
    }
 
    PeriodicSkill::StepResult
    MovePlatformToPose::step(const SpecializedMainInput& in)
    {
        clearLayer();
 
        {
            auto l = arviz.layer(layerName);
            auto a = armarx::viz::Sphere("MovePlatformToPose::targetPose");
            a.pose(in.parameters.pose);
            a.radius(40);
            a.color(viz::Color::yellow());
            l.add(a);
            arviz.commit(l);
        }
 
        auto robot =
            robotReader.getSynchronizedRobot(in.parameters.robotName,
                                             armem::Time::Now(),
                                             VirtualRobot::RobotIO::RobotDescription::eStructure);
        if (!robot)
        {
            ARMARX_WARNING << "Unable to get robot from robotstatememory. Abort.";
            return {ActiveOrTerminatedSkillStatus::Failed, nullptr};
        }
 
        FramedPosePtr robotPose = new FramedPose(robot->getRootNode()->getPoseInRootFrame(),
                                                 robot->getRootNode()->getName(),
                                                 robot->getName());
        auto robotPoseGlobalEigen = robotPose->toGlobalEigen(robot);
 
        float targetX = in.parameters.pose(0, 3);
        float targetY = in.parameters.pose(1, 3);
 
        Eigen::Matrix3f targetRotMat = in.parameters.pose.block<3, 3>(0, 0);
        Eigen::Vector3f targetRotMatX = targetRotMat * Eigen::Vector3f::UnitX();
        float targetOri = atan2(targetRotMatX[1], targetRotMatX[0]);
        if (targetOri > M_PI) // normalize
        {
            targetOri = -2 * M_PI + targetOri;
        }
 
        float robotX = robotPoseGlobalEigen(0, 3);
        float robotY = robotPoseGlobalEigen(1, 3);
 
        Eigen::Matrix3f robotRotMat = robotPoseGlobalEigen.block<3, 3>(0, 0);
        Eigen::Vector3f robotRotMatX = robotRotMat * Eigen::Vector3f::UnitX();
        float robotOri = atan2(robotRotMatX[1], robotRotMatX[0]);
        if (robotOri > M_PI) // normalize
        {
            robotOri = -2 * M_PI + robotOri;
        }
 
        ARMARX_INFO << deactivateSpam() << "Platform current pose: (" << robotX << ", " << robotY
                    << ", " << robotOri << ")";
        ARMARX_INFO << deactivateSpam() << "Platform target: (" << targetX << ", " << targetY
                    << ", " << targetOri << ")";
 
        float translationalError =
            (robotPoseGlobalEigen.block<3, 1>(0, 3) - Eigen::Vector3f(targetX, targetY, 0)).norm();
        float orientationalError = targetOri - robotOri;
 
        // transform alpha to [-pi, pi)
        while (orientationalError < -M_PI)
        {
            orientationalError += 2 * M_PI;
        }
 
        while (orientationalError >= M_PI)
        {
            orientationalError -= 2 * M_PI;
        }
 
        ARMARX_INFO << deactivateSpam() << "Platform target position error: (" << translationalError
                    << ")";
        ARMARX_INFO << deactivateSpam() << "Platform target orientation error: ("
                    << orientationalError << ")";
 
        if (translationalError > in.parameters.positionalAccuracy or
            orientationalError > in.parameters.orientationalAccuracy)
        {
            ARMARX_INFO << deactivateSpam() << "Platform position accuracy ("
                        << in.parameters.positionalAccuracy << ")";
            ARMARX_INFO << deactivateSpam() << "Platform orientation accuracy: ("
                        << in.parameters.orientationalAccuracy << ")";
            context.platformUnitPrx->moveTo(targetX,
                                            targetY,
                                            targetOri,
                                            in.parameters.positionalAccuracy * 0.8,
                                            in.parameters.orientationalAccuracy * 0.8);
            return {ActiveOrTerminatedSkillStatus::Running, nullptr};
        }
        return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
    }
} // namespace armarx::skills