grasp_generator.h

Go to the documentation of this file.

#ifndef IRI_GRASP_ACTIONS_GUARD_INCLUDE_GRASP_GENERATOR_H
#define IRI_GRASP_ACTIONS_GUARD_INCLUDE_GRASP_GENERATOR_H 1

#include <tf/transform_datatypes.h>
#include <iri_wam_common_msgs/bhand_cmd.h>
#include <iri_wam_common_msgs/SimpleBhandPickUpAction.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <object_manipulation_msgs/GripperTranslation.h>

enum GraspPhaseType
{
    SET_FINGER_POSITION,
    MOVE_ARM
};

class GraspPhase
{
    public:
        GraspPhaseType id_;

        GraspPhase(GraspPhaseType id) :
            id_(id)
        { }
};

typedef std::vector<std::string> GraspFingersConfiguration;

class GraspPhaseSetFingers : public GraspPhase
{
    public:
        GraspFingersConfiguration finger_config_;

        GraspPhaseSetFingers() :
            GraspPhase(SET_FINGER_POSITION)
        { }

        GraspPhaseSetFingers(GraspFingersConfiguration finger_config) :
            GraspPhase(SET_FINGER_POSITION),
            finger_config_(finger_config)
        { }
};

class GraspPhaseMoveArm : public GraspPhase
{
    public:
        geometry_msgs::PoseStamped pose_;

        GraspPhaseMoveArm(const geometry_msgs::PoseStamped pose) :
            GraspPhase(MOVE_ARM),
            pose_(pose)
        { }
};

typedef boost::shared_ptr<GraspPhase> GraspPhasePtr;

typedef std::vector<GraspPhasePtr> GraspPhaseSequence;

class GraspGenerator
{
    public:
        GraspGenerator()
        { }

        GraspPhaseSequence generate(iri_wam_common_msgs::SimpleBhandPickUpGoal goal)
        {
            GraspPhaseSequence cmd_list;

            // 0.a Convert pre-grasp in a stamped pose
            // 0.b Convert Lift in stamped pose
            geometry_msgs::PoseStamped pre_grasp_st = calculate_pre_grasp(goal.grasp_pose,
                                                                          goal.pre_grasp_modifier);
            geometry_msgs::PoseStamped lift_st      = calculate_lift(goal.grasp_pose, goal.lift);

            // 1. Move to pre-grasp position
            cmd_list.push_back(GraspPhasePtr(new
                                    GraspPhaseMoveArm(pre_grasp_st)));
            // 2. Pre-grasp finger posture
            cmd_list.push_back(GraspPhasePtr(new
                                   GraspPhaseSetFingers(goal.fingers_position_for_pre_grasp)));
            // 3. Move to grasp position
            cmd_list.push_back(GraspPhasePtr(new
                                    GraspPhaseMoveArm(goal.grasp_pose)));
            // 4. Grasp finger posture
            cmd_list.push_back(GraspPhasePtr(new
                                   GraspPhaseSetFingers(goal.fingers_position_for_grasp)));
            // 5. Lift movement
            cmd_list.push_back(GraspPhasePtr(new
                                   GraspPhaseMoveArm(lift_st)));

            return cmd_list;
        }

    private:
        geometry_msgs::PoseStamped calculate_pre_grasp(const geometry_msgs::PoseStamped grasp_pose,
                                                       const geometry_msgs::Pose pre_grasp)
        {
            geometry_msgs::PoseStamped final_pre_grasp;

            tf::Pose grasp_tf;
            tf::Pose pre_grasp_tf;
            tf::Pose result_tf;

            // 1. Get the trasnforms from both poses
            tf::poseMsgToTF(grasp_pose.pose, grasp_tf);
            tf::poseMsgToTF(pre_grasp,       pre_grasp_tf);

            result_tf = grasp_tf * pre_grasp_tf;

            // Save the info into the returning message
            final_pre_grasp.header = grasp_pose.header;
            tf::poseTFToMsg(result_tf, final_pre_grasp.pose); // will fill pose

            return final_pre_grasp;
        }

        geometry_msgs::PoseStamped calculate_lift(
                                 const geometry_msgs::PoseStamped grasp_pose,
                                 const object_manipulation_msgs::GripperTranslation gripper_trans)
        {
            geometry_msgs::PoseStamped result;
            geometry_msgs::Vector3     displacement;

            // Normalize the vector and apply the distance
            tf::Vector3 v_direction;
            tf::Vector3 v_displacement;
            tf::vector3MsgToTF(gripper_trans.direction.vector, v_direction);

            v_displacement =  v_direction.normalize() * gripper_trans.desired_distance;
            tf::vector3TFToMsg(v_displacement, displacement);

            result.header            = grasp_pose.header;
            result.pose.orientation  = grasp_pose.pose.orientation;
            // geometry::point can not be sum with the vector3 so ... ugly
            result.pose.position.x   = grasp_pose.pose.position.x + displacement.x;
            result.pose.position.y   = grasp_pose.pose.position.y + displacement.y;
            result.pose.position.z   = grasp_pose.pose.position.z + displacement.z;

            return result;
        }
};


#endif