PlanMotion.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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::MotionPlanningGroup
 * @author     Raphael Grimm ( ufdrv at student dot kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PlanMotion.h"
 
#include <algorithm>
 
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
 
using namespace armarx;
using namespace MotionPlanningGroup;
 
// DO NOT EDIT NEXT LINE
PlanMotion::SubClassRegistry PlanMotion::Registry(PlanMotion::GetName(), &PlanMotion::CreateInstance);
 
 
 
void PlanMotion::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    resetTask();
}
 
void PlanMotion::run()
{
    //    resetTask();
    //    //called prior to any locking of a mutex -> no deadlock
    //    ARMARX_ON_SCOPE_EXIT
    //    {
    //        resetTask();
    //    };
    //    // 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
 
    //    //setup cspace
    //    SimoxCSpacePtr cspaceSimox;
    //    if(in.getPlanPlatformMovement())
    //    {
    //        SimoxCSpaceWith2DPosePtr cspaceSimox2d = new SimoxCSpaceWith2DPose{getWorkingMemory()->getCommonStorage()};
    //        if(!in.isCSpacePlatformMovementBoundsMinSet())
    //        {
    //            ARMARX_ERROR_S << "platform movement should be planned, but the min movement bounds were not set.";
    //            emitFailure();
    //            return;
    //        }
    //        if(!in.isCSpacePlatformMovementBoundsMaxSet())
    //        {
    //          ARMARX_ERROR_S << "platform movement should be planned, but the max movement bounds were not set.";
    //          emitFailure();
    //          return;
    //        }
    //        cspaceSimox =cspaceSimox2d;
    //        Vector3fRange movBounds;
    //        movBounds.min.x = in.getCSpacePlatformMovementBoundsMin()->x;
    //        movBounds.min.y = in.getCSpacePlatformMovementBoundsMin()->y;
    //        movBounds.min.z = in.getCSpacePlatformMovementBoundsMin()->z;
    //        movBounds.max.x = in.getCSpacePlatformMovementBoundsMax()->x;
    //        movBounds.max.y = in.getCSpacePlatformMovementBoundsMax()->y;
    //        movBounds.max.z = in.getCSpacePlatformMovementBoundsMax()->z;
    //        cspaceSimox2d->setPoseBounds(movBounds);
    //    }
    //    else
    //    {
    //        cspaceSimox = new SimoxCSpace{getWorkingMemory()->getCommonStorage()};
    //    }
    //    //setup agent
 
 
 
    //    /*
 
 
    //    ::std::string agentProjectName;
    //    ::std::string agentRelativeFilePath;
    //    ::armarx::PoseBasePtr agentPose;
 
 
 
 
    //     */
    //    AgentPlanningInformation agent;
    //    VirtualRobot::RobotPtr stateComponentRobot;
    //    memoryx::AgentInstancePtr agentInstance;
 
    //    //do this before deriving init joint values from the robots
    //    if(in.isInitialJointValuesSet())
    //    {
    //        agent.initialJointValues = in.getInitialJointValues();
    //    }
    //    //get file path and optionally derive joint values
    //    if(in.isAgentProjectNameSet()&&in.isAgentRelativeFilePathSet())
    //    {
    //        agent.agentProjectName = in.getAgentProjectName();
    //        agent.agentRelativeFilePath = in.getAgentRelativeFilePath();
    //    }
    //    else if(in.isAgentInstanceSet())
    //    {
    //        //load from agent instance
    //        agentInstance = in.getAgentInstance();
    //        agent.agentRelativeFilePath = agentInstance->getAgentFilePath();
    //        //joints
    //        if(in.getInitalJointValuesFromAgent())
    //        {
    //            auto nodeValues = agentInstance->getSharedRobot()->getConfig();
    //            //override from initialJointValues, since probably is smaller
    //            std::swap(agent.initialJointValues, nodeValues);
    //            for(const auto& node: nodeValues)
    //            {
    //                agent.initialJointValues[node.first] = node.second;
    //            }
    //        }
    //        //pose
    //        agent.agentPose = agentInstance->getPose();
    //    }
    //    else
    //    {
    //        //load agent from robot state component
    //        stateComponentRobot = getRobot();
    //        agent.agentRelativeFilePath =stateComponentRobot->getFilename();
    //        //joints
    //        if(in.getInitalJointValuesFromAgent())
    //        {
    //            auto nodes = stateComponentRobot->getRobotNodes();
    //            for(const VirtualRobot::RobotNodePtr& node :nodes)
    //            {
    //                //does not update existing values!
    //                agent.initialJointValues.emplace(node->getName(), node->getJointValue());
    //            }
    //        }
    //        //pose
    //        agent.agentPose = new Pose{stateComponentRobot->getGlobalPose()};
    //    }
    //    ARMARX_VERBOSE_S << "Loading agent from project '" << agent.agentProjectName
    //                        <<"' and path '" <<agent.agentRelativeFilePath<<"'";
 
    //    //set agent pose if it is set by the user
    //    if(in.isInitialAgentPoseSet())
    //    {
    //        agent.agentPose = in.getInitialAgentPose();
    //    }
    //    else if(!agent.agentPose)
    //    {
    //        if(in.getPlanPlatformMovement())
    //        {
    //            //the pose is not used -> set some pose
    //            agent.agentPose = new Pose{};
    //        }
    //        else
    //        {
    //            //the agent pose is used, but not set
    //            ARMARX_ERROR_S << "the initial agent pose was not set. "
    //                            <<"(either define it explicit by setting InitialAgentPose, "
    //                            <<"implicit by loading a robot from an AgentInstance "
    //                            <<"or RobotComponent or plan the platforms position by setting PlanPlatformMovement to true";
    //            emitFailure();
    //            return;
    //        }
    //    }
 
    //    //joints to plan
    //    if(in.isCSpaceJointsExcludedFromPlanningSet())
    //    {
    //        agent.jointsExcludedFromPlanning = in.getCSpaceJointsExcludedFromPlanning();
    //    }
    //    if(in.isCSpaceKinemaicChainsSet())
    //    {
    //        agent.kinemaicChainNames = in.getCSpaceKinemaicChains();
    //    }
    //    if(in.isCSpacePlanningJointsSet())
    //    {
    //        agent.additionalJointsForPlanning = in.getCSpacePlanningJoints();
    //    }
 
    //    if(agent.kinemaicChainNames.empty() && agent.additionalJointsForPlanning.empty())
    //    {
    //        ARMARX_ERROR_S << "no joints set for planning! (define CSpacePlanningJoints and/or CSpaceKinemaicChains)";
    //        emitFailure();
    //        return;
    //    }
 
    //    //collision
    //    agent.collisionSetNames = in.getCollisionSetNames();
 
    //    cspaceSimox->setAgent(agent);
    //    //objects
    //    cspaceSimox->addObjectsFromWorkingMemory(getWorkingMemory());
 
    //    VectorXf start;
    //    if(in.isConfigStartSet())
    //    {
    //        start = in.getConfigStart();
    //        if(start.size() != static_cast<std::size_t>(cspaceSimox->getDimensionality()))
    //        {
    //            ARMARX_ERROR_S << "the start config has " << start.size() << "demensions, but the cspace has " << cspaceSimox->getDimensionality() << " dimensions.";
    //            emitFailure();
    //            return;
    //        }
    //    }
    //    else
    //    {
    //        if(agentInstance)
    //        {
 
    //        }
    //        else if(stateComponentRobot)
    //        {
 
    //        }
    //        else
    //        {
    //            ARMARX_ERROR_S << "no start configuration was set (either define ConfigStart explicit, or implicit by using an agent loaded from AgentInstance or RobotStateComponent";
    //            emitFailure();
    //            return;
    //        }
    //    }
 
    //    VectorXf goal;
    //    RRTConnectTaskPtr rrt;
 
    //    //create the task
    //    std::lock_guard< std::mutex> lock{taskMutex};
    //    task = getMotionPlanningServer()->enqueueTask(new RandomShortcutPostprocessorTask{rrt});
    //    //wait for the task to finish (we can't call wait, since we may want to abort)
    //    while(!isRunningTaskStopped() && task->finishedRunning())
    //    {
    //        //time does not need to respect the time server (this is just polling for a result)
    //        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    //    }
    //    if(task->finishedRunning())
    //    {
    //        //the task found a path!
    //        PathWithCost p = task->getPathWithCost();
    //        TrajectoryPtr trajectory = new Trajectory{p.nodes, Ice::DoubleSeq{}, cspaceSimox->getAgentJointNames()};
    //        setOutput("Trajectory", trajectory);
    //        emitSuccess();
    //        return;
    //    }
    //    //no path was found!
    //    emitFailure();
}
 
void PlanMotion::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
    resetTask();
}
 
void PlanMotion::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
    resetTask();
}
 
void PlanMotion::resetTask()
{
    //    std::lock_guard< std::mutex> lock{taskMutex};
    //    if(task)
    //    {
    //        task->abortTask();
    //        task = nullptr;
    //    }
}
 
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr PlanMotion::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new PlanMotion(stateData));
}