VisualServoTCPToTargetPose.cpp

Go to the documentation of this file.

#include "VisualServoTCPToTargetPose.h"
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/core/FramedOrientedPoint.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "util/VisualServoTCPControlUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        visual_servo_tcp_control::arondto::MoveTCPToTargetPoseAcceptedType
        GetDefaultParameterization()
        {
            visual_servo_tcp_control::arondto::MoveTCPToTargetPoseAcceptedType ret;
            ret.orientationalAccuracy = 0.01;
            ret.positionalAccuracy = 25;
            return ret;
        }
    } // namespace
 
    SkillDescription VisualServoToTargetPose::Description = skills::SkillDescription{
        .skillId = {.skillName = "VisualServoToTargetPose"},
        .description = "Visual Servo the TCP to a target pose",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(2000),
        .parametersType = visual_servo_tcp_control::arondto::MoveTCPToTargetPoseAcceptedType::ToAronType(),
    };
 
    VisualServoToTargetPose::VisualServoToTargetPose(armem::client::MemoryNameSystem& mns,
                                                     armarx::viz::Client& arviz,
                                                     VisualServoTCPControlSkillContext& context) :
        VisualServoTCPControlSkill(mns, arviz, Description.skillId.skillName, context),
        SimplePeriodicSpecializedSkill<
            visual_servo_tcp_control::arondto::MoveTCPToTargetPoseAcceptedType>(
            Description,
            armarx::core::time::Frequency::Hertz(10))
    {
    }
 
    Skill::InitResult
    VisualServoToTargetPose::init(const SpecializedInitInput& in)
    {
        robotReader.connect(mns);
        objectReader.connect(mns);
 
        tcpControlUnitRequestedOnStart = true;
        if (!context.tcpControlUnitPrx->isRequested())
        {
            tcpControlUnitRequestedOnStart = false;
            context.tcpControlUnitPrx->request();
        }
        context.tcpControlUnitPrx->setCycleTime(20);
 
        return {.status = TerminatedSkillStatus::Succeeded};
    }
 
    PeriodicSkill::StepResult
    VisualServoToTargetPose::step(const SpecializedMainInput& in)
    {
        return {ActiveOrTerminatedSkillStatus::Failed, nullptr};
 
        /*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 tcpName = robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
 
        const Eigen::Matrix4f tcpPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
        const Eigen::Matrix4f handRootPoseGlobal = robot->getRobotNode(in.parameters.handRootNodeName)->getGlobalPose();
        const Eigen::Matrix4f tcpInHandRoot = handRootPoseGlobal.inverse() * tcpPoseGlobal;
 
        ARMARX_CHECK(robotReader.synchronizeRobot(*robot, armem::Time::Now()));
 
        // get current pose of tcp
        auto currentTCPPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
 
        // get hand from memory
        auto handInstance = objectReader.queryObjectByObjectID(tcpName, armem::Time::Now());
        if (handInstance)
        {
            currentTCPPoseGlobal = handInstance->pose.objectPoseGlobal;
        }
 
 
        const auto tcpTargetPoseGlobal = in.parameters.targetPoseGlobal;
        const Eigen::Matrix4f handRootTargetPoseGlobal = tcpTargetPoseGlobal * tcpInHandRoot.inverse();
 
        const float baseSpeedFactor = 1.0;
        const float maxTranslationSpeed = baseSpeedFactor * 30;
        const float minTranslationSpeed = maxTranslationSpeed * 10;
        const float maxRotationSpeed = baseSpeedFactor * 0.7;
        const float minRotationSpeed = maxRotationSpeed * 0.1;
 
        // get translational difference
        Eigen::Vector3f diffPos = tcpTargetPoseGlobal.block<3, 1>(0, 3) - currentTCPPoseGlobal.block<3, 1>(0, 3);
 
        float sig = 1 / (1 + pow(M_E, -diffPos.norm() / 5.f + 1.0)); // sigmoid
        diffPos = baseSpeedFactor * diffPos * sig;
 
        // Check if target reached
        if (diffPos.norm() < in.parameters.positionalAccuracy)
        {
            diffPos = Eigen::Vector3f::Zero();
        }
 
        // Dont let it go to slow
        if (diffPos.norm() < minTranslationSpeed && diffPos.norm() != 0)
        {
            diffPos = diffPos * (minTranslationSpeed / diffPos.norm());
        }
 
        if (diffPos.norm() > maxTranslationSpeed && diffPos.norm() != 0)
        {
            diffPos = diffPos * (maxTranslationSpeed / diffPos.norm());
        }
 
        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", in.parameters.handFileName);
            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);
 
        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 if target reached
        if (angles.norm() < in.parameters.orientationalAccuracy)
        {
            angles = Eigen::Vector3f::Zero();
        }
 
        // Check velocities
        if (angles.norm() < minRotationSpeed && angles.norm() != 0)
        {
            angles *= minRotationSpeed / angles.norm();
        }
 
        if (angles.norm() > maxRotationSpeed && angles.norm() != 0)
        {
            angles *= maxRotationSpeed / angles.norm();
        }
 
        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 = diffPos.norm();
        auto angleError = angles.norm(); // 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";
 
        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
    VisualServoToTargetPose::exit(const SpecializedExitInput& in)
    {
        clearLayer();
 
        if (!tcpControlUnitRequestedOnStart)
        {
            context.tcpControlUnitPrx->release();
        }
 
        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);
 
        return {.status = TerminatedSkillStatus::Succeeded};
    }
} // namespace armarx::skills