SelectArm.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::BringObjectGroup
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SelectArm.h"
 
#include <cmath>
 
#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/interface/core/FramedPoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <RobotSkillTemplates/statecharts/BringObjectGroup/BringObjectGroupStatechartContext.h>
 
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
 
using namespace armarx;
using namespace BringObjectGroup;
 
// DO NOT EDIT NEXT LINE
SelectArm::SubClassRegistry SelectArm::Registry(SelectArm::GetName(), &SelectArm::CreateInstance);
 
SelectArm::SelectArm(XMLStateConstructorParams stateData) :
    XMLStateTemplate<SelectArm>(stateData), SelectArmGeneratedBase<SelectArm>(stateData)
{
}
 
bool
SelectArm::checkForCollision(Vector3Ptr targetPlatformPose)
{
    // use simox for collision check
    //    BringObjectGroupStatechartContext* c = getContext<BringObjectGroupStatechartContext>();
    //    c->getPathPlanner()->
    return true;
}
 
int
getClosestTCP(FramedPosePtr localObjectPose,
              BringObjectGroupStatechartContext* c,
              const std::vector<std::string>& memoryNames,
              const std::vector<std::string>& chains)
{
    int closestChainId = -1;
    float minDistance = std::numeric_limits<float>::max();
    ARMARX_CHECK_EXPRESSION(chains.size() == memoryNames.size());
 
    for (size_t i = 0; i < chains.size(); i++)
    {
        std::string chain = chains[i];
        auto set = c->getRobot()->getRobotNodeSet(chain);
        auto distance =
            (set->getTCP()->getPositionInRootFrame() - localObjectPose->toEigen().block<3, 1>(0, 3))
                .norm();
        ARMARX_VERBOSE_S << VAROUT(distance) << VAROUT(minDistance) << VAROUT(i);
 
        if (distance < minDistance)
        {
            minDistance = distance;
            closestChainId = i;
        }
    }
 
    return closestChainId;
}
 
