SetTCPVelocity.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::GraspObjectGroup
 * @author     David ( david dot schiebener at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SetTCPVelocity.h"
 
#include <VirtualRobot/MathTools.h>
 
#include "VisualServoGroupStatechartContext.generated.h"
 
using namespace armarx;
using namespace VisualServoGroup;
 
 
// DO NOT EDIT NEXT LINE
SetTCPVelocity::SubClassRegistry SetTCPVelocity::Registry(SetTCPVelocity::GetName(),
                                                          &SetTCPVelocity::CreateInstance);
 
SetTCPVelocity::SetTCPVelocity(XMLStateConstructorParams stateData) :
    XMLStateTemplate<SetTCPVelocity>(stateData),
    SetTCPVelocityGeneratedBase<SetTCPVelocity>(stateData)
{
}
 
void
SetTCPVelocity::onEnter()
{
    bool tcpTargetPoseSet = in.isTcpTargetPoseSet();
    bool tcpTargetPositionSet = in.isTcpTargetPositionSet();
    bool tcpTargetOrientationSet = in.isTCPTargetOrientationSet();
    if (!tcpTargetPoseSet && !tcpTargetPositionSet)
    {
        ARMARX_ERROR << "Either TcpTargetPose or TcpTargetPosition has to be set for VisualServo, "
                        "but none is set.";
        emitFailure();
        return;
    }
 
    VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
    VirtualRobot::RobotPtr robot = context->getRobot();
 
    FramedPosePtr handPose = in.getHandPose();
    Eigen::Matrix4f handPoseEigen = handPose->toGlobal(robot)->toEigen();
 
 
    FramedPosePtr tcpTargetPose;
    Eigen::Matrix4f tcpTargetPoseEigen;
    if (tcpTargetPoseSet)
    {
        tcpTargetPose = in.getTcpTargetPose();
        tcpTargetPoseEigen = tcpTargetPose->toGlobal(robot)->toEigen();
    }
    else if (tcpTargetPositionSet && !tcpTargetOrientationSet)
    {
        tcpTargetPoseEigen = handPoseEigen;
        tcpTargetPoseEigen.block<3, 1>(0, 3) =
            in.getTcpTargetPosition()->toGlobal(robot)->toEigen();
        tcpTargetPose = new FramedPose(tcpTargetPoseEigen, GlobalFrame, robot->getName());
    }
    else if (tcpTargetPositionSet && tcpTargetOrientationSet)
    {
        tcpTargetPoseEigen.block<3, 3>(0, 0) =
            in.getTCPTargetOrientation()->toGlobal(robot)->toEigen();
        tcpTargetPoseEigen.block<3, 1>(0, 3) =
            in.getTcpTargetPosition()->toGlobal(robot)->toEigen();
        tcpTargetPose = new FramedPose(tcpTargetPoseEigen, GlobalFrame, robot->getName());
    }
 
    const float baseSpeedFactor = in.getBaseVelocityFactor(); // 0.8
    const float maxSpeed = baseSpeedFactor * in.getMaxVelocityTranslation(); // 40
    const float maxRotationSpeed =
        baseSpeedFactor * in.getMaxVelocityRotation(); //60.0f*M_PI/180.0f; // 15
    const float baseAngleRelativeSpeedFactor = 10;
 
 
    // get translational difference
    Eigen::Vector3f diffPos =
        tcpTargetPoseEigen.block<3, 1>(0, 3) - handPoseEigen.block<3, 1>(0, 3);
    ARMARX_INFO << in.getKinematicChainName() << " : difference to target " << diffPos;
    const float distanceToTarget = diffPos.norm();
 
    //    float speedFactor = baseSpeedFactor;
    //    if (speedFactor*distanceToTarget > maxSpeed) speedFactor = maxSpeed / distanceToTarget;
    ARMARX_VERBOSE << "TCP translation: " << diffPos;
 
    float sig = sigmoid(diffPos.norm(), 1.0);
    ARMARX_VERBOSE << "Input for sig: " << diffPos.norm() << " " << VAROUT(sig);
    diffPos = baseSpeedFactor * diffPos * sig;
    float minSpeedPerc = in.getMinimumSpeedPercentage();
    //    // Dont let it go to slow
    if (diffPos.norm() < maxSpeed * minSpeedPerc)
    {
        diffPos = diffPos.normalized() * (maxSpeed * minSpeedPerc);
    }
 
    if (diffPos.norm() > maxSpeed)
    {
        diffPos = diffPos.normalized() * (maxSpeed);
    }
 
    ARMARX_VERBOSE << "Speed: " << diffPos.norm();
    FramedDirectionPtr cartesianVelocity = new FramedDirection(diffPos, GlobalFrame, "");
 
    Eigen::Vector3f angles(0, 0, 0);
    std::stringstream distanceStr;
    distanceStr << distanceToTarget << " mm";
    float axisRot = 0;
    if (tcpTargetPoseSet || (tcpTargetPositionSet && tcpTargetOrientationSet))
    {
        // get rotational difference
        Eigen::Matrix3f diffRot =
            tcpTargetPoseEigen.block<3, 3>(0, 0) * handPoseEigen.block<3, 3>(0, 0).inverse();
        //    Eigen::Quaternionf handQuat(handPose->toEigen().block<3,3>(0,0));
        //    Eigen::Quaternionf targetQuat(tcpTargetPose->toEigen().block<3,3>(0,0));
        //    float rotErrorAngle = acos(2 * (handQuat.dot(targetQuat)) -1 );
        Eigen::Matrix4f diffRot4 = Eigen::Matrix4f::Identity();
        diffRot4.block<3, 3>(0, 0) = diffRot;
        VirtualRobot::MathTools::eigen4f2rpy(diffRot4, 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);
 
        distanceStr << ",  " << axisRot * 180 / M_PI << " deg";
 
        float angleRelativeSpeedFactor = baseAngleRelativeSpeedFactor;
        if (distanceToTarget < in.getPositionalAccuracy())
        {
            angleRelativeSpeedFactor *= 1.5f;
        }
 
        angles = angleRelativeSpeedFactor * baseSpeedFactor * angles;
        if (angles.norm() > maxRotationSpeed)
        {
            ARMARX_IMPORTANT << deactivateSpam(1) << "angle velo to high reducing from "
                             << angles.norm();
            angles *= maxRotationSpeed / angles.norm();
        }
    }
 
    FramedDirectionPtr angleVelocity = new FramedDirection(angles, GlobalFrame, "");
 
    std::string kinematicChainName = getInput<std::string>("KinematicChainName");
 
    ARMARX_VERBOSE << "Angle motion  (" << angles(0) << ", " << angles(1) << ", " << angles(2)
                   << ")";
 
    //    // 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;
    //    }
 
 
    setOutput("HandPose", handPose);
 
    float accuracyFactor = 1.0;
    float elapsed = in.getStartTimeRef()->getDataField("elapsedMs")->getInt();
    float timeout = in.getAccuracyIncreaseTimeout();
 
    if (timeout > 0 && elapsed < timeout)
    {
        accuracyFactor = 0.5 + pow(elapsed / timeout, 2);
    }
 
    accuracyFactor = std::min(1.0f, accuracyFactor);
    ARMARX_IMPORTANT << "Distance to target: " << distanceStr.str() << ", Elapsed time: " << elapsed
                     << "\nCurrent Thresholds: " << in.getPositionalAccuracy() * accuracyFactor
                     << "mm,  "
                     << (in.getOrientationalAccuracyRad() * accuracyFactor) * 180.0f / M_PI
                     << "deg";
 
 
    // visualize TCP target
    if (context->getWorkingMemory()
            ->getAgentInstancesSegment() /*&& elapsed/100 - (int)(elapsed/100) % 3 == 0*/)
    {
        // visualize Poses
        auto globalHandPose = handPose->toGlobal(robot);
        auto globalTargetPose = tcpTargetPose->toGlobal(robot);
        //robot->print();
        auto chainName = in.getKinematicChainName();
        context->getEntityDrawerTopic()->updateObjectPose(
            "VisualServoHandVisu", "tcpTarget_" + chainName, globalTargetPose);
        //        context->getDebugDrawerTopic()->setPoseVisu("VisualServo", "tcpTargetPose_" + chainName, globalTargetPose);
        context->getDebugDrawerTopic()->setSphereVisu("VisualServo",
                                                      "tcpTargetPoseAccuracy_" + chainName,
                                                      globalTargetPose->position,
                                                      DrawColor{1, 1, 0, 0.5},
                                                      in.getPositionalAccuracy() * accuracyFactor);
        context->getDebugDrawerTopic()->setLineVisu("VisualServo",
                                                    "Distance to target_" + chainName,
                                                    globalHandPose->position,
                                                    globalTargetPose->position,
                                                    5,
                                                    DrawColor{1, 1, 1, 1});
        context->getDebugDrawerTopic()->setTextVisu("VisualServo",
                                                    "Distance_" + chainName,
                                                    distanceStr.str(),
                                                    globalTargetPose->position,
                                                    DrawColor{1, 0, 0, 1},
                                                    15);
    }
 
 
    // stop when target reached
    if ((distanceToTarget < in.getPositionalAccuracy() * accuracyFactor &&
         axisRot < in.getOrientationalAccuracyRad() * accuracyFactor) ||
        (elapsed > in.getTimeout() && distanceToTarget < in.getPositionalAccuracy() * 3 &&
         axisRot < in.getOrientationalAccuracyRad() * 3))
    {
        ARMARX_IMPORTANT << "Target reached";
 
        stopMovement();
        emitTargetReached();
    }
    else if (elapsed > in.getTimeout())
    {
        ARMARX_ERROR << "Could not reach object in time " << in.getTimeout()
                     << "ms -  Remaining error: " << distanceToTarget << "mm and "
                     << axisRot * 180 / M_PI << "deg";
        stopMovement();
        emitTimeout();
    }
    else
    {
        IceUtil::Time start = IceUtil::Time::now();
        context->getTCPControlUnit()->setTCPVelocity(
            kinematicChainName, "", cartesianVelocity, angleVelocity);
        ARMARX_VERBOSE << "Time for SetHandVelocity - hand IK: "
                       << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
 
        emitTCPVelocitySet();
    }
}
 
void
SetTCPVelocity::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void
SetTCPVelocity::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
float
SetTCPVelocity::sigmoid(float t, float offset) const
{
    return 1 / (1 + pow(M_E, -t / 5.f + offset));
}
 
void
SetTCPVelocity::stopMovement()
{
    VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
    FramedDirectionPtr cartesianVelocity =
        new FramedDirection(Eigen::Vector3f::Zero(),
                            context->getRobot()->getRootNode()->getName(),
                            context->getRobot()->getName());
    FramedDirectionPtr angleVelocity =
        new FramedDirection(Eigen::Vector3f::Zero(),
                            context->getRobot()->getRootNode()->getName(),
                            context->getRobot()->getName());
 
    std::string kinematicChainName = getInput<std::string>("KinematicChainName");
 
    context->getTCPControlUnit()->setTCPVelocity(
        kinematicChainName, "", cartesianVelocity, angleVelocity);
 
    removeDebugVisu();
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
SetTCPVelocity::GetName()
{
    return "SetTCPVelocity";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
SetTCPVelocity::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new SetTCPVelocity(stateData));
}
 
void
SetTCPVelocity::removeDebugVisu()
{
    VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
    context->getDebugDrawerTopic()->removeTextDebugLayerVisu("Distance");
    context->getDebugDrawerTopic()->removeLineDebugLayerVisu("Distance to target");
    context->getDebugDrawerTopic()->removePoseDebugLayerVisu("tcpTargetPose");
    context->getDebugDrawerTopic()->removeSphereDebugLayerVisu("tcpTargetPoseAccuracy");
}