MoveTCPToTarget.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MoveTCPToTarget.h"
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include "MotionControlGroupStatechartContext.h"
 
using namespace armarx;
using namespace MotionControlGroup;
 
// DO NOT EDIT NEXT LINE
MoveTCPToTarget::SubClassRegistry MoveTCPToTarget::Registry(MoveTCPToTarget::GetName(),
                                                            &MoveTCPToTarget::CreateInstance);
 
Eigen::Vector3f
GetClosestPoint(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, const Eigen::Vector3f& p)
{
    Eigen::Vector3f dir = p2 - p1;
    return p1 - (p1 - p).dot(dir) * dir / dir.squaredNorm();
}
 
float
GetT(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, const Eigen::Vector3f& p)
{
    Eigen::Vector3f dir = p2 - p1;
    return (p - p1).dot(dir) / dir.squaredNorm();
}
 
Eigen::Vector3f
Get(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float t)
{
    Eigen::Vector3f dir = p2 - p1;
    return p1 + t * dir;
}
 
void
MoveTCPToTarget::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
}
 
void
MoveTCPToTarget::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>();
    bool wasAlreadyRequested = c->getTCPControlUnit()->isRequested();
    if (!wasAlreadyRequested)
    {
        c->getTCPControlUnit()->request();
    }
    std::string tcpName;
    if (in.isTCPNameSet())
    {
        tcpName = in.getTCPName();
    }
    else
    {
        tcpName = c->getRobot()->getRobotNodeSet(in.getKinematicChainName())->getTCP()->getName();
    }
    std::string kinematicChainName = in.getKinematicChainName();
    FramedPosePtr startPose =
        new FramedPose(c->getRobot()->getRobotNode(tcpName)->getPoseInRootFrame(),
                       c->getRobot()->getRootNode()->getName(),
                       c->getRobot()->getName());
    FramedPosePtr targetPose = in.getTargetPose();
    auto robot = c->getLocalRobot();
 
    FramedPosePtr globalTargetPose = targetPose->toGlobal(robot);
 
    Eigen::Vector3f startPositionInRootFrame = startPose->toEigen().block<3, 1>(0, 3);
    Eigen::Vector3f targetPositionInRootFrame =
        targetPose->toRootEigen(c->getRobot()->clone()).block<3, 1>(0, 3);
    //    out.setStartPose(FramedPosePtr::dynamicCast(startPose->clone()));
    auto sharedRobot = c->getRobotStateComponent()->getSynchronizedRobot();
    auto tcp = robot->getRobotNode(tcpName);
    float tcpVelocity = in.getTCPVelocity();
    //    Eigen::Vector3f tcpDirectionInRootFrame = in.getMoveDirection()->toRootEigen(robot).normalized();
    //    int sleepTimeMilliSec = int(1.0f / in.getControlFrequency() * 1000.0f);
    //    ARMARX_INFO << "Keeping orientation: " << keepOrientation;
    //    Eigen::Quaternionf deltaOri((startPose->toEigen().inverse() * in.getTargetPose()->toEigen()).block<3,3>(0,0));
    Eigen::Quaternionf startOri(startPose->toEigen().block<3, 3>(0, 0));
    Eigen::Quaternionf targetOri(in.getTargetPose()->toRootEigen(robot).block<3, 3>(0, 0));
    float tOffset = in.getTOffset();
    float ignoreOrientation = in.getIgnoreOrientationBeforeT();
    armarx::CycleUtil cycle(1000.f / in.getControlFrequency());
    auto toleratedAngleError = in.getToleratedOrientationDifference();
 
    float pOri = in.getKpOrientation();
    // 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);
 
        float t = GetT(
            startPositionInRootFrame, targetPositionInRootFrame, tcp->getPositionInRootFrame());
        float tPos = t + tOffset;
        Eigen::Quaternionf currentTargetOri = startOri.slerp(t, targetOri);
 
 
        FramedDirectionPtr orientationVel, positionVel;
 
        Eigen::Vector3f orientationDelta;
        FramedOrientationPtr TCPDeltaToCurrentTarget = new FramedOrientation(
            currentTargetOri.toRotationMatrix(), robot->getRootNode()->getName(), robot->getName());
        TCPDeltaToCurrentTarget->changeFrame(robot, tcpName);
        Eigen::Matrix4f orientationDelta4x4 = Eigen::Matrix4f::Identity();
        orientationDelta4x4.block<3, 3>(0, 0) = TCPDeltaToCurrentTarget->toEigen();
        VirtualRobot::MathTools::eigen4f2rpy(orientationDelta4x4, orientationDelta);
        orientationVel = new FramedDirection(orientationDelta * pOri, tcpName, robot->getName());
        orientationVel->changeFrame(robot, robot->getRootNode()->getName());
        Eigen::Vector3f axis;
        float angleError = 0;
        orientationDelta4x4.block<3, 3>(0, 0) = TCPDeltaToCurrentTarget->toEigen();
        VirtualRobot::MathTools::eigen4f2axisangle(orientationDelta4x4, axis, angleError);
 
 
        Eigen::Vector3f currentTargetPosition =
            Get(startPositionInRootFrame, targetPositionInRootFrame, tPos);
        float positionError = (currentTargetPosition - tcp->getPositionInRootFrame()).norm();
        c->getDebugDrawerTopicProxy()->setPoseVisu(
            "MoveTCPToTarget", "CurrentPose", new Pose(tcp->getGlobalPose()));
        c->getDebugDrawerTopicProxy()->setPoseVisu(
            "MoveTCPToTarget", "TargetPose", globalTargetPose);
        c->getDebugDrawerTopicProxy()->setSphereVisu(
            "MoveTCPToTarget",
            "CurrentTargetPose",
            FramedPosition(currentTargetPosition, robot->getRootNode()->getName(), robot->getName())
                .toGlobal(robot),
            DrawColor{1.0, 0, 0, 1},
            10);
 
        Eigen::Vector3f positionVelEigen =
            (currentTargetPosition - tcp->getPositionInRootFrame()).normalized() * tcpVelocity;
        positionVel = new FramedDirection(
            positionVelEigen, robot->getRootNode()->getName(), robot->getName());
 
        if (t >= 1)
        {
            emitSuccess();
            break;
        }
        ARMARX_INFO
            << deactivateSpam(1) << VAROUT(t) << VAROUT(positionError) << VAROUT(angleError)
            << VAROUT(
                   positionVel
                       ->output()); // << VAROUT(startPositionInRootFrame) << VAROUT(tcp->getPositionInRootFrame());
        orientationVel = t < ignoreOrientation ? FramedDirectionPtr() : orientationVel;
        if (angleError < toleratedAngleError)
        {
            orientationVel = nullptr;
        }
        c->getTCPControlUnit()->setTCPVelocity(
            kinematicChainName, tcpName, positionVel, orientationVel);
        cycle.waitForCycleDuration();
    }
    c->getDebugDrawerTopicProxy()->clearLayer("MoveTCPToTarget");
 
    FramedDirectionPtr zeroVelocity(new FramedDirection(0, 0, 0, tcpName, robot->getName()));
    c->getTCPControlUnit()->setTCPVelocity(kinematicChainName, tcpName, zeroVelocity, zeroVelocity);
    TimeUtil::MSSleep(100);
    if (!wasAlreadyRequested)
    {
        c->getTCPControlUnit()->release();
    }
}
 
//void MoveTCPToTarget::onBreak()
//{
//    // put your user code for the breaking point here
//    // execution time should be short (<100ms)
//}
 
void
MoveTCPToTarget::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
MoveTCPToTarget::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new MoveTCPToTarget(stateData));
}