SimpleRobotPlacement.h

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::SimpleRobotPlacement
 * @author      Harry Arnst (harry dot arnst at student dot kit dot edu)
 *              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
 */
 
#pragma once
 
#include <ArmarXCore/core/Component.h>
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/core/GridFileManager.h>
#include <RobotAPI/interface/core/RobotState.h>
 
#include <VirtualRobot/Workspace/WorkspaceGrid.h>
#include <RobotComponents/interface/components/RobotIK.h>
#include <VirtualRobot/Workspace/Manipulability.h>
#include <VirtualRobot/Workspace/Reachability.h>
#include <VirtualRobot/SceneObjectSet.h>
 
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
#include <MemoryX/interface/gui/EntityDrawerInterface.h>
#include <VirtualRobot/CollisionDetection/CDManager.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
//#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
//#include <RobotComponents/components/MotionPlanning/Tasks/AStar/Task.h>
#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
 
#include <Eigen/Geometry>
 
namespace armarx
{
    /**
     * @class SimpleRobotPlacementPropertyDefinitions
     * @brief
     */
    class SimpleRobotPlacementPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        SimpleRobotPlacementPropertyDefinitions(std::string prefix);
    };
 
    /**
     * @defgroup Component-SimpleRobotPlacement SimpleRobotPlacement
     * @ingroup RobotComponents-Components
     * @brief The SimpleRobotPlacement component provides methods to calculate candidate robot placements (i.e. base poses) in order to grasp a given object.
     *
     * This component requires following properties:
     * \li RobotName: The name of the robot
     * \li RobotFilePath: Filepath of the VirtualRobot robot to use (relative to an armarx package).
     * \li CollisionModel: The RobotNodeSet name to be used for collision testing with the environment.
     * \li WorkspaceFilePaths: Paths to manipulability and reachability files (';' delimited). Each path is relative to an armarx package.
     *
     * This component is a processing stage in the grasping pipeline run by the GraspingManager-Component.
     * It receives an object instance id and a set of grasps (i.e. global tcp poses and -names) via an Ice interface method and returns a list of possible
     * robot placements (i.e. global base poses). The generated robot placements are collisionfree between the collision model (given in the properties)
     * in its default configuration and the collision model of the environment. The collision model of the environment is created via the WorkingMemory-Instance.
     * Resulting robot placements and workspaceGrids are visualized via 'DebugDrawerUpdates'.
     */
 
    /**
     * @ingroup Component-SimpleRobotPlacement
     * @brief refer to \ref Component-SimpleRobotPlacement
     */
    class SimpleRobotPlacement :
        virtual public Component,
        virtual public RobotPlacementInterface
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "SimpleRobotPlacement";
        }
 
        struct RobotPlacement
        {
            float x;
            float y;
            float z;
            float score;
        };
 
        /**
         * @brief Computes a list of candidate robot placements for a list of grasps.
         * More precisely: computes global (x,y) coordinates and a yaw rotation for the robot's base pose such that the given object becomes reachable for the robot.
         * The (x,y) positions are computed based on Simox Workspacegrids created from the preloaded workspace files given in the properties.
         * This method tries to find (x,y) positions of high reachability by using a randomized approach, thus the returned robot placements are not
         * best possible solutions in terms of reachability.
         * The computed yaw rotations are chosen, such that the robot is directly facing the given object (i.e. the angle between the robot's x-axis and the object's (x,y) position is 0).
         * @param grasps    the grasps associated with the given object
         * @param objectInstanceEntityId the id of the object instance located in the WorkingMemory-Instance
         * @param c
         * @return A list of possible robot placements
         */
        GraspingPlacementList generateRobotPlacements(const GeneratedGraspList& grasps, const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief Computes a list of candidate robot placements for a grasp pose.
         *
         * @see generateRobotPlacements
         * @param endEffectorName Name of the Endeffector
         * @param target Pointer to the FramedPose of the target
         * @param planarObstacles List of planar obstacles
         * @param placementArea Convex hull of the placement Area
         * @param c Ice::Current
         * @return A list of possible robot placements
         */
        GraspingPlacementList generateRobotPlacementsForGraspPose(const std::string& endEffectorName, const FramedPoseBasePtr& target, const PlanarObstacleList& planarObstacles, const ConvexHull& placementArea, const Ice::Current& c = Ice::emptyCurrent) override;
 
 
        GraspingPlacementList generateRobotPlacementsEx(const GeneratedGraspList& grasps, const Ice::Current& c = Ice::emptyCurrent) override;
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return PropertyDefinitionsPtr(new SimpleRobotPlacementPropertyDefinitions(getConfigIdentifier()));
        }
 
        /**
         * @brief Tries to get a workspace with the same TCP as the EEF of the robot
         *
         * @param g generated Grasps
         * @return Pointer to the WorkspaceRepresentation
         */
        VirtualRobot::WorkspaceRepresentationPtr getWorkspaceRepresentation(const GeneratedGrasp& g);
    private:
        memoryx::WorkingMemoryInterfacePrx wm;
        memoryx::WorkingMemoryEntitySegmentBasePrx objectInstances;
        memoryx::AgentInstancesSegmentBasePrx agentInstances;
        memoryx::PriorKnowledgeInterfacePrx prior;
        memoryx::PersistentObjectClassSegmentBasePrx objectClasses;
 
        memoryx::GridFileManagerPtr fileManager;
 
        std::string robotName;
        std::string robotFilePath;
        std::string colModel;
        std::vector<std::string> workspaceFilePaths;
        std::vector<VirtualRobot::WorkspaceRepresentationPtr> workspaces;
        std::string drawRobotId;
        std::string visuLayerName;
        VirtualRobot::WorkspaceGridPtr visualizedGrid;
 
        armarx::DebugDrawerInterfacePrx debugDrawerPrx;
        memoryx::EntityDrawerInterfacePrx entityDrawerPrx;
        RobotIKInterfacePrx rik;
        RobotStateComponentInterfacePrx robotStateComponentPrx;
 
        VirtualRobot::RobotPtr robot;
        VirtualRobot::SceneObjectSetPtr sceneObjects;
        //        VirtualRobot::CDManager cd;
        SimoxCSpacePtr cspace;
        SimoxCSpacePtr cacheCSpace;
 
        std::vector<CSpaceVisualizerTaskHandle> planningTasks;
 
 
        VirtualRobot::WorkspaceGridPtr createWorkspaceGrid(GeneratedGrasp g, const Eigen::Matrix4f& globalObjectPose);
        void drawWorkspaceGrid(VirtualRobot::WorkspaceGridPtr reachGrid);
        // finds a collision free position, tries to maximize the position score
        bool getSuitablePosition(const GeneratedGrasp& g, VirtualRobot::WorkspaceGridPtr reachGrid, const Eigen::Matrix4f& globalGraspPose, float& storeGlobalX, float& storeGlobalY, float& storeGlobalYaw, int& score, const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea = VirtualRobot::MathTools::ConvexHull2DPtr());
 
        std::vector<RobotPlacement> getSuitablePositions(const GeneratedGrasp& g, VirtualRobot::WorkspaceGridPtr reachGrid, const Eigen::Matrix4f& globalGraspPose, float requiredReachabilityFraction, int requiredCount, int maxIterations, const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea = VirtualRobot::MathTools::ConvexHull2DPtr());
        // loads the robot given in the properties for collision testing
        void loadRobot();
        // loads the workspaces with the file paths given in the properties
        void loadWorkspaces();
        // checks whether there is a preloaded workspace with the given tcp
        bool hasWorkspace(std::string tcp);
        // filters all generated grasps, whose tcp is not given in any of the preloaded workspaces
        GeneratedGraspList filterGrasps(const GeneratedGraspList grasps);
        // returns the rotation needed for the robot to face the given target pose
        float getPlatformRotation(const Eigen::Matrix4f& frameGlobal, const Eigen::Matrix4f& globalTarget);
 
        void updateRobot(std::string id, Eigen::Matrix4f globalPose, DrawColor color);
        void drawNewRobot(Eigen::Matrix4f globalPose);
        void drawWorkspaceGrid(const ::armarx::GeneratedGrasp&, const std::string& objectInstanceEntityId);
    };
}