PlanMotionBase.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 ( raphael dot grimm at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PlanMotionBase.h"
 
#include <ArmarXCore/core/util/OnScopeExit.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
 
using namespace armarx;
using namespace MotionPlanningGroup;
 
// DO NOT EDIT NEXT LINE
PlanMotionBase::SubClassRegistry PlanMotionBase::Registry(PlanMotionBase::GetName(),
                                                          &PlanMotionBase::CreateInstance);
 
void
PlanMotionBase::run()
{
    resetTask();
    //called prior to any locking of a mutex -> no deadlock
    ARMARX_ON_SCOPE_EXIT
    {
        resetTask();
    };
 
    //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.e0 = in.getCSpacePlatformMovementBoundsMin()->x;
        movBounds.min.e1 = in.getCSpacePlatformMovementBoundsMin()->y;
        movBounds.min.e2 = in.getCSpacePlatformMovementBoundsMin()->z;
        movBounds.max.e0 = in.getCSpacePlatformMovementBoundsMax()->x;
        movBounds.max.e1 = in.getCSpacePlatformMovementBoundsMax()->y;
        movBounds.max.e2 = in.getCSpacePlatformMovementBoundsMax()->z;
        cspaceSimox2d->setPoseBounds(movBounds);
    }
    else
    {
        cspaceSimox = new SimoxCSpace{getWorkingMemory()->getCommonStorage()};
    }
 
    //agent
    AgentPlanningInformation agent;
    agent.agentProjectNames = in.getAgentProjectName();
    agent.agentRelativeFilePath = in.getAgentRelativeFilePath();
 
    if (in.isCSpaceKinemaicChainNamesSet())
    {
        agent.kinemaicChainNames = in.getCSpaceKinemaicChainNames();
    }
    if (in.isCSpaceAdditionalJointsForPlanningSet())
    {
        agent.additionalJointsForPlanning = in.getCSpaceAdditionalJointsForPlanning();
    }
    if (in.isCSpaceJointsExcludedFromPlanningSet())
    {
        agent.jointsExcludedFromPlanning = in.getCSpaceJointsExcludedFromPlanning();
    }
 
    agent.collisionSetNames = in.getCollisionSetNames();
    if (in.isInitialJointValuesSet())
    {
        agent.initialJointValues = in.getInitialJointValues();
    }
    agent.agentPose = in.getInitialAgentPose();
 
    if (agent.kinemaicChainNames.empty() && agent.additionalJointsForPlanning.empty())
    {
        ARMARX_ERROR_S << "no joints set for planning! (define CSpaceAdditionalJointsForPlanning "
                          "and/or CSpaceKinemaicChainNames)";
        emitFailure();
        return;
    }
 
    ARMARX_VERBOSE_S << "Loading agent from project '" << agent.agentProjectName << "' and path '"
                     << agent.agentRelativeFilePath << "'";
 
    cspaceSimox->setAgent(agent);
    //objects
    cspaceSimox->addObjectsFromWorkingMemory(getWorkingMemory());
    cspaceSimox->setStationaryObjectMargin(in.getStationaryObjectMargin());
    VectorXf start = cspaceSimox->jointMapToVector(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;
    }
 
    VectorXf stop = cspaceSimox->jointMapToVector(in.getConfigStop());
    if (stop.size() != static_cast<std::size_t>(cspaceSimox->getDimensionality()))
    {
        ARMARX_ERROR_S << "the stop config has " << stop.size() << "demensions, but the cspace has "
                       << cspaceSimox->getDimensionality() << " dimensions.";
        emitFailure();
        return;
    }
    ARMARX_IMPORTANT_S << "planned joints: " << cspaceSimox->getAgentJointNames();
 
    CSpaceBasePtr cspace = cspaceSimox;
    ScaledCSpacePtr scaledCspace;
    if (in.getPlanPlatformMovement())
    {
        //we need scaling
        VectorXf scale = {0.005, 0.005, 1};
        scale.resize(cspaceSimox->getDimensionality(), 1);
        scaledCspace = new ScaledCSpace{cspaceSimox, scale};
        scaledCspace->scaleConfig(start);
        scaledCspace->scaleConfig(stop);
        cspace = scaledCspace;
    }
 
    float dcdStepSize = 0.1;
 
    std::lock_guard<std::mutex> lock{taskMutex};
    assert(!task);
    task = getMotionPlanningServer()->enqueueTask(new RandomShortcutPostprocessorTask{
        new RRTConnectTask{cspace,
                           start,
                           stop,
                           "Statechart.PlanMotionBase.RRT",
                           in.getPlanningTimeInSeconds(),
                           dcdStepSize},
        "Statechart.PlanMotionBase.PostProcess",
        in.getTrajectoryOptimizationTimeout() //max time spend for postprocessing
    });
 
    //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 (TaskStatus::eDone == task->getTaskStatus())
    {
        //found a path!
        PathWithCost p = task->getPathWithCost();
        if (scaledCspace)
        {
            //we planned movement->we need to unscale the path
            scaledCspace->unscalePath(p.nodes);
        }
        TrajectoryPtr trajectory = cspaceSimox->pathToTrajectory(
            p); // new Trajectory{p.nodes, Ice::DoubleSeq{}, cspaceSimox->getAgentJointNames()};
        ARMARX_INFO_S << "Traj: " << trajectory->output();
        setOutput("Trajectory", trajectory);
        emitSuccess();
        return;
    }
    //no path was found!
    emitFailure();
}
 
void
PlanMotionBase::resetTask()
{
    std::lock_guard<std::mutex> lock{taskMutex};
    if (task)
    {
        task->abortTask();
        task = nullptr;
    }
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
PlanMotionBase::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new PlanMotionBase(stateData));
}