GraspingManager.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::GraspingManager
 * @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
 */
 
#pragma once
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/TimeKeeper.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.h>
#include <RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.h>
#include <RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.h>
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
#include <RobotComponents/interface/components/PlannedMotionProviderInterface.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/gui/EntityDrawerInterface.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
namespace armarx
{
    /**
     * @class GraspingManagerPropertyDefinitions
     * @brief
     */
    class GraspingManagerPropertyDefinitions : public ComponentPropertyDefinitions
    {
    public:
        GraspingManagerPropertyDefinitions(std::string prefix) :
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "GraspGeneratorName", "SimpleGraspGenerator", "Name of the GraspGenerator proxy");
            defineOptionalProperty<std::string>(
                "RobotPlacementName", "SimpleRobotPlacement", "Name of the RobotPlacement proxy");
            defineOptionalProperty<std::string>(
                "RobotNodeSetNames",
                "HipYawRightArm;HipYawLeftArm",
                "Names of the robot node sets to use for IK calculations (';' delimited)");
            defineOptionalProperty<std::string>(
                "RobotCollisionNodeSet",
                "Robot",
                "Name of the collision nodeset used for motion planning",
                PropertyDefinitionBase::eModifiable);
            defineRequiredProperty<std::string>(
                "JointToLinkSetMapping",
                "Mapping from joint set to link set, i.e. which collision set to use for which "
                "kinematic chain. Format: JointSet1:CollisionSet1;JointSet2:CollisionSet2",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<std::string>(
                "ReachabilitySpaceFilePaths",
                "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/"
                "reachability_left_hand_smoothened.bin",
                "Paths to the reachability space files (needed only if no reachability space is "
                "loaded for the chosen RobotNodeSets) (';' delimited)");
            defineOptionalProperty<std::string>("PlanningBoundingBox",
                                                "-15000, -4000, -3.1416, 15000, 15000, 3.1416",
                                                "x_min, y_min, alpha_min, x_max, y_max, alpha_max");
            defineOptionalProperty<float>("VisualizationSlowdownFactor",
                                          1.0f,
                                          "1.0 is a good value for clear visualization, 0 the "
                                          "visualization should not slow down the process",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("EnableVisualization",
                                         true,
                                         "If false no visualization is done.",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>(
                "MaxDistanceForDirectGrasp",
                1200.0f,
                "x-y-Distance from robot base to object for which the grasping manager tries to "
                "find a solution without platform movement",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>(
                "UseVoxelGridCSpace",
                false,
                "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<std::string>("VoxelGridProviderName",
                                                "VoxelGridProvider",
                                                "Name of the Voxel Grid Provider",
                                                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("FilterUnnaturalGrasps",
                                         false,
                                         "If set to true, uses the NaturalGraspFilter to exclude "
                                         "unnatural looking grasps and grasp poses.",
                                         PropertyDefinitionBase::eModifiable);
        }
    };
 
    /**
     * @defgroup Component-GraspingManager GraspingManager
     * @ingroup RobotComponents-Components
     * @brief Provides methods for grasp planning purposes.
     *
     * The GraspingManager implements a grasping pipeline containing the following stages (each stage is implemented by the respective Component in the parathesis):
     * \li Grasp generation (SimpleGraspGenerator)
     * \li Grasp filtering (GraspSelectionManager)
     * \li Robot placement (SimpleRobotPlacement)
     * \li Finding a target configuration (RobotIK)
     * \li Trajectory planning (MotionPlanningServer)
     *
     * The stages are executed sequentially.
     * The Input for the grasping pipeline is a graspable object.
     * The output contains trajectories in configuration and pose space to execute a grasp for the given object.
     *
     * The GraspingManager requires the following properties:
     * \li a list of robotnodeset names (; delimited) for IK calculations
     * \li a list of workspace files (; delimited) for reachability considerations
     *
     * \note The Armar3Simulation scenario must be active before running the GraspingManager scenario.
     */
 
    /**
     * @ingroup Component-GraspingManager
     * @brief The GraspingManager class
     */
    class GraspingManager :
        virtual public Component,
        virtual public GraspingManagerInterface,
        virtual public NaturalIKComponentPluginUser
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "GraspingManager";
        }
 
        /**
         * @brief Runs the grasping pipeline.
         *
         * Checks if the robot is close enough to the object to directly grasp it, attempting to calculate the
         * inverse kinematics directly. In case nothing is found, tries to calculate the movement a robotPlacement
         * is generated and the calculation of the IKs is repeated for the new position.
         *
         * @param objectInstanceEntityId the id for retrieval of the target object from working memory.
         * @param c
         * @return A list of grasping trajectories in configuration and pose space.
         */
        GraspingTrajectory
        generateGraspingTrajectory(const std::string& objectInstanceEntityId,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        GraspingTrajectoryList
        generateGraspingTrajectoryList(const std::string& objectInstanceEntityId,
                                       const Ice::Current& c = Ice::emptyCurrent) override;
        GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(
            const GeneratedGraspList& grasps,
            const Ice::Current& c = Ice::emptyCurrent) override;
        void visualizeGraspingTrajectory(const GraspingTrajectory& trajectory,
                                         float visuSlowdownFactor,
                                         const Ice::Current& c = Ice::emptyCurrent) override;
 
        MotionPlanningDataList generateIKs(const std::string& objectInstanceEntityId,
                                           const Ice::Current& c = Ice::emptyCurrent) override;
 
 
        ///// single step methods /////
        GeneratedGraspList
        generateGraspsByObjectName(const std::string& objectName,
                                   const Ice::Current& = 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 GraspingManagerPropertyDefinitions(getConfigIdentifier()));
        }
 
        /**
         * @brief Visualizes the grasp using the DebugDrawer topic
         */
        std::pair<std::string, Ice::StringSeq>
        visualizeGrasp(const GeneratedGrasp& grasp,
                       int id,
                       const DrawColor& color = DrawColor{1, 1, 1, 1});
 
    private:
        /**
         * @brief Creates a pointer to the SimoxCSpace::SimoxCSpace() and initializes it.
         *
         * Creates the pointer, adds objects from the working memory and sets up the AgentPlanningInformation.
         * The Agent is then linked to the pointer and the CSpace is initialized.
         * @return Pointer to a SimoxCSpace
         */
        SimoxCSpacePtr createCSpace();
 
        /**
         * @brief Wrapper for the GraspGeneratorInterface::generateGrasps() method of the GraspGenerator.
         * @param objectInstanceEntityId
         *
         * Gets a list of grasps from the GraspGenerator and sorts them depending on their score. If no grasps are
         * generated, a warning is thrown. Initiates the visualization of the grasps.
         * @return
         */
        GeneratedGraspList generateGrasps(const std::string& objectInstanceEntityId);
 
        /**
         * @brief Wrapper for the GraspSelectionManagerInterface::filterGrasps() method of the GraspSelectionManager.
         */
        GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps);
        /**
         * @brief Wrapper for the GraspSelectionManagerInterface::filterPlacements() method o
         * the GraspSelectionManager.
         */
        GraspingPlacementList filterPlacements(const GraspingPlacementList& placements);
 
        /**
         * Wrapper for the RobotPlacementInterface::generateRobotPlacements().
         * @param grasps
         * @param objectInstanceEntityId
         * @return List of grasping placements.
         */
        GraspingPlacementList generateRobotPlacements(const GeneratedGraspList& grasps);
 
        /**
         * Wrapper for the PlannedMotionProviderInterface::planMotion() method.
         *
         * Calculates the center and size of the bounding box and initializes one SimoxCSpace for the arms and one for
         * the platform.
         *
         * @param mpd
         * @return
         */
        GraspingTrajectory planMotion(const MotionPlanningData& mpd);
 
        /**
         * @brief Draws the trajectory in the DebugDrawer topic.
         * @param t
         */
        void drawTrajectory(const GraspingTrajectory& t, float visuSlowdownFactor);
 
        /**
         * @brief Iterates through the list of found graspPlacements to calculate SingleIKs
         *
         * Iterates through all found graspPlacements and calls the GraspingManager::calculateSingleIK() method with
         * the correct parameters. Then calculates the required parameters, like the currentRobotPose, GlobalFrame,
         * desiredRobotPose, currentConfig, ikSolution, rnsToUse, gp.grasp.eefName and gp.grasp, for the
         * MotionPlanningData for each non-empty IKSolution.
         *
         * @return Vector of of MotionPlanningData
         */
        std::vector<MotionPlanningData> calculateIKs(const GraspingPlacementList& graspPlacements);
 
        /**
         * @brief Calculates the inverse kinematics for a single grasp.
         *
         * Calculates the inverse kinematics for a target grasp configuration. If checkCollisionFree is true, the arms
         * and the platform are added as constraints in the calculation. Otherwise, only the position and orientation
         * of the rns, tcp & target_pose are considered.
         *
         * @param robotNodeSetName
         * @param eef
         * @param globalRobotPose
         * @param tcpPose
         * @param checkCollisionFree
         * @return NameValueMap of the solved IK
         */
        NameValueMap calculateSingleIK(const ::std::string& robotNodeSetName,
                                       const std::string& eef,
                                       const PoseBasePtr& globalRobotPose,
                                       const ::armarx::FramedPoseBasePtr& tcpPose,
                                       bool checkCollisionFree = true);
 
        /**
         * Wrapper for the property JointToLinkSetMapping
         *
         * @return Map of each pair in the JointSetCollisionSet
         */
        armarx::StringStringDictionary getJointSetCollisionSetMapping();
 
        /**
         * Generate a list of valid GraspingTrajectories given a list of grasps.
         *
         * @param grasps The list of grasps to generate trajetories for
         * @param objId The object ID. Only used for logging.
         * @return The list of valid generated grasps
         */
        GraspingTrajectoryList
        generateGraspingTrajectoryListForGraspListInternal(const GeneratedGraspList& grasps);
        MotionPlanningDataList generateIKsInternal(const GeneratedGraspList& grasps);
 
        std::vector<std::string> getRobotNodeSetNodesWithoutAllwaysColliding(
            const std::vector<std::string>& robotColNodeNames,
            const std::string& armCollisionSet);
 
        void setDescriptionPositionForObject(const std::string& objId);
        void setNextStepDescription(const std::string& description);
        void resetStepDescription();
        std::string graspGeneratorName;
        std::string robotPlacementName;
        std::vector<std::string> robotNodeSetNames;
        std::vector<std::string> reachabilitySpaceFilePaths;
 
        Vector3fRange planningBoundingBox;
        GraspGeneratorInterfacePrx gg;
        GraspSelectionManagerInterfacePrx gsm;
        GraspSelectionCriterionInterfacePrx gsc;
        RobotPlacementInterfacePrx rp;
 
        PlannedMotionProviderInterfacePrx pmp;
        SimoxCSpacePtr cacheCSpace;
 
        //RobotIKInterfacePrx rik;
        RobotStateComponentInterfacePrx rsc;
        VirtualRobot::RobotPtr localRobot, ikRobot;
        SimoxCSpacePtr cspace;
 
        memoryx::CommonStorageInterfacePrx cs;
        memoryx::WorkingMemoryInterfacePrx wm;
        memoryx::PriorKnowledgeInterfacePrx prior;
 
        memoryx::EntityDrawerInterfacePrx entityDrawer;
        std::string layerName;
        std::string robotVisuId;
 
        int step = 0;
        Vector3Ptr globalDescriptionPosition;
 
        std::vector<CSpaceVisualizerTaskHandle> planningTasks;
        Mutex graspManagerMutex;
        armarx::TimeKeeper stepTimer;
 
        RobotNameHelperPtr rnh;
    };
} // namespace armarx