FindAndGraspObjectContext.h

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::FindAndGraspObject
* @author     Nikolaus Vahrenkamp
* @date       2012 Nikolaus Vahrenkamp
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
 
#pragma once
 
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
//#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/statechart/StatechartContext.h>
 
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/core/RobotStatechartContext.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/interface/components/HandLocalization.h>
#include <VisionX/interface/core/ImageProcessorInterface.h>
 
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
 
namespace armarx
{
 
    // ****************************************************************
    // Component and context
    // ****************************************************************
 
    struct FindAndGraspObjectContextProperties : RobotStatechartContextProperties
    {
        FindAndGraspObjectContextProperties(std::string prefix) :
            RobotStatechartContextProperties(prefix)
        {
            defineOptionalProperty<std::string>("KinematicUnitName",
                                                "Armar3KinematicUnit",
                                                "Name of the kinematic unit that should be used");
            defineOptionalProperty<std::string>(
                "RobotStateComponentName",
                "RobotStateComponent",
                "Name of the robot state component that should be used");
            defineOptionalProperty<std::string>(
                "ObjectMemoryObserverName",
                "ObjectMemoryObserver",
                "Name of the ObjectMemoryObserver component that should be used");
            defineOptionalProperty<std::string>(
                "PriorKnowledgeName",
                "PriorKnowledge",
                "Name of the PriorKnowledge component that should be used");
            //defineOptionalProperty<std::string>("SimulatorName", "Simulator", "Name of the simulator component that should be used");
 
            defineOptionalProperty<std::string>(
                "HeadIKUnitName", "", "Name of the head unit that should be used.");
            defineOptionalProperty<std::string>(
                "HeadIKKinematicChainName",
                "",
                "Name of the inematic chain that should be used for head IK.");
        }
    };
 
    class ARMARXCOMPONENT_IMPORT_EXPORT FindAndGraspObjectContext :
        virtual public RobotStatechartContext
    {
    public:
        // inherited from Component
        virtual std::string
        getDefaultName()
        {
            return "FindAndGraspObjectContext";
        }
 
        virtual void
        onInitStatechartContext()
        {
            ARMARX_LOG << eINFO << "Init FindAndGraspObjectContext" << flush;
 
            RobotStatechartContext::onInitStatechartContext();
 
            usingProxy(getProperty<std::string>("KinematicUnitObserverName").getValue());
            usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
            usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
            usingProxy(getProperty<std::string>("ObjectMemoryObserverName").getValue());
            usingProxy(getProperty<std::string>("PriorKnowledgeName").getValue());
 
            //For Visualization of (debug) coordinate systems
            std::string simPrxName = getProperty<std::string>("SimulatorName").getValue();
 
            if (!simPrxName.empty())
            {
                usingProxy(simPrxName);
            }
 
            // headIKUnit
            headIKUnitName = getProperty<std::string>("HeadIKUnitName").getValue();
            ARMARX_LOG << eINFO << "headIKUnitName:" << headIKUnitName << flush;
            headIKKinematicChainName =
                getProperty<std::string>("HeadIKKinematicChainName").getValue();
 
            if (!headIKUnitName.empty())
            {
                usingProxy(headIKUnitName);
            }
        }
 
        virtual void
        onConnectStatechartContext()
        {
            RobotStatechartContext::onConnectStatechartContext();
            ARMARX_LOG << eINFO << "Starting FindAndGraspObjectContext" << flush;
            // retrieve proxies
 
            objectMemoryObserver = getProxy<memoryx::ObjectMemoryObserverInterfacePrx>(
                getProperty<std::string>("ObjectMemoryObserverName").getValue());
            priorMemoryProxy = getProxy<memoryx::PriorKnowledgeInterfacePrx>(
                getProperty<std::string>("PriorKnowledgeName").getValue());
            std::string rbStateName =
                getProperty<std::string>("RobotStateComponentName").getValue();
            robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(rbStateName);
            std::string kinUnitName = getProperty<std::string>("KinematicUnitName").getValue();
            kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinUnitName);
 
            // initialize remote robot
            remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
 
            //For Visualization of (debug) coordinate systems
            /*std::string simPrxName = getProperty<std::string>("SimulatorName").getValue();
            if (!simPrxName.empty())
            {
                simulatorProxy = getProxy<SimulatorInterfacePrx>(simPrxName);
            }
            //ARMARX_LOG << eINFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
            */
            if (!headIKUnitName.empty())
            {
                headIKUnitPrx = getProxy<HeadIKUnitInterfacePrx>(headIKUnitName);
                ARMARX_LOG << eINFO << "Fetched headIK proxy " << headIKUnitName << ":"
                           << headIKUnitPrx << ", head IK kin chain:" << headIKKinematicChainName
                           << flush;
            }
 
            localRobot = RemoteRobot::createLocalClone(robotStateComponent);
            ARMARX_LOG << eINFO << "Created local robot" << flush;
        }
 
        virtual PropertyDefinitionsPtr
        createPropertyDefinitions()
        {
            return PropertyDefinitionsPtr(
                new FindAndGraspObjectContextProperties(getConfigIdentifier()));
        }
 
        //From GraspingWithVisualFeedbackContext:
        //TCPControlUnitInterfacePrx tcpControlUnitProxy;   //Maybe not necessary?
 
        //visionx::ImageFileSequenceProviderInterfacePrx imageSequenceProviderProxy;
        memoryx::ObjectMemoryObserverInterfacePrx objectMemoryObserver;
        memoryx::PriorKnowledgeInterfacePrx priorMemoryProxy;
        VirtualRobot::GraspPtr graspDefinition;
        //visionx::HandLocalizationInterfacePrx handLocalizationProxy;
        //std::string kinematicChainName, graspedObjectName;
 
        //From VisualServoContext:
        RobotStateComponentInterfacePrx robotStateComponent;
        KinematicUnitInterfacePrx kinematicUnitPrx;
        VirtualRobot::RobotPtr remoteRobot;
 
        //For Visualization of (debug) coordinate systems
        //SimulatorInterfacePrx simulatorProxy;
 
        HeadIKUnitInterfacePrx headIKUnitPrx;
        std::string headIKKinematicChainName;
        std::string headIKUnitName;
 
        // a temporary robot which has to be manually synchronized. Can be used for IK computations.
        VirtualRobot::RobotPtr localRobot;
    };
 
} // namespace armarx