MoveTCPOnLine.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotSkillTemplates::MotionControlGroup
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MoveTCPOnLine.h"
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include "MotionControlGroupStatechartContext.h"
 
using namespace armarx;
using namespace MotionControlGroup;
 
// DO NOT EDIT NEXT LINE
MoveTCPOnLine::SubClassRegistry MoveTCPOnLine::Registry(MoveTCPOnLine::GetName(),
                                                        &MoveTCPOnLine::CreateInstance);
 
MoveTCPOnLine::MoveTCPOnLine(const XMLStateConstructorParams& stateData) :
    XMLStateTemplate<MoveTCPOnLine>(stateData), MoveTCPOnLineGeneratedBase<MoveTCPOnLine>(stateData)
{
}
 
void
MoveTCPOnLine::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
}
 
void
MoveTCPOnLine::run()
{
    // put your user code for the execution-phase here
    // runs in seperate thread, thus can do complex operations
    // should check constantly whether isRunningTaskStopped() returns true
    MotionControlGroupStatechartContext* c = getContext<MotionControlGroupStatechartContext>();
    auto robot = c->getLocalRobot();
    auto sharedRobot = c->getRobotStateComponent()->getSynchronizedRobot();
    RemoteRobot::synchronizeLocalClone(robot, sharedRobot);
    c->getTCPControlUnit()->request();
    std::string tcpName;
    if (in.isTCPNameSet())
    {
        tcpName = in.getTCPName();
    }
    else
    {
        tcpName = robot->getRobotNodeSet(in.getKinematicChainName())->getTCP()->getName();
    }
    auto tcp = robot->getRobotNode(tcpName);
    std::string kinematicChainName = in.getKinematicChainName();
    auto startTime = TimeUtil::GetTime();
    FramedPosePtr startPose = new FramedPose(tcp->getPoseInRootFrame(),
                                             c->getRobot()->getRootNode()->getName(),
                                             c->getRobot()->getName());
    Eigen::Vector3f startPositionInRootFrame = startPose->toEigen().block<3, 1>(0, 3);
    out.setStartPose(FramedPosePtr::dynamicCast(startPose->clone()));
 
    float tcpVelocity = in.getTCPVelocity();
    Eigen::Vector3f tcpDirectionInRootFrame =
        in.getMoveDirection()->toRootEigen(robot).normalized();
    int sleepTimeMilliSec = int(1.0f / in.getControlFrequency() * 1000.0f);
    bool keepOrientation = in.getKeepStartOrientation();
    ARMARX_INFO << "Keeping orientation: " << keepOrientation;
    // uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
    while (!isRunningTaskStopped()) // stop run function if returning true
    {
        RemoteRobot::synchronizeLocalClone(robot, sharedRobot);
 
        FramedDirectionPtr orientationVel, positionVel;
        if (keepOrientation)
        {
            Eigen::Vector3f orientationDelta;
            FramedPosePtr TCPDeltaToGoal = FramedPosePtr::dynamicCast(startPose->clone());
            TCPDeltaToGoal->changeFrame(robot, tcpName);
            VirtualRobot::MathTools::eigen4f2rpy(TCPDeltaToGoal->toEigen(), orientationDelta);
            orientationVel = new FramedDirection(orientationDelta * 10, tcpName, robot->getName());
            Eigen::Vector3f axis;
            float angleError = 0;
            VirtualRobot::MathTools::eigen4f2axisangle(TCPDeltaToGoal->toEigen(), axis, angleError);
            ARMARX_INFO
                << deactivateSpam(1)
                << VAROUT(
                       angleError); // << VAROUT(startPositionInRootFrame) << VAROUT(tcp->getPositionInRootFrame());
        }
        auto timePassed = (TimeUtil::GetTime() - startTime).toSecondsDouble();
 
        Eigen::Vector3f currentTargetPosition =
            startPositionInRootFrame + timePassed * tcpVelocity * tcpDirectionInRootFrame;
        positionVel = new FramedDirection(currentTargetPosition - tcp->getPositionInRootFrame(),
                                          robot->getRootNode()->getName(),
                                          robot->getName());
 
        float distanceTravelled = (tcp->getPositionInRootFrame() - startPositionInRootFrame).norm();
        ARMARX_INFO
            << deactivateSpam(1)
            << VAROUT(
                   distanceTravelled); // << VAROUT(startPositionInRootFrame) << VAROUT(tcp->getPositionInRootFrame());
        if (distanceTravelled >= in.getMaxDistance())
        {
            emitSuccess();
            usleep(100000);
            c->getTCPControlUnit()->release();
            break;
        }
        c->getTCPControlUnit()->setTCPVelocity(
            kinematicChainName, tcpName, positionVel, orientationVel);
        TimeUtil::MSSleep(sleepTimeMilliSec);
    }
 
    FramedDirectionPtr zeroVelocity(new FramedDirection(0, 0, 0, tcpName, robot->getName()));
    c->getTCPControlUnit()->setTCPVelocity(kinematicChainName, tcpName, zeroVelocity, zeroVelocity);
    TimeUtil::MSSleep(100);
    c->getTCPControlUnit()->release();
 
 
    {
        RemoteRobot::synchronizeLocalClone(robot, sharedRobot);
        float distanceTravelled = (tcp->getPositionInRootFrame() - startPositionInRootFrame).norm();
        ARMARX_INFO << "Final " << VAROUT(distanceTravelled);
        out.setDistanceTravelled(distanceTravelled);
 
        FramedPosePtr TCPDeltaToGoal = FramedPosePtr::dynamicCast(startPose->clone());
        TCPDeltaToGoal->changeFrame(robot, tcpName);
        Eigen::Vector3f axis;
        float angleError = 0;
        VirtualRobot::MathTools::eigen4f2axisangle(TCPDeltaToGoal->toEigen(), axis, angleError);
        ARMARX_INFO << deactivateSpam(1) << "Final angle error: " << angleError;
    }
}
 
void
MoveTCPOnLine::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
    waitForRunningTaskToFinish();
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
MoveTCPOnLine::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new MoveTCPOnLine(stateData));
}