MovePlatformToPose.cpp

Go to the documentation of this file.

#include "MovePlatformToPose.h"
 
#include "util/PlatformUtil.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx::skills
{
    namespace
    {
        platform_control::arondto::MovePlatformToPoseAcceptedType GetDefaultParameterization()
        {
            platform_control::arondto::MovePlatformToPoseAcceptedType ret;
            ret.orientationalAccuracy = 0.1;
            ret.positionalAccuracy = 100;
            return ret;
        }
    }
 
    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};
    }
}