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 <VirtualRobot/CollisionDetection/CDManager.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/Workspace/Manipulability.h>
#include <VirtualRobot/Workspace/Reachability.h>
#include <VirtualRobot/Workspace/WorkspaceGrid.h>
 
#include <ArmarXCore/core/Component.h>
 
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
#include <RobotAPI/interface/core/RobotState.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include <RobotComponents/interface/components/RobotIK.h>
 
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/gui/EntityDrawerInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
//#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
//#include <RobotComponents/components/MotionPlanning/Tasks/AStar/Task.h>
#include <Eigen/Geometry>
 
#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
 
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);
    };
} // namespace armarx