CalculateApproachTablePose.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::CalculateApproachTablePoseGroup
 * @author     [Author Name] ( [Author Email] )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CalculateApproachTablePose.h"
#include <VirtualRobot/MathTools.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 
using namespace armarx;
using namespace CoupledInteractionGroup;
 
// DO NOT EDIT NEXT LINE
CalculateApproachTablePose::SubClassRegistry
    CalculateApproachTablePose::Registry(CalculateApproachTablePose::GetName(),
                                         &CalculateApproachTablePose::CreateInstance);
 
CalculateApproachTablePose::CalculateApproachTablePose(XMLStateConstructorParams stateData) :
    XMLStateTemplate<CalculateApproachTablePose>(stateData),
    CalculateApproachTablePoseGeneratedBase<CalculateApproachTablePose>(stateData)
{
}
 
float
CalculateApproachTablePose::getYawAngle(const Eigen::Matrix4f& pose) const
{
    Eigen::Vector3f rpy;
    VirtualRobot::MathTools::eigen4f2rpy(pose, rpy);
    return rpy[2];
}
 
void
CalculateApproachTablePose::onEnter()
{
    CoupledInteractionGroupStatechartContext* context =
        getContext<CoupledInteractionGroupStatechartContext>();
    float approachDistance = in.getApproachDistance();
    float finalDistance = in.getFinalDistance();
    FramedPosePtr tablePose = in.getTablePose();
    ARMARX_LOG << "Incoming Table Pose " << *tablePose;
    Eigen::Matrix4f approachoffset = Eigen::Matrix4f::Identity();
    approachoffset(0, 3) = approachDistance;
    Eigen::Matrix4f approachPoseGlobal =
        tablePose->toGlobal(context->getRobot())->toEigen() * approachoffset;
 
    Eigen::Matrix4f finaloffset = Eigen::Matrix4f::Identity();
    finaloffset(0, 3) = finalDistance;
    Eigen::Matrix4f finalPoseGlobal =
        tablePose->toGlobal(context->getRobot())->toEigen() * finaloffset;
 
    Eigen::Vector3f finalplatformPosVec(
        finalPoseGlobal(0, 3), finalPoseGlobal(1, 3), getYawAngle(finalPoseGlobal) + M_PI_2);
 
    Eigen::Vector3f approachplatformPosVec(approachPoseGlobal(0, 3),
                                           approachPoseGlobal(1, 3),
                                           getYawAngle(approachPoseGlobal) + M_PI_2);
 
    std::vector<Vector3Ptr> approachPoses;
    approachPoses.push_back(new Vector3(approachplatformPosVec));
    approachPoses.push_back(new Vector3(finalplatformPosVec));
    out.setApproachTablePose(approachPoses);
    emitEvApproachPoseCalculated();
 
 
    //  FramedDirectionPtr tablePose = in.getTablePose();
    //    //Convert from Robot Base Frame to global Base frame
 
 
    //  Eigen::Matrix4f approachLocal = Eigen::Matrix4f::Identity();
    //  approachLocal(0,3) = tablePose->x;
    //  approachLocal(1,3) = tablePose->y - approachDistance;
    //  Eigen::Matrix4f finalLocal = Eigen::Matrix4f::Identity();
    //  finalLocal(0,3) = tablePose->x;
    //  finalLocal(1,3) = tablePose->y - finalDistance;
 
    //  Eigen::Matrix4f globalApproachPose = context->getRobot()->getRootNode()->toGlobalCoordinateSystem(approachLocal);
    //  Eigen::Matrix4f globalFinalPose = context->getRobot()->getRootNode()->toGlobalCoordinateSystem(finalLocal);
 
    //  PosePtr poseApproach(new Pose(globalApproachPose));
    //  PosePtr poseFinal(new Pose(globalFinalPose));
    //  context->getDebugDrawerTopicProxy()->setPoseDebugLayerVisu("Approach Table", poseApproach);
    //  context->getDebugDrawerTopicProxy()->setPoseDebugLayerVisu("Final Table", poseFinal);
 
    //  /*
    //  Eigen::Matrix4f tableBaseTransform = Eigen::Matrix4f::Identity();
    //  tableBaseTransform(0,3) = tablePose->x;
    //  tableBaseTransform(1,3) = tablePose->y;
    //  tableBaseTransform(0,0) = cos(tablePose->z);
    //  tableBaseTransform(0,1) = -sin(tablePose->z);
    //  tableBaseTransform(1,1) = cos(tablePose->z);
    //  tableBaseTransform(1,0) = -sin(tablePose->z);
    //  Eigen::Matrix4f approachPoseTableTransform = Eigen::Matrix4f::Identity();
    //  approachPoseTableTransform(1,3) = -approachDistance;
    //  Eigen::Matrix4f finalPoseTableTransform = Eigen::Matrix4f::Identity();
    //  finalPoseTableTransform(1,3) = -finalDistance;
    //  Eigen::Matrix4f globalApproachPose = context->getRobot()->getRootNode()->getGlobalPose()*tableBaseTransform*approachPoseTableTransform;
    //  Eigen::Matrix4f globalFinalPose = context->getRobot()->getRootNode()->getGlobalPose()*tableBaseTransform*finalPoseTableTransform;*/
 
    //  ARMARX_INFO << "Approach and Final Distance " << approachDistance << " " << finalDistance;
    //  armarx::ChannelRefPtr poseRef = context->getChannelRef(context->getPlatformUnitObserverName(), "platformPose");
    //  //currentBaseVec(0) = poseRef->getDataField("positionX")->getFloat();
    //  //currentBaseVec(1) = poseRef->getDataField("positionY")->getFloat();
    //  //currentBaseVec(2) = poseRef->getDataField("rotation")->getFloat();
    //    ARMARX_INFO << "Approach Pose " << globalApproachPose;
    //    ARMARX_INFO << "Approach Pose " << globalFinalPose;
    //    Vector3Ptr approachPose1 = new Vector3();
 
 
    //    approachPose1->x = poseRef->getDataField("positionX")->getFloat()+0.75*(globalApproachPose(0,3)-poseRef->getDataField("positionX")->getFloat());
 
    //    approachPose1->y = poseRef->getDataField("positionY")->getFloat()+0.75*(globalApproachPose(1,3)-poseRef->getDataField("positionY")->getFloat());
    //    approachPose1->z = poseRef->getDataField("rotation")->getFloat()+0.5*(tablePose->z);
    //    approachPose1->x = globalFinalPose(0,3)+2.0*(approachPose1->x-globalFinalPose(0,3));
    //    approachPose1->y = globalFinalPose(1,3)+2.0*(approachPose1->y-globalFinalPose(1,3));
    //    Vector3Ptr approachPose2 = new Vector3();
    //    approachPose2->x = globalApproachPose(0,3);
    //    approachPose2->y = globalApproachPose(1,3);
    //    approachPose2->z = poseRef->getDataField("rotation")->getFloat()+tablePose->z;
 
    //  Vector3Ptr finalPose = new Vector3();
    //  finalPose->x = globalFinalPose(0,3);
    //  finalPose->y = globalFinalPose(1,3);
    //  finalPose->z = poseRef->getDataField("rotation")->getFloat()+tablePose->z;
    //  std::vector< Vector3Ptr > approachPoses;
    //  approachPoses.push_back(approachPose1);
    //  approachPoses.push_back(approachPose2);
    //  approachPoses.push_back(finalPose);
    //  out.setApproachTablePose(approachPoses);
    //  emitEvApproachPoseCalculated();
}
 
void
CalculateApproachTablePose::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
CalculateApproachTablePose::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void
CalculateApproachTablePose::onExit()
{
 
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
CalculateApproachTablePose::GetName()
{
    return "CalculateApproachTablePose";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
CalculateApproachTablePose::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CalculateApproachTablePose(stateData));
}