MoveTCPToTargetPose.cpp

Go to the documentation of this file.

#include <SimoxUtility/algorithm/string/string_tools.h>
#include "MoveTCPToTargetPose.h"
 
#include <VirtualRobot/MathTools.h>
 
#include "util/TCPControlUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        tcp_control::arondto::MoveTCPToTargetPoseAcceptedType
        GetDefaultParameterization()
        {
            tcp_control::arondto::MoveTCPToTargetPoseAcceptedType ret;
            ret.orientationalAccuracy = 0.01;
            ret.positionalAccuracy = 25;
            return ret;
        }
    } // namespace
 
    SkillDescription MoveTCPToTargetPose::Description = skills::SkillDescription{
        .skillId = {.skillName = "MoveTCPToTargetPose"},
        .description = "Move the TCP to a target pose",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(15000),
        .parametersType = tcp_control::arondto::MoveTCPToTargetPoseAcceptedType::ToAronType(),
    };
 
    MoveTCPToTargetPose::MoveTCPToTargetPose(armem::client::MemoryNameSystem& mns,
                                             armarx::viz::Client& arviz,
                                             TCPControlSkillContext& context) :
        TCPControlSkill(mns, arviz, Description.skillId.skillName, context),
        mixin::RobotReadingSkillMixin(),
        SimplePeriodicSpecializedSkill<tcp_control::arondto::MoveTCPToTargetPoseAcceptedType>(
            Description,
            armarx::core::time::Frequency::Hertz(100))
    {
    }
 
    Skill::InitResult
    MoveTCPToTargetPose::init(const SpecializedInitInput& in)
    {
        robotReader.connect(mns);
 
        tcpControlUnitRequestedOnStart = true;
        if (!context.tcpControlUnitPrx->isRequested())
        {
            tcpControlUnitRequestedOnStart = false;
            context.tcpControlUnitPrx->request();
        }
        context.tcpControlUnitPrx->setCycleTime(100);
 
        return {.status = TerminatedSkillStatus::Succeeded};
    }
 
    PeriodicSkill::StepResult
    MoveTCPToTargetPose::step(const SpecializedMainInput& in)
    {
        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};
        }
 
        std::string handRootNodeName = "Hand L Base";
        std::string robotAPIHandFileName = "RobotAPI/robots/Armar3/ArmarIII-LeftHand.xml";
        if (simox::alg::starts_with(in.parameters.kinematicChainName, "Right"))
        {
            handRootNodeName = "Hand R Base";
            robotAPIHandFileName = "RobotAPI/robots/Armar3/ArmarIII-RightHand.xml";
        }
 
        std::string tcpName =
            robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
 
        const Eigen::Matrix4f tcpPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
        const Eigen::Matrix4f handRootPoseGlobal =
            robot->getRobotNode(handRootNodeName)->getGlobalPose();
        const Eigen::Matrix4f tcpInHandRoot = handRootPoseGlobal.inverse() * tcpPoseGlobal;
 
        // get current pose of tcp
        auto currentTCPPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
 
        const auto tcpTargetPoseGlobal = in.parameters.targetPoseGlobal;
        const Eigen::Matrix4f handRootTargetPoseGlobal =
            tcpTargetPoseGlobal * tcpInHandRoot.inverse();
 
        const float baseSpeedFactor = 1.5;
        const float maxTranslationSpeed = baseSpeedFactor * 30;
        const float minTranslationSpeed = maxTranslationSpeed * 0.1; // 10% of max
        const float maxRotationSpeed = baseSpeedFactor * 0.7;
        const float minRotationSpeed = maxRotationSpeed * 0.1; // 10% of max
 
        // get translational difference
        Eigen::Vector3f diffPos =
            tcpTargetPoseGlobal.block<3, 1>(0, 3) - currentTCPPoseGlobal.block<3, 1>(0, 3);
        float diffPosError = diffPos.norm();
 
        float sig = 1 / (1 + pow(M_E, -diffPos.norm() / 5.f + 1.0)); // sigmoid
        diffPos = baseSpeedFactor * diffPos * sig;
 
        // Dont let it go to slow
        if (diffPosError < minTranslationSpeed && diffPosError != 0)
        {
            diffPos *= (minTranslationSpeed / diffPosError);
        }
 
        else if (diffPosError > maxTranslationSpeed && diffPosError != 0)
        {
            diffPos *= (maxTranslationSpeed / diffPosError);
        }
 
        if (diffPosError < in.parameters.positionalAccuracy)
        {
            diffPos = Eigen::Vector3f::Zero();
        }
 
        FramedDirection cartesianVelocity(diffPos, GlobalFrame, "");
 
        {
            auto l = arviz.layer(layerName);
            auto a = armarx::viz::Arrow("cartesianVelocity");
            a.pose(currentTCPPoseGlobal);
            a.direction(diffPos);
            a.length(diffPos.norm() * 3);
            l.add(a);
 
            auto b = armarx::viz::Sphere("targetPose");
            b.pose(tcpTargetPoseGlobal);
            b.radius(diffPos.norm());
            l.add(b);
 
            auto o = armarx::viz::Robot("grasp");
            o.file("RobotAPI", robotAPIHandFileName);
            o.pose(handRootTargetPoseGlobal);
            l.add(o);
 
            arviz.commit(l);
        }
 
        Eigen::Vector3f angles(0, 0, 0);
        //float axisRot = 0;
 
        // get rotational difference
        Eigen::Matrix3f diffRot = tcpTargetPoseGlobal.block<3, 3>(0, 0) *
                                  currentTCPPoseGlobal.block<3, 3>(0, 0).inverse();
        angles = VirtualRobot::MathTools::eigen3f2rpy(diffRot);
        float anglesError = angles.norm();
 
        ARMARX_IMPORTANT << VAROUT(angles);
 
        //rotation diff around one axis
        //Eigen::Matrix4f  diffPose = Eigen::Matrix4f::Identity();
        //diffPose.block<3, 3>(0, 0) = diffRot;
        //Eigen::Vector3f axis;
        //VirtualRobot::MathTools::eigen4f2axisangle(diffPose, axis, axisRot);
 
        // Check velocities
        if (anglesError < minRotationSpeed && anglesError != 0)
        {
            angles *= (minRotationSpeed / anglesError);
        }
 
        else if (anglesError > maxRotationSpeed && anglesError != 0)
        {
            angles *= (maxRotationSpeed / anglesError);
        }
 
        if (anglesError < in.parameters.orientationalAccuracy)
        {
            angles = Eigen::Vector3f::Zero();
        }
 
        FramedDirection angleVelocity(angles, GlobalFrame, "");
 
        //    // make sure we don't move the hand too low
        //    if (handPoseFromKinematicModelEigen(2,3) < 1000 && cartesianVelocity->z < 0)
        //    {
        //        ARMARX_IMPORTANT << "Hand too low, moving it up a bit";
        //        if (handPoseFromKinematicModelEigen(2,3) < 950) cartesianVelocity->z = 0.1f*maxSpeed;
        //        else cartesianVelocity->z = 0.05f*maxSpeed;
        //    }
 
        auto cartesianError = diffPosError;
        auto angleError = anglesError; // axisRot
 
        ARMARX_INFO << deactivateSpam(0.5) << "Distance to target: "
                    << (std::to_string(cartesianError) + " mm, " +
                        std::to_string(angleError * 180 / M_PI) + " deg")
                    << ", \n"
                    << "Elapsed time: "
                    << (armarx::core::time::DateTime::Now() - started).toMilliSeconds() << "\n"
                    << "Current Thresholds: " << in.parameters.positionalAccuracy << "mm,  "
                    << in.parameters.orientationalAccuracy * 180.0f / M_PI << "deg";
 
        ARMARX_INFO << deactivateSpam(0.5) << "Request to move TCP: angle vel: (" << angleVelocity.x
                    << ", " << angleVelocity.y << ", " << angleVelocity.z << ")\n"
                    << "catesian vel: " << cartesianVelocity.x << ", " << cartesianVelocity.y
                    << ", " << cartesianVelocity.z << ")";
 
        if (angleError <= in.parameters.orientationalAccuracy &&
            cartesianError <= in.parameters.positionalAccuracy)
        {
            return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
        }
 
        context.tcpControlUnitPrx->setTCPVelocity(in.parameters.kinematicChainName,
                                                  tcpName,
                                                  new FramedDirection(cartesianVelocity),
                                                  new FramedDirection(angleVelocity));
        return {ActiveOrTerminatedSkillStatus::Running, nullptr};
    }
 
    Skill::ExitResult
    MoveTCPToTargetPose::exit(const SpecializedExitInput& in)
    {
        clearLayer();
 
        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 exit method.";
            return {.status = TerminatedSkillStatus::Failed};
        }
 
        std::string tcpName =
            robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
 
        stopTCPMovement(in.parameters.kinematicChainName, tcpName);
 
        if (!tcpControlUnitRequestedOnStart)
        {
            context.tcpControlUnitPrx->release();
        }
 
        return {.status = TerminatedSkillStatus::Succeeded};
    }
} // namespace armarx::skills