PlannedMotionProvider.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    RobotComponents::ArmarXObjects::PlannedMotionProvider
 * @author     Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PlannedMotionProvider.h"
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
//#include <RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h>
 
namespace armarx
{
    GraspingTrajectory
    PlannedMotionProvider::planMotion(const SimoxCSpaceBasePtr& cSpaceBase,
                                      const SimoxCSpaceBasePtr& cSpacePlatformBase,
                                      const MotionPlanningData& mpd,
                                      const Ice::Current& c)
    {
        TrajectoryPtr jointTrajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
        TrajectoryPtr platformTrajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
        return {platformTrajectory, jointTrajectory, mpd.rnsToUse, mpd.endeffector, mpd.grasp};
    }
 
    GraspingTrajectory
    PlannedMotionProvider::planMotionParallel(const SimoxCSpaceBasePtr& cSpaceBase,
                                              const SimoxCSpaceBasePtr& cSpacePlatformBase,
                                              const MotionPlanningData& mpd,
                                              const Ice::Current& c)
    {
        VectorXf startCfg, goalCfg, startPos, goalPos;
        ScaledCSpacePtr scaledJointCSpace, scaledPlatformCSpace;
        Eigen::Vector3f rpyAgent;
        SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
        SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
        prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
        preparePlatformCSpace(
            cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
        ARMARX_INFO << "Starting BiRRT for Joints and Platform";
        // TODO: This is not async. Why?? Are the tasks not started when they are enqueued?
        RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspJointHandle =
            startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
        RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspPlatformHandle =
            startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
        ARMARX_INFO << "Collecting the jointTask";
        Path jointTrajectoryPath = finishBiRRT(rspJointHandle, scaledJointCSpace, "Joint");
        ARMARX_INFO << "Collecting the platformTask";
        Path posTrajectoryPath = finishBiRRT(rspPlatformHandle, scaledPlatformCSpace, "Platform");
        //    BiRRTTask;
 
        auto transformToGlobal = [&](armarx::VectorXf& pos2D)
        {
            Eigen::Vector2f globalPos =
                Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
            pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
            pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
            pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
        };
        for (auto& e : posTrajectoryPath.nodes)
        {
            transformToGlobal(e);
        }
        TrajectoryPtr jointTrajectory, platformTrajectory;
        jointTrajectory = cSpace->pathToTrajectory(jointTrajectoryPath);
        platformTrajectory = cSpacePlatform->pathToTrajectory(posTrajectoryPath);
        return {platformTrajectory, jointTrajectory, mpd.rnsToUse, mpd.endeffector, mpd.grasp};
    }
 
    void
    PlannedMotionProvider::onInitComponent()
    {
        usingProxy("MotionPlanningServer");
        usingProxy("RobotStateComponent");
        maxPostProcessingSteps = 50;
    }
 
    void
    PlannedMotionProvider::onConnectComponent()
    {
        getProxy(mps, "MotionPlanningServer");
        getProxy(robotStateComponent, "RobotStateComponent");
        localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent,
                                                           VirtualRobot::RobotIO::eStructure);
    }
 
    void
    PlannedMotionProvider::onDisconnectComponent()
    {
        planningTasks.clear();
    }
 
    void
    PlannedMotionProvider::onExitComponent()
    {
    }
 
    armarx::PropertyDefinitionsPtr
    PlannedMotionProvider::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new PlannedMotionProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
    TrajectoryBasePtr
    PlannedMotionProvider::planJointMotion(const armarx::SimoxCSpaceBasePtr& cSpaceBase,
                                           const armarx::MotionPlanningData& mpd,
                                           const Ice::Current& c)
    {
        TrajectoryPtr trajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
        return TrajectoryBasePtr::dynamicCast(trajectory);
    }
 
    TrajectoryBasePtr
    PlannedMotionProvider::planPlatformMotion(const armarx::SimoxCSpaceBasePtr& cSpacePlatformBase,
                                              const armarx::MotionPlanningData& mpd,
                                              const Ice::Current& c)
    {
        TrajectoryPtr trajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
        return TrajectoryBasePtr::dynamicCast(trajectory);
    }
 
