NaturalGraspFilter.cpp

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
*/
 
#include <functional>
 
#include "NaturalGraspFilter.h"
 
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
using namespace armarx;
 
NaturalGraspFilter::NaturalGraspFilter()
{
}
 
void NaturalGraspFilter::onInitGraspSelectionCriterion()
{
    usingProxy("RobotStateComponent");
    //    usingProxy("LongtermMemory");
}
 
void NaturalGraspFilter::onConnectGraspSelectionCriterion()
{
    getProxy(rsc, "RobotStateComponent");
    localRobot = armarx::RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eCollisionModel);
    //    Enabling this would automatically register the SelectionCriterion with the GraspSelectionManager
    //    graspSelectionManager = getProxy<GraspSelectionManagerInterfacePrx>(getProperty<std::string>("GraspSelectionManagerName").getValue());
    //    graspSelectionManager->registerAsGraspSelectionCriterion(GraspSelectionCriterionInterfacePrx::uncheckedCast(getProxy()));
}
 
std::string NaturalGraspFilter::getDefaultName() const
{
    return "NaturalGraspFilter";
}
 
GeneratedGraspList NaturalGraspFilter::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
{
    GeneratedGraspList result;
    for (const GeneratedGrasp& g : grasps)
    {
        Eigen::Matrix3f orientation = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(localRobot).block<3, 3>(0, 0);
        Eigen::Vector3f rotatedYAxis = orientation * Eigen::Vector3f::UnitY();
        rotatedYAxis.normalize();
        if (rotatedYAxis(2) > 0.1)
        {
            ARMARX_WARNING << g.graspName << " seems to be an unnatural Grasp; it will not be used further!";
            continue;
        }
        result.push_back(g);
    }
    return result;
}
 
GraspingPlacementList
NaturalGraspFilter::filterPlacements(const GraspingPlacementList& placements, const Ice::Current&)
{
    GraspingPlacementList result;
    ARMARX_INFO << "Number of Placements: " << placements.size();
    //    TODO: Create a Ranking function; maybe rank for the fewest angles in the kinematic chain.
 
    for (const GraspingPlacement& placement : placements)
    {
        //        Note: something goes wrong if auto is used
        Eigen::Matrix3f robotOrientation = FramedPosePtr::dynamicCast(placement.robotPose)->toGlobalEigen(localRobot).block<3, 3>(
                                               0, 0);
        Eigen::Matrix3f graspOrientation = FramedPosePtr::dynamicCast(placement.grasp.framedPose)->toGlobalEigen(
                                               localRobot).block<3, 3>(0, 0);
        Eigen::Matrix3f graspToRobotTransform = robotOrientation.inverse() * graspOrientation;
        Eigen::Vector3f graspXAxisInRobotCoordinates = graspToRobotTransform * Eigen::Vector3f::UnitX();
        graspXAxisInRobotCoordinates.normalize();
        float x = graspXAxisInRobotCoordinates(0);
        float y = graspXAxisInRobotCoordinates(1);
        float z = graspXAxisInRobotCoordinates(2);
        //        Eigen::Vector2f Projection2D {graspXAxisInRobotCoordinates(0), graspXAxisInRobotCoordinates(1)};
        //      TODO: Don't hardcode this. Use the left eefName smhw;
        //      TODO: check if the values are sensible;
        bool isInSegment = false;
        if (placement.grasp.eefName == "Hand_L_EEF")
        {
            //            isInSegment = vectorIsWithinCircleSegment(Projection2D, 0.333 * M_PI , 1.1666 * M_PI);
            isInSegment = (x > -0.1 && (y < 0 || x * x + z * z > 0.5 * 0.5));
        }
        else
        {
            //            isInSegment = vectorIsWithinCircleSegment(Projection2D, -M_PI / 6., 0.666 * M_PI);
            isInSegment = (x > -0.1 && (y > 0 || x * x + z * z > 0.5 * 0.5));
        }
        if (isInSegment)
        {
            result.push_back(placement);
        }
    }
    return result;
}
 
Ice::Int NaturalGraspFilter::getHash(const Ice::Current&) const
{
    return std::hash<std::string> {}(graspSelectionManager->ice_getIdentity().category + "." + graspSelectionManager->ice_getIdentity().name);
}
 
bool NaturalGraspFilter::vectorIsWithinCircleSegment(const Eigen::Vector2f vector, double angle1, double angle2)
{
    ARMARX_CHECK_EXPRESSION(std::abs(angle2 - angle1) < M_PI) <<
            "Works only for circle segments smaller than Pi and is " << std::abs(angle2 - angle1);
    Eigen::Rotation2Df rot1(angle1);
    Eigen::Rotation2Df rot2(angle2);
    Eigen::Vector2f boundary1 = rot1 * Eigen::Vector2f::UnitX();
    Eigen::Vector2f boundary2 = rot2 * Eigen::Vector2f::UnitX();
    bool vectorIsCounterClockwiseToBoundary1 = boundary1(0) * vector(1) - boundary1(1) * vector(0) > 0;
    bool vectorIsClockwiseToBoundary2 = - boundary2(0) * vector(1) + boundary2(1) * vector(0) > 0;
    return vectorIsCounterClockwiseToBoundary1 && vectorIsClockwiseToBoundary2;
}