CheckTargetPoseReached.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::CheckTargetPoseReachedGroup
 * @author     [Author Name] ( [Author Email] )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CheckTargetPoseReached.h"
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
using namespace armarx;
using namespace CoupledInteractionGroup;
 
// DO NOT EDIT NEXT LINE
CheckTargetPoseReached::SubClassRegistry CheckTargetPoseReached::Registry(CheckTargetPoseReached::GetName(), &CheckTargetPoseReached::CreateInstance);
 
 
 
CheckTargetPoseReached::CheckTargetPoseReached(XMLStateConstructorParams stateData) :
    XMLStateTemplate < CheckTargetPoseReached > (stateData), CheckTargetPoseReachedGeneratedBase < CheckTargetPoseReached > (stateData)
{
}
 
bool CheckTargetPoseReached::checkForceTorqueExceeded(std::string tcpName, FramedDirectionPtr refForces, FramedDirectionPtr refTorques, float forceThreshhold, float torqueThreshhold)
{
    CoupledInteractionGroupStatechartContext* context = getContext<CoupledInteractionGroupStatechartContext>();
    ARMARX_INFO << "here stop" << tcpName;
    DatafieldRefPtr force = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->getForceDatafield(tcpName));
    DatafieldRefPtr torque = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->getTorqueDatafield(tcpName));
    ARMARX_INFO << "here stop";
    FramedDirectionPtr curForce = force->getDataField()->get<FramedDirection>();
    ARMARX_INFO << "here stop1";
    curForce = FramedDirection::ChangeFrame(context->getRobot(), *curForce, "Armar3_Base");
    ARMARX_INFO << "here stop2";
    FramedDirectionPtr curTorque = torque->getDataField()->get<FramedDirection>();
    curTorque = FramedDirection::ChangeFrame(context->getRobot(), *curTorque, "Armar3_Base");
 
    float deviationForce = (curForce->toEigen() - refForces->toEigen()).norm();
    float deviationTorque = (curTorque->toEigen() - refTorques->toEigen()).norm();
    ARMARX_INFO << "dev Force Torque " << deviationForce << " " << deviationTorque;
 
    if (deviationForce > forceThreshhold && deviationTorque > torqueThreshhold)
    {
        return true;
    }
 
    return false;
 
}
 
void CheckTargetPoseReached::onEnter()
{
    //TODO implement check Get final Target
    //get current
    //check
    Eigen::Vector3f platformPose;
    CoupledInteractionGroupStatechartContext* context = getContext<CoupledInteractionGroupStatechartContext>();
    armarx::ChannelRefPtr poseRef = context->getChannelRef(context->getPlatformUnitObserverName(), "platformPose");
    platformPose(0) = poseRef->getDataField("positionX")->getFloat();
    platformPose(1) = poseRef->getDataField("positionY")->getFloat();
    platformPose(2) = poseRef->getDataField("rotation")->getFloat();
    FramedDirectionPtr tableTargetPose = in.getTableTargetPose();
    float distance = sqrt((platformPose(0) - tableTargetPose->x) * (platformPose(0) - tableTargetPose->x) + (platformPose(1) - tableTargetPose->y) * (platformPose(1) - tableTargetPose->y));
    float angleDistance = fabs(platformPose(2) - tableTargetPose->z);
    ARMARX_INFO << "Distance in CheckTargetPoseReached " << distance << " " << angleDistance;
    ARMARX_INFO << "Robot Pose " << platformPose(0) << " " << platformPose(1) << " " << platformPose(2);
    ARMARX_INFO << "Target Pose " << tableTargetPose->x << " " << tableTargetPose->y << " " << tableTargetPose->z;
    float distancetravelled = sqrt((platformPose(0) - in.getInitialPose()->x) * (platformPose(0) - in.getInitialPose()->x) + (platformPose(1) - in.getInitialPose()->y) * (platformPose(1) - in.getInitialPose()->y));
    ARMARX_INFO << "Distance travelled " << distancetravelled;
    std::string leftTcpName("TCP L");
    std::string rightTcpName("TCP R");
    Eigen::Vector3f nullVec;
    nullVec << 0, 0, 0;
    FramedDirectionPtr refForce = new FramedDirection(nullVec, "Armar3_Base", "Armar3");
    FramedDirectionPtr refTorque = new FramedDirection(nullVec, "Armar3_Base", "Armar3");
 
    if (checkForceTorqueExceeded(leftTcpName, refForce, refTorque, 8.0, 0.0) ||
        checkForceTorqueExceeded(rightTcpName, refForce, refTorque, 12.0, 0.0))
    {
        PlatformUnitInterfacePrx platformPrx = context->getPlatformUnitProxy();
        //platformPrx->move(tableTargetPose(0,3),relativeTarget(0),relativeTarget(1),relativeTarget(2));
        platformPrx->move(0, 0, 0);
 
        // If the platform didn't move too much, we emit a special signal (push affordance not verified)
        Eigen::Vector3f initial = in.getInitialPose()->toEigen();
        ARMARX_INFO << "Initial platform pose: " << initial;
        ARMARX_INFO << "Current platform pose: " << platformPose;
        ARMARX_INFO << "Distance travelled: " << (initial - platformPose).norm();
 
        if ((initial - platformPose).norm() < 500)
        {
            emitEvForcesTooHighNotReached();
        }
 
        emitEvTargetPoseReached();
    }
    else
    {
        emitEvTargetPoseNotReached();
    }
}
 
void CheckTargetPoseReached::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
 
}
 
void CheckTargetPoseReached::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void CheckTargetPoseReached::onExit()
{
 
    CoupledInteractionGroupStatechartContext* context = getContext<CoupledInteractionGroupStatechartContext>();
    context->getTCPControlUnit()->release();
    // put your user code for the exit point here
    // execution time should be short (<100ms)
 
}
 
// DO NOT EDIT NEXT FUNCTION
std::string CheckTargetPoseReached::GetName()
{
    return "CheckTargetPoseReached";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr CheckTargetPoseReached::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CheckTargetPoseReached(stateData));
}