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));
}