CalculateGazeIk.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     Manfred Kroehnert ( Manfred dot Kroehnert at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CalculateGazeIk.h"
 
#include <memory>
 
#include <Eigen/Eigen>
 
#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h> // IWYU pragma: keep
#include <VirtualRobot/XML/RobotIO.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include "MotionControlGroupStatechartContext.h"
 
using namespace armarx;
using namespace MotionControlGroup;
 
// DO NOT EDIT NEXT LINE
CalculateGazeIk::SubClassRegistry CalculateGazeIk::Registry(CalculateGazeIk::GetName(),
                                                            &CalculateGazeIk::CreateInstance);
 
CalculateGazeIk::CalculateGazeIk(XMLStateConstructorParams stateData) :
    XMLStateTemplate<CalculateGazeIk>(stateData),
    CalculateGazeIkGeneratedBase<CalculateGazeIk>(stateData)
{
}
 
void
CalculateGazeIk::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
}
 
void
CalculateGazeIk::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* context =
        getContext<MotionControlGroupStatechartContext>();
    VirtualRobot::RobotPtr robot = context->getRobotPool()->getRobot(); //->createLocalClone();
 
 
    while (!isRunningTaskStopped()) // stop run function if returning true
    {
        //        VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(context->getRobotStateComponent()->getRobotSnapshot("CalculateTCPPoseTime")));
        RemoteRobot::synchronizeLocalClone(robot, context->getRobotStateComponent());
 
        if (!robot)
        {
            ARMARX_WARNING << "No robot!";
            emitFailure();
        }
 
        VirtualRobot::RobotNodeSetPtr kinematicChain =
            robot->getRobotNodeSet(in.getKinematicChainName());
 
        if (!kinematicChain)
        {
            ARMARX_WARNING << "No kinematicChain with name " << in.getKinematicChainName();
            emitFailure();
        }
 
        std::string tcpName;
 
        if (in.isVirtualPrismaticJointNameSet())
        {
            tcpName = in.getVirtualPrismaticJointName();
        }
        else
        {
            tcpName = kinematicChain->getTCP()->getName();
        }
 
        VirtualRobot::RobotNodePrismaticPtr virtualJoint =
            std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(
                robot->getRobotNode(tcpName));
 
        if (!virtualJoint)
        {
            ARMARX_WARNING << "No virtualJoint with name '" << tcpName << "'";
            emitFailure();
        }
 
        VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
        ikSolver.setup(30, 50, 30);
        ikSolver.enableJointLimitAvoidance(true);
 
        Eigen::Matrix3f m = Eigen::Matrix3f::Identity();
        FramedPositionPtr targetPosition = in.getGazeTarget();
        FramedOrientationPtr targetOrientation =
            new FramedOrientation(m, targetPosition->frame, targetPosition->agent);
        FramedPosePtr target = new FramedPose(
            targetPosition, targetOrientation, targetPosition->frame, targetPosition->agent);
        ARMARX_INFO << "Target: " << target->output();
        Eigen::Matrix4f goalInRoot = target->toRootEigen(robot);
        Eigen::Vector3f targetPosRootFrame = goalInRoot.block<3, 1>(0, 3);
        Eigen::Matrix4f globalPose = target->toGlobalEigen(robot);
 
        // Visualize gaze target via debug drawer interface
        context->getDebugDrawerTopicProxy()->setPoseDebugLayerVisu(
            "gazeTarget", new FramedPose(globalPose, GlobalFrame, ""));
 
        ARMARX_INFO << "Calculating IK for target position in root frame " << targetPosRootFrame;
 
        if (!ikSolver.solve(globalPose.block<3, 1>(0, 3)))
        {
            ARMARX_WARNING << "IKSolver found no solution! ";
            emitFailure();
            return;
        }
        else
        {
            std::map<std::string, float> targetJointValues;
 
            for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
            {
                if (kinematicChain->getNode(i)->getName().compare(tcpName) != 0)
                {
                    targetJointValues[kinematicChain->getNode(i)->getName()] =
                        kinematicChain->getNode(i)->getJointValue();
                    ARMARX_INFO << " joint: " << kinematicChain->getNode(i)->getName()
                                << "   value: " << kinematicChain->getNode(i)->getJointValue();
                }
            }
 
            ARMARX_INFO << "number of joints to be set: " << targetJointValues.size();
            ARMARX_INFO << "setting output: targetJointValues";
            out.setTargetJointValues(targetJointValues);
            emitCalculationDone();
            return;
        }
    }
}
 
void
CalculateGazeIk::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
CalculateGazeIk::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CalculateGazeIk(stateData));
}