GraspTableVisualServo.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::GraspTableVisualServoGroup
 * @author     [Author Name] ( [Author Email] )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "GraspTableVisualServo.h"
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 
using namespace armarx;
using namespace CoupledInteractionGroup;
 
// DO NOT EDIT NEXT LINE
GraspTableVisualServo::SubClassRegistry
    GraspTableVisualServo::Registry(GraspTableVisualServo::GetName(),
                                    &GraspTableVisualServo::CreateInstance);
 
GraspTableVisualServo::GraspTableVisualServo(XMLStateConstructorParams stateData) :
    XMLStateTemplate<GraspTableVisualServo>(stateData),
    GraspTableVisualServoGeneratedBase<GraspTableVisualServo>(stateData)
{
}
 
void
GraspTableVisualServo::onEnter()
{
 
    CoupledInteractionGroupStatechartContext* context =
        getContext<CoupledInteractionGroupStatechartContext>();
    // set first view directions manually for initial localization of hands and objects
    FramedPositionPtr viewTarget;
    std::string frameName =
        context->getRobotStateComponent()->getSynchronizedRobot()->getRootNode()->getName();
    std::string agentName = context->getRobotStateComponent()->getSynchronizedRobot()->getName();
    Eigen::Vector3f targetPos;
    targetPos(0) = 0;
    targetPos(1) = 700;
    targetPos(2) = 1100;
    viewTarget = new FramedPosition(targetPos, frameName, agentName);
    context->getViewSelection()->addManualViewTarget(viewTarget);
    targetPos(0) = 300;
    targetPos(1) = 500;
    targetPos(2) = 1100;
    viewTarget = new FramedPosition(targetPos, frameName, agentName);
    context->getViewSelection()->addManualViewTarget(viewTarget);
    targetPos(0) = 0;
    targetPos(1) = 700;
    targetPos(2) = 1100;
    viewTarget = new FramedPosition(targetPos, frameName, agentName);
    context->getViewSelection()->addManualViewTarget(viewTarget);
    targetPos(0) = -300;
    targetPos(1) = 500;
    targetPos(2) = 1100;
    viewTarget = new FramedPosition(targetPos, frameName, agentName);
    context->getViewSelection()->addManualViewTarget(viewTarget);
 
    local.setStartTimeRef(ChannelRefPtr::dynamicCast(
        context->systemObserverPrx->startTimer("GraspTableVisualServoStartTime")));
    Eigen::Matrix4f tablePose;
    tablePose << 1, 0, 0, 0, 0, 1, 0, 700, 0, 0, 1, 950, 0, 0, 0, 1;
    Eigen::Matrix4f rightTcpTargetPose;
    Eigen::Matrix4f rightPreTcpTargetPose;
    /*rightTcpTargetPose << 0.243387, -0.965925, 0.0880421, 365.426,
    -0.342303, -0.000609159, 0.939589, 510.511,
    -0.907519, -0.25882, -0.330787, 905.272,
    0, 0, 0, 1;*/
    /*rightTcpTargetPose << 0.0404131,  -0.641525,  -0.766036,    297.815,
    -0.0199878,  -0.767028,   0.641302,    400.0,
    -0.998983, -0.0106055, -0.0438203,    in.getTableHeight(),
            0,          0,          0,          1;*/
    /*rightTcpTargetPose << 0.53654, -0.178574,  -0.824764,  280.355,
     0.253516,  -0.898098,  0.359374,   384.811,
    -0.804894,  -0.401909, -0.436594,    in.getTableHeight(),
            0,         0,         0,         1;*/
    /*rightTcpTargetPose << 0.19216,  -0.838976,   -0.509109,   282.275,
    0.0409204,   -0.511477,   0.858322,     542.37,
    -0.98051,   -0.185769, -0.0639541,    988.324,
           0,          0,          0,          1;*/
    rightTcpTargetPose << 0.56283, -0.354918, -0.746496, 0.5 * in.getTableWidth(), 0.0284304,
        -0.910899, 0.411648, 439.878, -0.826084, -0.210465, -0.522772, in.getTableHeight(), 0, 0, 0,
        1;
 
    Eigen::Matrix4f leftTcpTargetPose;
    Eigen::Matrix4f leftPreTcpTargetPose;
    /*leftTcpTargetPose << -0.24339, -0.965924, -0.0880399, -365.426,
    -0.34231, 0.000614762, 0.939587, 510.507,
    -0.907516, 0.258823, -0.330795, 905.269,
    0, 0, 0, 1;*/
    /*leftTcpTargetPose << -0.0404158,  -0.641524,   0.766038,   -297.815,
    -0.0199951,    0.76703,     0.6413,    400.0,
     -0.998983,  0.0106016, -0.0438274,    in.getTableHeight(),
             0,          0,          0,          1;*/
    /*leftTcpTargetPose << -0.19216,  -0.838976,   0.509109,   -282.275,
    0.0409204,   0.511477,   0.858322,     542.37,
    -0.98051,   0.185769, -0.0639541,    988.324,
           0,          0,          0,          1;*/
    /*leftTcpTargetPose << -0.53654, -0.178574,  0.824764,  -280.355,
     0.253516,  0.898098,  0.359374,   384.811,
    -0.804894,  0.401909, -0.436594,    in.getTableHeight(),
            0,         0,         0,         1;*/
    leftTcpTargetPose << -0.56283, -0.354918, 0.746496, -0.5 * in.getTableWidth(), -0.0284304,
        0.910899, 0.411648, 439.878, -0.826084, 0.210465, -0.522772, in.getTableHeight(), 0, 0, 0,
        1;
 
    leftPreTcpTargetPose = leftTcpTargetPose;
    rightPreTcpTargetPose = rightTcpTargetPose;
    leftPreTcpTargetPose(0, 3) -= 150.0;
    rightPreTcpTargetPose(0, 3) += 150.0;
    leftPreTcpTargetPose(1, 3) -= 50.0;
    rightPreTcpTargetPose(1, 3) -= 50.0;
    ARMARX_IMPORTANT << "in GraspTableState robot global pose:"
                     << context->getRobot()->getGlobalPose();
    FramedPose tableFramedPoseBase =
        FramedPose(tablePose, std::string("Armar3_Base"), std::string("Armar3"));
    ;
    FramedPose leftTcpTargetFramedPose =
        FramedPose(leftTcpTargetPose, std::string("Armar3_Base"), std::string("Armar3"));
    FramedPose leftPreTcpTargetFramedPose =
        FramedPose(leftPreTcpTargetPose, std::string("Armar3_Base"), std::string("Armar3"));
    ;
    FramedPose rightTcpTargetFramedPose =
        FramedPose(rightTcpTargetPose, std::string("Armar3_Base"), std::string("Armar3"));
    ;
    FramedPose rightPreTcpTargetFramedPose =
        FramedPose(rightPreTcpTargetPose, std::string("Armar3_Base"), std::string("Armar3"));
    ;
    ARMARX_INFO << "leftTcpTargetGlobal "
                << leftTcpTargetFramedPose.toGlobal(context->getRobot())->toEigen();
    //TODO calculate these poses
    local.setFramedTablePose(tableFramedPoseBase);
    local.setLeftTcpPreTargetPose(leftPreTcpTargetFramedPose);
    local.setLeftTcpTargetPose(leftTcpTargetFramedPose);
    local.setRightTcpPreTargetPose(rightPreTcpTargetFramedPose);
    local.setRightTcpTargetPose(rightTcpTargetFramedPose);
}
 
void
GraspTableVisualServo::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
GraspTableVisualServo::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void
GraspTableVisualServo::onExit()
{
    CoupledInteractionGroupStatechartContext* context =
        getContext<CoupledInteractionGroupStatechartContext>();
    context->systemObserverPrx->removeTimer(local.getStartTimeRef());
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
GraspTableVisualServo::GetName()
{
    return "GraspTableVisualServo";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
GraspTableVisualServo::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new GraspTableVisualServo(stateData));
}