NaturalGraspFilter.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    RobotComponents
* @author     Christoph Pohl ( christoph.pohl at kit dot edu)
* @date       2016
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.h>
#include <MemoryX/components/CommonPlacesLearner/CommonPlacesLearner.h>
#include <ArmarXCore/core/Component.h>
 
namespace armarx
{
    class NaturalGraspFilterPropertyDefinitions:
        public GraspSelectionCriterionPropertyDefinitions
    {
    public:
        NaturalGraspFilterPropertyDefinitions(std::string prefix):
            GraspSelectionCriterionPropertyDefinitions(prefix)
        {
            //            defineOptionalProperty<std::string>("CommonPlacesLearnerName", "CommonPlacesLearnerGraspSelectionCriterion", "The CommonPlacesLearner to use");
        }
    };
 
    class NaturalGraspFilter : public GraspSelectionCriterionBase
    {
    public:
        NaturalGraspFilter();
 
        // ManagedIceObject interface
    protected:
        void onInitGraspSelectionCriterion() override;
        void onConnectGraspSelectionCriterion() override;
        std::string getDefaultName() const override;
 
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(
                       new NaturalGraspFilterPropertyDefinitions(
                           getConfigIdentifier()));
        }
 
    public:
        GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&) override;
 
        /*****
         * @brief Filters Grasps that are unnatural from the human point of view
         *
         * Checks if the X-Axis of the Grasp (points out of the back of the Hand for the Right Hand and into the
         * direction for the Left Hand) is in the Unit-3Sphere in Robot Coordinates with the left half excluded
         * (x < -0.1) and a circle cut out towards the robot; Based on heuristics for naturalness of a grasp.
         **/
        GraspingPlacementList filterPlacements(const GraspingPlacementList& placements, const Ice::Current&) override;
 
        Ice::Int getHash(const Ice::Current&) const override;
    private:
        RobotStateComponentInterfacePrx rsc;
        VirtualRobot::RobotPtr localRobot;
        bool vectorIsWithinCircleSegment(const Eigen::Vector2f vector, double angle1, double angle2);
    };
 
} // namespace spoac