CheckForcesChanged.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::CheckForcesChangedGroup
 * @author     [Author Name] ( [Author Email] )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CheckForcesChanged.h"
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 
using namespace armarx;
using namespace CoupledInteractionGroup;
 
// DO NOT EDIT NEXT LINE
CheckForcesChanged::SubClassRegistry
    CheckForcesChanged::Registry(CheckForcesChanged::GetName(),
                                 &CheckForcesChanged::CreateInstance);
 
CheckForcesChanged::CheckForcesChanged(XMLStateConstructorParams stateData) :
    XMLStateTemplate<CheckForcesChanged>(stateData),
    CheckForcesChangedGeneratedBase<CheckForcesChanged>(stateData)
{
}
 
bool
CheckForcesChanged::checkForceTorqueExceeded(std::string tcpName,
                                             FramedDirectionPtr refForces,
                                             FramedDirectionPtr refTorques,
                                             float forceThreshhold,
                                             float torqueThreshhold)
{
    CoupledInteractionGroupStatechartContext* context =
        getContext<CoupledInteractionGroupStatechartContext>();
 
    DatafieldRefPtr force = DatafieldRefPtr::dynamicCast(
        context->getForceTorqueUnitObserver()->getForceDatafield(tcpName));
    DatafieldRefPtr torque = DatafieldRefPtr::dynamicCast(
        context->getForceTorqueUnitObserver()->getTorqueDatafield(tcpName));
 
    FramedDirectionPtr curForce = force->getDataField()->get<FramedDirection>();
    curForce = FramedDirection::ChangeFrame(context->getRobot(), *curForce, "Armar3_Base");
    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();
 
    if (deviationForce > forceThreshhold && deviationTorque > torqueThreshhold)
    {
        return true;
    }
 
    return false;
}
 
