CalculateTargetRobotPose.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::CalculateTargetRobotPoseGroup
 * @author     [Author Name] ( [Author Email] )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CalculateTargetRobotPose.h"
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 
using namespace armarx;
using namespace CoupledInteractionGroup;
 
// DO NOT EDIT NEXT LINE
CalculateTargetRobotPose::SubClassRegistry CalculateTargetRobotPose::Registry(CalculateTargetRobotPose::GetName(), &CalculateTargetRobotPose::CreateInstance);
 
 
 
CalculateTargetRobotPose::CalculateTargetRobotPose(XMLStateConstructorParams stateData) :
    XMLStateTemplate < CalculateTargetRobotPose > (stateData), CalculateTargetRobotPoseGeneratedBase < CalculateTargetRobotPose > (stateData)
{
}
 
void CalculateTargetRobotPose::onEnter()
{
 
    CoupledInteractionGroupStatechartContext* context = getContext<CoupledInteractionGroupStatechartContext>();
    //Eigen::Matrix4f robotPose = context->getRobot()->getRootNode()->getGlobalPose(); //unused variable
    //                Eigen::Matrix4f m = nodePose*objectPose.toEigen();
    //                objectPose = FramedPose(m, context->getRobot()->getRootNode()->getName(), context->getRobotStateComponent()->getSynchronizedRobot()->getName());
    FramedDirectionPtr curOriVelLeft = in.getCurrentLeftTcpOrientationVelocity();
    FramedDirectionPtr curOriVelRight = in.getCurrentRightTcpOrientationVelocity();
    FramedDirectionPtr curVelRight = in.getCurrentRightTcpPositionVelocity();
    FramedDirectionPtr curVelLeft = in.getCurrentLeftTcpPositionVelocity();
    Vector3Ptr curVelPlatform = in.getCurrentPlatformVelocity();
 
    FramedDirectionPtr tableTarget = in.getTableTargetPose();
 
    Eigen::Vector3f targetLeftTcp;
    Eigen::Vector3f targetRightTcp;
    Eigen::Vector3f platformPose;
    Eigen::Matrix4f relativeTargetPose;
    Eigen::Vector3f initPlatformPose = in.getInitialPlatformPose()->toEigen();
 
    armarx::ChannelRefPtr poseRef = context->getChannelRef(context->getPlatformUnitObserverName(), "platformPose");
    platformPose(0) = poseRef->getDataField("positionX")->getFloat();
    platformPose(1) = poseRef->getDataField("positionY")->getFloat();
    platformPose(2) = poseRef->getDataField("rotation")->getFloat();
    Eigen::Matrix4f tableTargetPose = Eigen::Matrix4f::Identity();
    tableTargetPose(0, 3) = tableTarget->x;
    tableTargetPose(1, 3) = tableTarget->y;
    tableTargetPose(0, 0) = cos(tableTarget->z);
    tableTargetPose(0, 1) = -sin(tableTarget->z);
    tableTargetPose(1, 1) = cos(tableTarget->z);
    tableTargetPose(1, 0) = -sin(tableTarget->z);
    FramedPosePtr tableGlobalPose = new FramedPose(tableTargetPose, "Global", "");
    ARMARX_INFO << "Table Global Pose " << tableTargetPose(0, 3) << " " << tableTargetPose(1, 3);
    tableGlobalPose->changeFrame(context->getRobot(), context->getRobot()->getRootNode()->getName());
 
    relativeTargetPose = tableGlobalPose->toEigen();//robotPose.inverse()*tableTargetPose;
    ARMARX_INFO << "Table Base Pose " << relativeTargetPose(0, 3) << " " << relativeTargetPose(1, 3);
    float maxVelocityTrans = 100.0;
    float maxVelocityRot = 0.2;
    float minVelocityTrans = 0.0;
    float minVelocityRot = 0.0;
    float finalDistance = (initPlatformPose(0) - tableTarget->x) * (initPlatformPose(0) - tableTarget->x);
    finalDistance += (initPlatformPose(1) - tableTarget->y) * (initPlatformPose(1) - tableTarget->y);
    finalDistance = sqrt(finalDistance);
    float remainingDistance = (platformPose(0) - tableTarget->x) * (platformPose(0) - tableTarget->x);
    remainingDistance += (platformPose(1) - tableTarget->y) * (platformPose(1) - tableTarget->y);
    remainingDistance = sqrt(remainingDistance);
    //if (remainingDistance > 0.66*finalDistance)
    //{
    //    maxVelocityRot = 0.0;
    //    minVelocityRot = 0.0;
    //}
    Eigen::Vector3f relativeTarget;
    float angle = tableTarget->z - platformPose(2);
 
    if (fabs(angle) > M_PI)
    {
        if (angle > 0.0)
        {
            angle = angle - 2.0 * M_PI;
        }
        else
        {
            angle = 2.0 * M_PI + angle;
        }
    }
 
    //float targetGain = in.getTargetGain(); //unused variable
    //float curVelGain = std::max(1.0 - targetGain, 0.0); //unused variable
    relativeTarget(0) = std::max(std::min((relativeTargetPose(0, 3)), maxVelocityTrans), -maxVelocityTrans);
    relativeTarget(1) = std::max(std::min((relativeTargetPose(1, 3)), maxVelocityTrans), -maxVelocityTrans);
    relativeTarget(2) = std::max(std::min((angle), maxVelocityRot), -maxVelocityRot);
 
    if (relativeTarget(0) < minVelocityTrans && relativeTarget(0) > 0)
    {
        relativeTarget(0) = minVelocityTrans;
    }
    else if (relativeTarget(0) > -minVelocityTrans  && relativeTarget(0) < 0)
    {
        relativeTarget(0) = -minVelocityTrans;
    }
 
    if (relativeTarget(1) < minVelocityTrans && relativeTarget(1) > 0)
    {
        relativeTarget(1) = minVelocityTrans;
    }
    else if (relativeTarget(1) > -minVelocityTrans  && relativeTarget(1) < 0)
    {
        relativeTarget(1) = -minVelocityTrans;
    }
 
    if (relativeTarget(2) < minVelocityRot && relativeTarget(2) > 0)
    {
        relativeTarget(2) = minVelocityRot;
    }
    else if (relativeTarget(2) > -minVelocityRot  && relativeTarget(2) < 0)
    {
        relativeTarget(2) = -minVelocityRot;
    }
 
    //relativeTarget(0) = 0.0;
    ARMARX_INFO << "Rotating around " << relativeTarget(2) << " " << remainingDistance << " " << finalDistance << " " << tableTarget->z << " " << platformPose(2);
    ARMARX_INFO << "Current platform Pose " << platformPose(0) << " " << platformPose(1);
    ARMARX_INFO << "Current platform Pose " << relativeTargetPose(0, 3) << " " << relativeTargetPose(1, 3);
    Eigen::Vector3f zeroVec;
    zeroVec << 0.0, 0.0, 0.0;
    PlatformUnitInterfacePrx platformPrx = context->getPlatformUnitProxy();
    //platformPrx->move(tableTargetPose(0,3),relativeTarget(0),relativeTarget(1),relativeTarget(2));
    platformPrx->move(0, 30, 0);
 
    emitEvDone();
    //based on that calculate platform target
    //fuse with current velocities
    //Verify in global or ???
    //get Target Pose
    //divide steps
    //put through velocity
    //turn 0.5 degres
    //then move arms
    //out.set
}
 
void CalculateTargetRobotPose::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 CalculateTargetRobotPose::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void CalculateTargetRobotPose::onExit()
{
 
    // put your user code for the exit point here
    // execution time should be short (<100ms)
 
}
 
// DO NOT EDIT NEXT FUNCTION
std::string CalculateTargetRobotPose::GetName()
{
    return "CalculateTargetRobotPose";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr CalculateTargetRobotPose::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CalculateTargetRobotPose(stateData));
}