VisualServoTowardsTargetPoseTest.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::VisualServoGroup
 * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "VisualServoGroupStatechartContext.generated.h"
#include "VisualServoTowardsTargetPoseTest.h"
 
using namespace armarx;
using namespace VisualServoGroup;
 
 
#include <MemoryX/libraries/observers/ObjectMemoryObserver.h>
 
 
// DO NOT EDIT NEXT LINE
VisualServoTowardsTargetPoseTest::SubClassRegistry VisualServoTowardsTargetPoseTest::Registry(VisualServoTowardsTargetPoseTest::GetName(), &VisualServoTowardsTargetPoseTest::CreateInstance);
 
 
 
VisualServoTowardsTargetPoseTest::VisualServoTowardsTargetPoseTest(const XMLStateConstructorParams& stateData) :
    XMLStateTemplate<VisualServoTowardsTargetPoseTest>(stateData),  VisualServoTowardsTargetPoseTestGeneratedBase<VisualServoTowardsTargetPoseTest>(stateData)
{
}
 
void VisualServoTowardsTargetPoseTest::onEnter()
{
    VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
 
    std::string handName = in.getHandNameInMemory();
    ARMARX_IMPORTANT << "Get hand memory channel. Hand name = " << handName;
    ChannelRefBasePtr handMemoryChannel = context->getObjectMemoryObserver()->requestObjectClassRepeated(handName, 150, armarx::DEFAULT_VIEWTARGET_PRIORITY);
 
    if (handMemoryChannel)
    {
        handMemoryChannel->validate();
        local.setHandMemoryChannel(ChannelRefPtr::dynamicCast(handMemoryChannel));
    }
    else
    {
        ARMARX_ERROR << "Could not find hand in prior memory. handMemoryChannel is null: handName = " << handName;
    }
 
    sleep(1); // wait for hant to be localized
 
    Eigen::Vector3f targetPosition1(300, 650, 1050);
    Eigen::Vector3f targetPosition2(150, 800, 1050);
    //Eigen::Vector3f targetOrientationVector(-1,1,0);
    Eigen::Vector3f targetOrientationVector(0, 1, 0);
    targetOrientationVector.normalize();
 
    Eigen::Matrix3f targetOrientation = getOrientationFromApproachVector(targetOrientationVector);
    FramedPosePtr targetPose1 = new FramedPose(targetOrientation, targetPosition1, context->getRobot()->getRootNode()->getName(), context->getRobot()->getName());
    FramedPosePtr targetPose2 = new FramedPose(targetOrientation, targetPosition2, context->getRobot()->getRootNode()->getName(), context->getRobot()->getName());
 
    local.setTcpTargetPose1(targetPose1);
    local.setTcpTargetPose2(targetPose2);
 
 
    ARMARX_IMPORTANT << "Requesting TCPControlUnit";
    context->getTCPControlUnit()->request();
    ARMARX_IMPORTANT << "Requested TCPControlUnit";
 
    //ARMARX_IMPORTANT << "Activating Automatic View Selection";
    //context->getViewSelection()->activateAutomaticViewSelection();
 
    Eigen::Matrix4f viewTargetEigen = context->getRobot()->getRobotNode(in.getHandNameInRobotModel())->getPoseInRootFrame();
    viewTargetEigen(2, 3) += 50;
    FramedPositionPtr viewTarget = new FramedPosition(viewTargetEigen, context->getRobot()->getRootNode()->getName(), context->getRobot()->getName());
    ARMARX_IMPORTANT << "addManualViewTarget: " << *viewTarget;
    context->getViewSelection()->addManualViewTarget(viewTarget);
    context->getViewSelection()->addManualViewTarget(viewTarget);
    context->getViewSelection()->addManualViewTarget(viewTarget);
 
    sleep(7); // wait for IK to be solved
 
}
 
Eigen::Matrix3f VisualServoTowardsTargetPoseTest::getOrientationFromApproachVector(const Eigen::Vector3f& approachVector)
{
    Eigen::Vector3f z = approachVector; // TCP z: Direction of Hand
    Eigen::Vector3f y(0, 0, -1); // TCP y: Down for normal orientation
    Eigen::Vector3f x = y.cross(z); // calculate x for right hand coord system
    Eigen::Matrix3f orientation;
    orientation.block<3, 1>(0, 0) = x;
    orientation.block<3, 1>(0, 1) = y;
    orientation.block<3, 1>(0, 2) = z;
    return orientation;
}
 
void VisualServoTowardsTargetPoseTest::run()
{
    VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
 
    ChannelRefPtr handMemoryChannel = local.getHandMemoryChannel();
 
 
 
    int counter = 0;
 
    while (!isRunningTaskStopped()) // stop run function if returning true
    {
        memoryx::ChannelRefBaseSequence instances = context->getObjectMemoryObserver()->getObjectInstances(handMemoryChannel);
 
        if (instances.size() == 0)
        {
            ARMARX_WARNING << "No instances of the hand in the memory";
        }
        else
        {
            FramedPose handPoseFromKinematicModel(context->getRobot()->getRobotNode(in.getHandNameInRobotModel())->getPoseInRootFrame(),
                                                  context->getRobot()->getRootNode()->getName(),
                                                  context->getRobot()->getName());
 
            FramedPositionPtr position = ChannelRefPtr::dynamicCast(instances.front())->get<FramedPosition>("position");
            FramedOrientationPtr orientation = ChannelRefPtr::dynamicCast(instances.front())->get<FramedOrientation>("orientation");
            FramedPose handPoseFromMemoryX = FramedPose(orientation->toEigen(), position->toEigen(), position->getFrame(), position->agent);
 
            ARMARX_INFO << "handPoseFromKinematicModel: " << handPoseFromKinematicModel;
            ARMARX_INFO << "handPoseFromMemoryX: " << handPoseFromMemoryX;
        }
 
 
        // Manual view selection:
        if (counter % 30 == 0) // 30 * 100ms = 3s
        {
            Eigen::Matrix4f viewTargetEigen = context->getRobot()->getRobotNode(in.getHandNameInRobotModel())->getPoseInRootFrame();
            //viewTargetEigen(0,3) -= 250;
            FramedPositionPtr viewTarget = new FramedPosition(viewTargetEigen, context->getRobot()->getRootNode()->getName(), context->getRobot()->getName());
            ARMARX_IMPORTANT << "addManualViewTarget: " << *viewTarget;
            context->getViewSelection()->addManualViewTarget(viewTarget);
        }
 
        usleep(100000); // 100ms
    }
 
}
 
void VisualServoTowardsTargetPoseTest::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void VisualServoTowardsTargetPoseTest::onExit()
{
    VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
    ARMARX_IMPORTANT << "Releasing TCPControlUnit";
    context->getTCPControlUnit()->release();
}
 
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr VisualServoTowardsTargetPoseTest::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new VisualServoTowardsTargetPoseTest(stateData));
}