void
CheckForcesChanged::onEnter()
{
    CoupledInteractionGroupStatechartContext* context =
        getContext<CoupledInteractionGroupStatechartContext>();
 
    //std::string leftTcpName("Wrist 2 L");// = in.getLeftHandName();
    //std::string rightTcpName ("Wrist 2 R");//= in.getRightHandName();
    std::string leftTcpName = in.getLeftHandName();
    std::string rightTcpName = in.getRightHandName();
    //leftTcpName = std::string("Wrist 2 L");
    //rightTcpName = std::string("Wrist 2 R");
    FramedDirectionPtr leftRefForce = in.getReferenceForcesLeftHand();
    FramedDirectionPtr leftRefTorque = in.getReferenceTorquesLeftHand();
    FramedDirectionPtr rightRefForce = in.getReferenceForcesRightHand();
    FramedDirectionPtr rightRefTorque = in.getReferenceTorquesRightHand();
    FramedPositionPtr rightInitPos = in.getInitialRightHandPosition();
    FramedPositionPtr leftInitPos = in.getInitialLeftHandPosition();
    float maxDisplacement = in.getMaxDisplacement();
    bool maxDisplacementReached = false;
    Eigen::Matrix4f leftTcpPoseBase =
        context->getRobot()->getRobotNode(in.getLeftHandName())->getPoseInRootFrame();
    Eigen::Matrix4f rightTcpPoseBase =
        context->getRobot()->getRobotNode(in.getRightHandName())->getPoseInRootFrame();
    float leftDisplacement =
        (leftTcpPoseBase(0, 3) - leftInitPos->x) * (leftTcpPoseBase(0, 3) - leftInitPos->x);
    leftDisplacement +=
        (leftTcpPoseBase(1, 3) - leftInitPos->y) * (leftTcpPoseBase(1, 3) - leftInitPos->y);
    leftDisplacement +=
        (leftTcpPoseBase(2, 3) - leftInitPos->z) * (leftTcpPoseBase(2, 3) - leftInitPos->z);
    leftDisplacement = fabs(leftTcpPoseBase(2, 3) - leftInitPos->z);
    float rightDisplacement =
        (rightTcpPoseBase(0, 3) - rightInitPos->x) * (rightTcpPoseBase(0, 3) - rightInitPos->x);
    rightDisplacement +=
        (rightTcpPoseBase(1, 3) - rightInitPos->y) * (rightTcpPoseBase(1, 3) - rightInitPos->y);
    rightDisplacement +=
        (rightTcpPoseBase(2, 3) - rightInitPos->z) * (rightTcpPoseBase(2, 3) - rightInitPos->z);
    rightDisplacement = fabs(rightTcpPoseBase(2, 3) - rightInitPos->z);
 
    if (leftDisplacement > maxDisplacement || rightDisplacement > maxDisplacement)
    {
        maxDisplacementReached = true;
    }
 
    ARMARX_INFO << "Place Table displacements " << leftDisplacement << " " << maxDisplacement << " "
                << rightDisplacement << " " << maxDisplacement;
    float minForceThreshold = in.getMinForceThreshhold();
    float velocityInmms = in.getVelocityInMillimetersSecond();
 
    if (maxDisplacementReached)
    {
        out.setRightHandStopped(in.getRightHandStopped());
        out.setLeftHandStopped(in.getLeftHandStopped());
        out.setLeftHandChecked(true);
        out.setRightHandChecked(true);
        Eigen::Vector3f handPositionVelocity;
        handPositionVelocity << 0, 0, 0;
        Eigen::Vector3f handOrientationVelocity;
        handOrientationVelocity << 0, 0, 0;
 
        out.setNewHandPositionVelocity(
            FramedDirection(handPositionVelocity, "Armar3_Base", "Armar3"));
        out.setNewHandOrientationVelocity(
            FramedDirection(handOrientationVelocity, "Armar3_Base", "Armar3"));
        emitEvNotChecked();
        ARMARX_INFO << "Max Displacement reached";
    }
    else
    {
        if (in.getLeftHandChecked())
        {
            ARMARX_INFO << "Checked Forces in Left Hand";
        }
 
        if (in.getRightHandChecked())
        {
            ARMARX_INFO << "Checked Forces in Right Hand";
        }
 
        if (in.getLeftHandStopped())
        {
            ARMARX_INFO << "Stopped Left Hand";
        }
 
        if (in.getLeftHandStopped())
        {
            ARMARX_INFO << "Stopped Right Hand";
        }
 
        if (!in.getLeftHandStopped() && !in.getLeftHandChecked())
        {
            out.setRightHandStopped(in.getRightHandStopped());
 
            if (in.getRightHandStopped())
            {
                out.setLeftHandChecked(false);
            }
            else
            {
                out.setLeftHandChecked(true);
            }
 
            out.setRightHandChecked(false);
 
            if (checkForceTorqueExceeded(
                    leftTcpName, leftRefForce, leftRefTorque, minForceThreshold, 0.0))
            {
                Eigen::Vector3f handPositionVelocity;
                handPositionVelocity << 0, 0, 0;
                Eigen::Vector3f handOrientationVelocity;
                handOrientationVelocity << 0, 0, 0;
 
                out.setNewHandPositionVelocity(
                    FramedDirection(handPositionVelocity, "Armar3_Base", "Armar3"));
                out.setNewHandOrientationVelocity(
                    FramedDirection(handOrientationVelocity, "Armar3_Base", "Armar3"));
                out.setLeftHandStopped(true);
 
                emitEvForcesLeftHandChanged();
            }
            else
            {
                Eigen::Vector3f handPositionVelocity;
                handPositionVelocity << 0, 0, -velocityInmms;
                Eigen::Vector3f handOrientationVelocity;
                handOrientationVelocity << 0, 0, 0;
                out.setNewHandPositionVelocity(
                    FramedDirection(handPositionVelocity, "Armar3_Base", "Armar3"));
                out.setNewHandOrientationVelocity(
                    FramedDirection(handOrientationVelocity, "Armar3_Base", "Armar3"));
                out.setLeftHandStopped(in.getLeftHandStopped());
                emitEvForcesLeftHandNotChanged();
            }
        }
        else if (!in.getRightHandStopped() && !in.getRightHandChecked())
        {
            out.setLeftHandStopped(in.getLeftHandStopped());
 
            if (in.getLeftHandStopped())
            {
                out.setRightHandChecked(false);
            }
            else
            {
                out.setRightHandChecked(true);
            }
 
            out.setLeftHandChecked(false);
 
            if (checkForceTorqueExceeded(
                    rightTcpName, rightRefForce, rightRefTorque, minForceThreshold, 0.0))
            {
                Eigen::Vector3f handPositionVelocity;
                handPositionVelocity << 0, 0, 0;
                Eigen::Vector3f handOrientationVelocity;
                handOrientationVelocity << 0, 0, 0;
 
                out.setNewHandPositionVelocity(
                    FramedDirection(handPositionVelocity, "Armar3_Base", "Armar3"));
                out.setNewHandOrientationVelocity(
                    FramedDirection(handOrientationVelocity, "Armar3_Base", "Armar3"));
                out.setRightHandStopped(true);
                emitEvForcesRightHandChanged();
            }
            else
            {
                Eigen::Vector3f handPositionVelocity;
                handPositionVelocity << 0, 0, -velocityInmms;
                Eigen::Vector3f handOrientationVelocity;
                handOrientationVelocity << 0, 0, 0;
                out.setNewHandPositionVelocity(
                    FramedDirection(handPositionVelocity, "Armar3_Base", "Armar3"));
                out.setNewHandOrientationVelocity(
                    FramedDirection(handOrientationVelocity, "Armar3_Base", "Armar3"));
                out.setRightHandStopped(in.getRightHandStopped());
                emitEvForcesRightHandNotChanged();
            }
        }
        else if ((in.getRightHandStopped() && in.getLeftHandStopped()))
        {
            out.setRightHandStopped(in.getRightHandStopped());
            out.setLeftHandStopped(in.getLeftHandStopped());
            out.setLeftHandChecked(true);
            out.setRightHandChecked(true);
            Eigen::Vector3f handPositionVelocity;
            handPositionVelocity << 0, 0, 0;
            Eigen::Vector3f handOrientationVelocity;
            handOrientationVelocity << 0, 0, 0;
 
            out.setNewHandPositionVelocity(
                FramedDirection(handPositionVelocity, "Armar3_Base", "Armar3"));
            out.setNewHandOrientationVelocity(
                FramedDirection(handOrientationVelocity, "Armar3_Base", "Armar3"));
            emitEvNotChecked();
            ARMARX_INFO << "Left and Right Hand stopped";
        }
    }
}
 
void
CheckForcesChanged::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
CheckForcesChanged::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void
CheckForcesChanged::onExit()
{
 
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
CheckForcesChanged::GetName()
{
    return "CheckForcesChanged";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
CheckForcesChanged::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CheckForcesChanged(stateData));
}