    TrajectoryPtr
    PlannedMotionProvider::calculateJointTrajectory(const SimoxCSpaceBasePtr& cSpaceBase,
                                                    const MotionPlanningData& mpd,
                                                    const Ice::Current& c)
    {
        VectorXf startCfg;
        VectorXf goalCfg;
        ScaledCSpacePtr scaledJointCSpace;
        SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
        prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
        RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle =
            startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
        Path jointTrajectoryPath = finishBiRRT(rspHandle, scaledJointCSpace, "Joint");
        return cSpace->pathToTrajectory(jointTrajectoryPath);
    }
 
    TrajectoryPtr
    PlannedMotionProvider::calculatePlatformTrajectory(const SimoxCSpaceBasePtr& cSpacePlatformBase,
                                                       const MotionPlanningData& mpd,
                                                       const Ice::Current& c)
    {
        VectorXf startPos;
        VectorXf goalPos;
        ScaledCSpacePtr scaledPlatformCSpace;
        Eigen::Vector3f rpyAgent;
        SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
        preparePlatformCSpace(
            cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
        //    BiRRTTask;
        RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle =
            startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
        Path posTrajectoryPath = finishBiRRT(rspHandle, scaledPlatformCSpace, "Platform");
 
        auto transformToGlobal = [&](armarx::VectorXf& pos2D)
        {
            Eigen::Vector2f globalPos =
                Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
            pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
            pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
            pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
        };
        for (auto& e : posTrajectoryPath.nodes)
        {
            transformToGlobal(e);
        }
 
        return cSpacePlatform->pathToTrajectory(posTrajectoryPath);
    }
 
    RemoteHandle<MotionPlanningTaskControlInterfacePrx>
    PlannedMotionProvider::startBiRRT(const ScaledCSpacePtr scaledCSpace,
                                      const armarx::VectorXf startPos,
                                      const armarx::VectorXf goalPos,
                                      float dcdStep,
                                      bool doRandomShortcutPostProcessing)
    {
        //    BiRRTTask;
        MotionPlanningTaskBasePtr taskRRT =
            new BiRRTTask{scaledCSpace,
                          startPos,
                          goalPos,
                          getDefaultName() + ".birrt" + IceUtil::generateUUID(),
                          60,
                          dcdStep};
        RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle;
        if (!doRandomShortcutPostProcessing)
        {
            handle = mps->enqueueTask(taskRRT);
        }
        else
        {
            handle = mps->enqueueTask(
                new RandomShortcutPostprocessorTask(taskRRT,
                                                    "RRTSmoothing" + IceUtil::generateUUID(),
                                                    1,
                                                    dcdStep,
                                                    maxPostProcessingSteps));
        }
        planningTasks.push_back(handle);
        return handle;
    }
 
    Path
    PlannedMotionProvider::finishBiRRT(RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle,
                                       const ScaledCSpacePtr scaledCSpace,
                                       const std::string roboPart)
    {
        ARMARX_CHECK_EXPRESSION(handle);
        handle->waitForFinishedRunning();
        ARMARX_IMPORTANT
            << roboPart + " trajectory planning took "
            << IceUtil::Time::microSeconds(handle->getRunningTime()).toMilliSecondsDouble()
            << " ms";
 
        if (handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed)
        {
            throw RuntimeError(" Motion Planning failed!");
        }
 
        ARMARX_INFO << "RRTConnectTask Planning "
                    << ((bool)(handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed)
                            ? "failed"
                            : "succeeded");
        ARMARX_INFO << "RRTConnectTask Planning status: " << handle->getTaskStatus();
 
        auto posTrajectoryPath = handle->getPath();
        ARMARX_CHECK_EXPRESSION(scaledCSpace);
        scaledCSpace->unscalePath(posTrajectoryPath);
        return posTrajectoryPath;
    }
 
    void
    PlannedMotionProvider::preparePlatformCSpace(SimoxCSpacePtr cSpacePlatform,
                                                 const MotionPlanningData& mpd,
                                                 VectorXf* storeStart,
                                                 VectorXf* storeGoal,
                                                 ScaledCSpacePtr* storeScaledCSpace,
                                                 Eigen::Vector3f* storeRpyAgent)
    {
        cSpacePlatform->setStationaryObjectMargin(
            getProperty<float>("PlatformMotionSafetyMargin").getValue());
        ARMARX_CHECK_EXPRESSION(cSpacePlatform);
 
        Eigen::Vector3f rpy, rpyAgent;
        VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseStart)->toEigen(),
                                             rpy);
        VirtualRobot::MathTools::eigen4f2rpy(
            PosePtr::dynamicCast(cSpacePlatform->getAgent().agentPose)->toEigen(), rpyAgent);
        armarx::VectorXf startPos{
            mpd.globalPoseStart->position->x - cSpacePlatform->getAgent().agentPose->position->x,
            mpd.globalPoseStart->position->y - cSpacePlatform->getAgent().agentPose->position->y,
            math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))};
 
        Eigen::Vector2f localStartPos =
            Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(startPos.at(0), startPos.at(1));
        startPos.at(0) = localStartPos(0);
        startPos.at(1) = localStartPos(1);
        VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseGoal)->toEigen(),
                                             rpy);
        armarx::VectorXf goalPos{
            mpd.globalPoseGoal->position->x - cSpacePlatform->getAgent().agentPose->position->x,
            mpd.globalPoseGoal->position->y - cSpacePlatform->getAgent().agentPose->position->y,
            math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))};
        Eigen::Vector2f localGoalPos =
            Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(goalPos.at(0), goalPos.at(1));
        goalPos.at(0) = localGoalPos(0);
        goalPos.at(1) = localGoalPos(1);
 
        ARMARX_INFO << VAROUT(startPos) << VAROUT(goalPos);
 
        ScaledCSpacePtr scaledPlatformCSpace = new ScaledCSpace(cSpacePlatform, {0.001, 0.001, 1});
        scaledPlatformCSpace->scaleConfig(startPos);
        scaledPlatformCSpace->scaleConfig(goalPos);
        *storeStart = startPos;
        *storeGoal = goalPos;
        *storeScaledCSpace = scaledPlatformCSpace;
        *storeRpyAgent = rpyAgent;
    }
 
    void
    PlannedMotionProvider::prepareJointCSpace(SimoxCSpacePtr cSpace,
                                              const MotionPlanningData& mpd,
                                              VectorXf* storeStart,
                                              VectorXf* storeGoal,
                                              ScaledCSpacePtr* storeScaledCSpace)
    {
        cSpace->setStationaryObjectMargin(getProperty<float>("JointMotionSafetyMargin").getValue());
        ARMARX_CHECK_EXPRESSION(cSpace);
        ARMARX_INFO << "Entered prepareJointCSpace";
        Ice::FloatSeq cSpaceScaling;
        for (VirtualRobot::RobotNodePtr node :
             localRobot->getRobotNodeSet(mpd.rnsToUse)->getAllRobotNodes())
        {
            cSpaceScaling.push_back(node->isTranslationalJoint() ? 0.001f : 1.0f);
        }
        ARMARX_IMPORTANT << "Robot position: " << VAROUT(mpd.globalPoseStart->output())
                         << VAROUT(mpd.globalPoseGoal->output());
 
        armarx::VectorXf startCfg = cSpace->jointMapToVector(mpd.configStart);
        armarx::VectorXf goalCfg = cSpace->jointMapToVector(mpd.configGoal);
 
        ARMARX_VERBOSE << VAROUT(startCfg) << VAROUT(goalCfg);
        ScaledCSpacePtr scaledJointCSpace = new ScaledCSpace(cSpace, cSpaceScaling);
        scaledJointCSpace->scaleConfig(startCfg);
        scaledJointCSpace->scaleConfig(goalCfg);
        *storeStart = startCfg;
        *storeGoal = goalCfg;
        *storeScaledCSpace = scaledJointCSpace;
    }
} // namespace armarx