SimpleGraspGenerator.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-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    RobotComponents::SimpleGraspGenerator
 * @author     Valerij Wittenbeck (valerij dot wittenbeck at student dot kit dot edu)
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimpleGraspGenerator.h"
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/LinkedPose.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
using namespace armarx;
using namespace memoryx;
 
void SimpleGraspGenerator::onInitComponent()
{
    usingProxy("WorkingMemory");
    usingProxy("PriorKnowledge");
    usingProxy("RobotStateComponent");
 
    offeringTopic("DebugDrawerUpdates");
}
 
void SimpleGraspGenerator::onConnectComponent()
{
    getProxy(rsc, "RobotStateComponent");
    getProxy(wm, "WorkingMemory");
    getProxy(prior, "PriorKnowledge");
    objectInstances = wm->getObjectInstancesSegment();
    objectClasses = prior->getObjectClassesSegment();
 
    fileManager = memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
 
    debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
    robot = RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eStructure);
 
}
 
void SimpleGraspGenerator::onDisconnectComponent()
{
}
 
void SimpleGraspGenerator::onExitComponent()
{
}
 
StringStringDictionary SimpleGraspGenerator::getTcpGcpMapping()
{
    StringStringDictionary mapping;
    auto entries = Split(getProperty<std::string>("TCPtoGCPMapping"), ";", true, true);
    for (auto& e : entries)
    {
        auto split = Split(e, ":", true, true);
        ARMARX_CHECK_EXPRESSION(split.size() == 2) << e;
        mapping[split.at(0)] = split.at(1);
    }
    return mapping;
}
 
GeneratedGraspList SimpleGraspGenerator::generateGrasps(const std::string& objectInstanceEntityId, const Ice::Current& c)
{
    GeneratedGraspList result;
    //    int counter = 0;
 
    ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(objectInstances->getEntityById(objectInstanceEntityId));
    ARMARX_CHECK_EXPRESSION(instance) << "no instance with id '" << objectInstanceEntityId << "'";
 
    ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(objectClasses->getEntityByName(instance->getMostProbableClass()));
    ARMARX_CHECK_EXPRESSION(objectClass) << "no object class with name '" << instance->getMostProbableClass() << "' found ";
    memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
    ARMARX_CHECK_EXPRESSION(simoxWrapper);
    VirtualRobot::ManipulationObjectPtr mo = simoxWrapper->getManipulationObject();
    ARMARX_CHECK_EXPRESSION(mo);
 
 
    auto objectFramedPose = instance->getPose();
    ARMARX_INFO << "object Pose: " << objectFramedPose->output();
    if (instance->hasLocalizationTimestamp())
    {
 
        RemoteRobot::synchronizeLocalCloneToTimestamp(robot, rsc, instance->getLocalizationTimestamp().toMicroSeconds());
        ARMARX_INFO << "object has timestamp " << instance->getLocalizationTimestamp().toDateTime() << " and frame " << objectFramedPose->frame << " with robot at " << robot->getGlobalPose();
        objectFramedPose->changeToGlobal(robot);
    }
    else
        // dangerous - robot might have moved!
    {
        objectFramedPose->changeToGlobal(rsc->getSynchronizedRobot());
    }
    ARMARX_INFO << "global object Pose: " << objectFramedPose->output();
 
    ARMARX_CHECK_EXPRESSION(objectFramedPose->frame == armarx::GlobalFrame);
    Eigen::Matrix4f globalObjectPose = objectFramedPose->toEigen();
    if (getProperty<bool>("EnableVisualization"))
    {
        debugDrawerPrx->setPoseVisu("GeneratedGrasps", "ObjectPose", objectFramedPose);
    }
 
    // draw the position of the object
    //    Eigen::Vector3f tmpObjectPos = localObjectPose.block<3, 1>(0, 3);
    //    armarx::Vector3Ptr objectPos = new armarx::Vector3(tmpObjectPos);
 
    //    debugDrawerPrx->setSphereDebugLayerVisu("objectSphere", objectPos, objectColor, 50.0f);
 
    auto tcpGcpMapping = getTcpGcpMapping();
    std::string graspNameInfix  = getProperty<std::string>("GraspNameInfix");
    int countGrasps = 0;
    auto robotType =  getProperty<std::string>("RobotType").getValue();
    for (const VirtualRobot::GraspSetPtr& gs : mo->getAllGraspSets())
    {
        if (gs->getRobotType() != robotType)
        {
            continue;
        }
        for (const VirtualRobot::GraspPtr& g : gs->getGrasps())
        {
            ARMARX_INFO << "Found Grasp: " << g->getName() << " for eef: " << g->getEefName();
            if (!Contains(g->getName(), graspNameInfix, true))
            {
                ARMARX_INFO << "grasp name does not contain infix " << graspNameInfix << " - skipping it";
                continue;
            }
            if (!robot->hasEndEffector(g->getEefName()))
            {
                ARMARX_INFO << "Endeffector " << g->getEefName() << " does not exist in this robot - skipping grasps";
                continue;
            }
            auto globalTcpPose = g->getTcpPoseGlobal(globalObjectPose);
            FramedPosePtr framedTcpPose {new FramedPose(globalTcpPose, objectFramedPose->frame, "")};
 
            // calculate prepose
            auto gcpName = tcpGcpMapping.at(robot->getEndEffector(g->getEefName())->getTcp()->getName());
            auto gcpNode = robot->getRobotNode(gcpName);
            ARMARX_CHECK_EXPRESSION(gcpNode) << gcpName;
            Eigen::Vector3f preposeOffsetGCPFrame = Eigen::Vector3f::Zero();
            preposeOffsetGCPFrame(2) = -getProperty<float>("PreposeOffset").getValue();
            Eigen::Vector3f preposeOffsetTCPFrame = robot->getEndEffector(g->getEefName())->getTcp()->transformTo(gcpNode, preposeOffsetGCPFrame);
            Eigen::Matrix4f preposeOffsetGlobal = globalTcpPose;
            preposeOffsetGlobal.block<3, 1>(0, 3) += globalTcpPose.block<3, 3>(0, 0) * preposeOffsetTCPFrame;
            ARMARX_INFO << " before up offset: " << preposeOffsetGlobal;
            preposeOffsetGlobal(2, 3) += getProperty<float>("UpwardsOffset").getValue();
            ARMARX_INFO << VAROUT(preposeOffsetGCPFrame) << "\n" << VAROUT(preposeOffsetTCPFrame) << "\n" << VAROUT(preposeOffsetGlobal) << VAROUT(globalTcpPose);
            FramedPosePtr framedTcpPrepose = new FramedPose(preposeOffsetGlobal, armarx::GlobalFrame, "");
            //            prepose->changeFrame(robot, gcpName);
            result.push_back({1.f, g->getEefName(), g->getName(), framedTcpPose, framedTcpPrepose});
 
            countGrasps++;
        }
    }
 
    if (result.empty())
    {
        ARMARX_WARNING << "No grasps defined for object class '" << instance->getMostProbableClass() << "'";
    }
    ARMARX_INFO << "Found " << result.size() << " grasps";
    return result;
}
 
GeneratedGraspList SimpleGraspGenerator::generateGraspsByObjectName(const std::string& objectName, const Ice::Current& c)
{
    memoryx::ObjectInstanceBasePtr objectInstance = wm->getObjectInstancesSegment()->getObjectInstanceByName(objectName);
    std::string id = objectInstance->getId();
    ARMARX_IMPORTANT << "objectName: " << objectName << "; instanceID: " << id;
    return generateGrasps(id);
}