void
SelectArm::onEnter()
{
 
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    BringObjectGroupStatechartContext* c = getContext<BringObjectGroupStatechartContext>();
 
    auto instance = in.getObjectInstanceToGraspChannel();
    FramedPositionPtr position = instance->get<FramedPosition>("position");
    FramedOrientationPtr orientation = instance->get<FramedOrientation>("orientation");
    FramedPosePtr globalObjectPose = new FramedPose(
        orientation->toEigen(), position->toEigen(), position->getFrame(), position->agent);
 
    FramedPosePtr objectPoseRootFrame;
 
    objectPoseRootFrame = FramedPosePtr::dynamicCast(globalObjectPose->clone());
    objectPoseRootFrame->changeFrame(c->getRobot(), c->getRobot()->getRootNode()->getName());
 
    //    memoryx::ChannelRefBaseSequence instances = c->getObjectMemoryObserverProxy()->getObjectInstances(in.getObjectToGraspChannel());
    if (in.isPreselectedKinematicChainSet() && in.isPreselectedMemoryHandNameSet())
    {
        out.setSelectedKinematicChain(in.getPreselectedKinematicChain());
        out.setSelectedMemoryHandName(in.getPreselectedMemoryHandName());
    }
    else if (in.isKinematicChainsSet() && in.isMemoryHandNamesSet())
    {
 
 
        ARMARX_IMPORTANT << VAROUT(*objectPoseRootFrame);
        auto chains = in.getKinematicChains();
        auto memoryNames = in.getMemoryHandNames();
        int closestChainId = getClosestTCP(objectPoseRootFrame, c, memoryNames, chains);
 
        out.setSelectedKinematicChain(chains.at(closestChainId));
        out.setSelectedMemoryHandName(memoryNames.at(closestChainId));
    }
    else
    {
        ARMARX_ERROR << "Either PreselectedKinematicChainSet and PreselectedMemoryHandNameSet or "
                        "KinematicChainsSet and MemoryHandNamesSet parameter must be set.";
        emitFailure();
        return;
    }
 
    ARMARX_IMPORTANT << "Selected Memory Name is: " << out.getSelectedMemoryHandName();
    out.setSelectedTCP(
        c->getRobot()->getRobotNodeSet(out.getSelectedKinematicChain())->getTCP()->getName());
    ARMARX_IMPORTANT << "Selected TCP is: " << out.getSelectedTCP();
    // get grasp definition
    std::string objectName =
        in.getObjectInstanceToGraspChannel()->getDataField("className")->getString();
 
    memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx =
        c->getPriorKnowledgeProxy()->getObjectClassesSegment();
    memoryx::CommonStorageInterfacePrx databasePrx =
        c->getPriorKnowledgeProxy()->getCommonStorage();
    memoryx::GridFileManagerPtr fileManager(new memoryx::GridFileManager(databasePrx));
    memoryx::EntityBasePtr entity = classesSegmentPrx->getEntityByName(objectName);
    memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(entity);
    memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
        objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
    VirtualRobot::ManipulationObjectPtr mo = simoxWrapper->getManipulationObject();
 
    VirtualRobot::GraspSetPtr graspSet = mo->getGraspSet(out.getSelectedTCP());
 
    auto findGrasp = [](VirtualRobot::GraspSetPtr graspSet,
                        const Ice::StringSeq& requiredInfixes,
                        const Ice::StringSeq& exludedInfixes)
    {
        if (graspSet)
        {
            auto grasps = graspSet->getGrasps();
 
            for (VirtualRobot::GraspPtr grasp : grasps)
            {
                std::string graspName = grasp->getName();
                bool found = true;
                for (const auto& infix : requiredInfixes)
                {
                    found &= Contains(graspName, infix, true);
                }
                for (const auto& infix : exludedInfixes)
                {
                    found &= !Contains(graspName, infix, true);
                }
                if (found)
                {
                    return graspName;
                }
            }
        }
        return std::string("");
    };
 
    if (graspSet)
    {
 
        std::string preposeName;
        Ice::StringSeq infixes;
        if (in.isGraspNameInfixSet())
        {
            infixes.push_back(in.getGraspNameInfix());
        }
        infixes.push_back({"pre"});
 
        preposeName = findGrasp(graspSet, infixes, Ice::StringSeq());
        if (!preposeName.empty())
        {
            out.setGraspPreposeName(preposeName);
        }
        else
        {
            infixes.clear();
            infixes.push_back({"pre"}); // now without infix..
            preposeName = findGrasp(graspSet, infixes, Ice::StringSeq());
            if (!preposeName.empty())
            {
                out.setGraspPreposeName(preposeName);
            }
        }
 
        infixes.clear();
        if (in.isGraspNameInfixSet())
        {
            infixes.push_back(in.getGraspNameInfix());
        }
        infixes.push_back({"grasp"});
        std::string graspName = findGrasp(graspSet, infixes, {"pre"});
        if (!graspName.empty())
        {
            out.setGraspName(graspName);
        }
        else
        {
            infixes.clear();
            infixes.push_back({"grasp"}); // now without infix..
            graspName = findGrasp(graspSet, infixes, {"pre"});
            if (!graspName.empty())
            {
                out.setGraspName(graspName);
            }
        }
 
 
        auto getGraspNames = [](VirtualRobot::GraspSetPtr graspSet)
        {
            Ice::StringSeq graspNames;
            for (VirtualRobot::GraspPtr& g : graspSet->getGrasps())
            {
                graspNames.push_back(g->getName());
            };
            return graspNames;
        };
 
        if (!out.isGraspPreposeNameSet())
        {
 
            ARMARX_ERROR << "could not find any matching pre pose for " << out.getSelectedTCP()
                         << " for object " << objectName
                         << " - Found Grasps are: " << getGraspNames(graspSet);
            out.setGraspPreposeName("");
            out.setGraspName("");
            emitFailure();
            return;
        }
 
        if (!out.isGraspNameSet())
        {
            ARMARX_ERROR << "could not find any matching grasp pose for " << out.getSelectedTCP()
                         << " for object " << objectName
                         << " - Found Grasps are: " << getGraspNames(graspSet);
            out.setGraspPreposeName("");
            out.setGraspName("");
            emitFailure();
            return;
        }
    }
    else
    {
        ARMARX_ERROR << "graspSet Ptr NULL - Could not find grasp set for tcp "
                     << out.getSelectedTCP() << " for object " << objectName;
        emitFailure();
        return;
    }
 
 
    Eigen::Vector3f tcpPosition = c->getRobot()
                                      ->getRobotNodeSet(out.getSelectedKinematicChain())
                                      ->getTCP()
                                      ->getPositionInRootFrame();
    if (tcpPosition[0] < 0) // Left Hand
    {
        out.setPregraspArmConfig(in.getPregraspArmConfigLeft());
    }
    else
    {
        out.setPregraspArmConfig(in.getPregraspArmConfigRight());
    }
 
 
    if (in.getUsePlatform())
    {
 
        Eigen::Vector3f objPosRootFrame =
            Vector3Ptr::dynamicCast(objectPoseRootFrame->position)->toEigen();
 
        ARMARX_INFO << VAROUT(*objectPoseRootFrame);
        Eigen::Vector3f newPlatformPos = objPosRootFrame;
        Vector3Ptr offset;
 
        if (tcpPosition[0] < 0) // Left Hand
        {
            offset = in.getPlatformToObjectOffsetLeft();
        }
        else
        {
            offset = in.getPlatformToObjectOffsetRight();
        }
 
        newPlatformPos[0] += offset->x;
        newPlatformPos[1] += offset->y;
 
        if (fabs(newPlatformPos[1]) > 500)
        {
            ARMARX_ERROR << "Target platform pose is too far away: " << newPlatformPos;
        }
 
        ARMARX_INFO << VAROUT(newPlatformPos);
        ARMARX_INFO << "rootnode pose: " << c->getRobot()->getRootNode()->getGlobalPose();
        Eigen::Vector3f newGlobalPlatformPos =
            c->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(newPlatformPos);
        Eigen::Matrix3f rotMat = c->getRobot()->getRootNode()->getGlobalPose().block<3, 3>(0, 0);
 
        ARMARX_INFO << "Root Node: " << c->getRobot()->getRootNode()->getName();
        ARMARX_INFO << c->getRobot()->getRootNode()->getGlobalPose().block<3, 3>(0, 0);
 
        Eigen::Vector3f x = rotMat * Eigen::Vector3f::UnitX();
        newGlobalPlatformPos[2] = atan2(x[1], x[0]);
        std::vector<Vector3Ptr> pose;
 
        // If necessary, clamp the platform motion to the defined constraints
        Eigen::Vector3f platformPos =
            c->getRobot()->getRootNode()->getGlobalPose().block<3, 1>(0, 3);
 
        if (in.isMaxPlatformMotionXSet() &&
            fabs(newGlobalPlatformPos[0] - platformPos[0]) > in.getMaxPlatformMotionX())
        {
            newGlobalPlatformPos[0] =
                platformPos[0] +
                std::copysign(in.getMaxPlatformMotionX(), newGlobalPlatformPos[0] - platformPos[0]);
            ARMARX_WARNING << "Platform motion clamped to maximum in X direction: "
                           << newGlobalPlatformPos[0];
        }
 
        if (in.isMaxPlatformMotionYSet() &&
            fabs(newGlobalPlatformPos[1] - platformPos[1]) > in.getMaxPlatformMotionY())
        {
            newGlobalPlatformPos[1] =
                platformPos[1] +
                std::copysign(in.getMaxPlatformMotionY(), newGlobalPlatformPos[1] - platformPos[1]);
            ARMARX_WARNING << "Platform motion clamped to maximum in Y direction: "
                           << newGlobalPlatformPos[1];
        }
 
        Vector3Ptr newPlatformVec = new Vector3(newGlobalPlatformPos);
 
        if (!checkForCollision(newPlatformVec))
        {
            ARMARX_ERROR << "Collision detected";
            emitFailure();
            return;
        }
        else
        {
            pose.push_back(newPlatformVec);
            ARMARX_INFO << VAROUT(*newPlatformVec);
            out.setPlatformTargetPose(pose);
            emitArmSelectedMovePlatform();
        }
    }
    else
    {
        emitArmSelected();
    }
}
 
void
SelectArm::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
SelectArm::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new SelectArm(stateData));
}