CartesianPositionControlVerification.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     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CartesianPositionControlVerification.h"
 
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/RobotStatechartContext.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include "../MotionControlGroupStatechartContext.h"
 
using namespace armarx;
using namespace MotionControlGroup;
 
// DO NOT EDIT NEXT LINE
CartesianPositionControlVerification::SubClassRegistry
    CartesianPositionControlVerification::Registry(
        CartesianPositionControlVerification::GetName(),
        &CartesianPositionControlVerification::CreateInstance);
 
CartesianPositionControlVerification::CartesianPositionControlVerification(
    XMLStateConstructorParams stateData) :
    XMLStateTemplate<CartesianPositionControlVerification>(stateData),
    CartesianPositionControlVerificationGeneratedBase<CartesianPositionControlVerification>(
        stateData)
{
}
 
void
CartesianPositionControlVerification::onEnter()
{
}
 
void
CartesianPositionControlVerification::run()
{
    MotionControlGroupStatechartContext* context =
        getContext<MotionControlGroupStatechartContext>();
 
    std::string kinChainName = in.getKinematicChainName();
    float positionTolerance = in.getTcpPositionTolerance();
    float orientationTolerance = in.getTcpOrientationTolerance();
 
    VirtualRobot::RobotPtr robot = context->getRobot()->clone();
 
    if (!robot)
    {
        ARMARX_WARNING << "Could not create robot" << flush;
        emitFailure();
    }
 
    if (!robot->hasRobotNodeSet(kinChainName))
    {
        ARMARX_WARNING << "Kinematic Chain '" << kinChainName << "' not available" << flush;
        emitFailure();
    }
 
    FramedPositionPtr framedPosition;
    FramedOrientationPtr framedOrientation;
 
    float tcpPositionDistanceToTarget = 0;
    float tcpOrientationDistanceToTarget = 0;
 
    // Compute distance to position target (if set)
    if (isInputParameterSet("TcpTargetPosition"))
    {
        framedPosition = in.getTcpTargetPosition();
        tcpPositionDistanceToTarget =
            (framedPosition->toGlobal(robot)->toEigen() -
             robot->getRobotNodeSet(kinChainName)->getTCP()->getGlobalPose().block<3, 1>(0, 3))
                .norm();
    }
 
    // Compute distance to orientation target (if set)
    if (isInputParameterSet("TcpTargetOrientation"))
    {
        framedOrientation = in.getTcpTargetOrientation();
        auto globalOri = framedOrientation->toGlobal(robot);
 
        Eigen::Quaternionf targetOrientation(globalOri->toEigen());
        Eigen::Quaternionf currentOrientation(
            robot->getRobotNodeSet(kinChainName)->getTCP()->getGlobalPose().block<3, 3>(0, 0));
 
        tcpOrientationDistanceToTarget = currentOrientation.angularDistance(targetOrientation);
    }
 
    out.setTcpFinalPositionDistanceToTarget(tcpPositionDistanceToTarget);
    out.setTcpFinalOrientationDistanceToTarget(tcpOrientationDistanceToTarget);
 
    // Check goal condition
    if (in.getDontMoveIfAlreadyClose() && tcpPositionDistanceToTarget < positionTolerance &&
        tcpOrientationDistanceToTarget < orientationTolerance)
    {
        emitSuccess();
    }
    else
    {
        ARMARX_INFO << "Target not yet reached: position distance = " << tcpPositionDistanceToTarget
                    << " (Tolerance: " << positionTolerance
                    << "), orientation distance = " << tcpOrientationDistanceToTarget
                    << " (Tolerance: " << orientationTolerance << ")";
        emitTargetNotReached();
    }
}
 
void
CartesianPositionControlVerification::onBreak()
{
}
 
void
CartesianPositionControlVerification::onExit()
{
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
CartesianPositionControlVerification::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CartesianPositionControlVerification(stateData));
}