grasp_chair.cpp

Go to the documentation of this file.

#include "grasp_chair.h"

#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <fstream>
#include <stdio.h>

#include <tf/tf.h>

#include <geometry_msgs/PointStamped.h>

GraspChair::GraspChair(simple_robot_control::Robot* robot, Gripper* gripper)
{  this->gripper = gripper;
   this->robot = robot;
   position_set = false;
}

bool GraspChair::grasp(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{   assert(robot && gripper);

        if (!position_set)
        {     ROS_WARN("no position yet to grasp");
                                  return false;
        }

        float dy = left_Gripper.point.y - right_Gripper.point.y;
        float dx = left_Gripper.point.x - right_Gripper.point.x;

        float alpha = atan2(dy, dx);

        ROS_INFO("alpha: %.1f %.1f %.1f" , dx *100, dy * 100, alpha/M_PI * 180);

        tf::Quaternion q2_right = tf::createQuaternionFromRPY(0, 0, alpha);
        //tf::Quaternion q2_right2 = tf::createQuaternionFromRPY(0, M_PI / 2, alpha);
        tf::Quaternion q2_left = tf::createQuaternionFromRPY(0, 0, -(M_PI - alpha));
        //tf::Quaternion q2_left2 = tf::createQuaternionFromRPY(0, M_PI / 2, -(M_PI - alpha));

        //tf::Quaternion corr_left = tf::createQuaternionFromRPY(0, 0, M_PI);
        tf::Quaternion q2_start = tf::createQuaternionFromRPY(0, 0, 0);

        Eigen::Vector3f vector_right_to_left(left_Gripper.point.x - right_Gripper.point.x, left_Gripper.point.y - right_Gripper.point.y, left_Gripper.point.z - right_Gripper.point.z);

        vector_right_to_left.normalize();

        Eigen::Vector3f position_Right(right_Gripper.point.x, right_Gripper.point.y, right_Gripper.point.z);
        Eigen::Vector3f position_Left(left_Gripper.point.x, left_Gripper.point.y, left_Gripper.point.z);

        Eigen::Vector3f preposition_Right = position_Right - 0.1 * vector_right_to_left;
        Eigen::Vector3f preposition_Left = position_Left + 0.15 * vector_right_to_left;

        ROS_INFO("Position_right %.1f %.1f %.1f" , position_Right.x(), position_Right.y(), position_Right.z());
        ROS_INFO("Position_left %.1f %.1f %.1f" , position_Left.x(), position_Left.y(), position_Left.z());

        //------preGrasp_Positions----------------------------
        tf::StampedTransform preGrasp_Right (tf::Transform(q2_right,
                        tf::Vector3(preposition_Right.x() - 0.03, preposition_Right.y(), preposition_Right.z())),
                        ros::Time::now(),"torso_lift_link","doesnt_matter");

        tf::StampedTransform preGrasp_Left (tf::Transform(q2_left,
                        tf::Vector3(preposition_Left.x() - 0.03, preposition_Left.y(), preposition_Left.z())),
                        ros::Time::now(),"torso_lift_link","doesnt_matter");

        tf::StampedTransform goal_right (tf::Transform(q2_right,
                   tf::Vector3(right_Gripper.point.x ,right_Gripper.point.y + 0.05 ,right_Gripper.point.z)),
                   ros::Time::now(),"torso_lift_link","doesnt_matter");

        tf::StampedTransform goal_left (tf::Transform(q2_left,
                   tf::Vector3(left_Gripper.point.x ,left_Gripper.point.y - 0.06 ,left_Gripper.point.z)),
                   ros::Time::now(),"torso_lift_link","doesnt_matter");

        //-----correction poses----------------------
        tf::StampedTransform correction_left (tf::Transform(q2_left,
                        tf::Vector3(preposition_Left.x() + 0.1, preposition_Left.y(), preposition_Left.z())),
                        ros::Time::now(), "torso_lift_link", "doesnt_matter");

        tf::StampedTransform correction_right (tf::Transform(q2_right,
                        tf::Vector3(preposition_Right.x() + 0.05, preposition_Right.y(), preposition_Right.z())),
                        ros::Time::now(), "torso_lift_link", "doesnt_matter");

        //------start poses--------------------------
        tf::StampedTransform start_pose_right (tf::Transform(q2_start,
                        tf::Vector3(0.4, - 0.5, -0.2)),
                        ros::Time::now(),"torso_lift_link","doesnt_matter");

        tf::StampedTransform start_pose_left (tf::Transform(q2_start,
                        tf::Vector3(0.4, 0.5, -0.2)),
                        ros::Time::now(),"torso_lift_link","doesnt_matter");

        //------start_configuration------------------------
        gripper->open_gripper('r');
        gripper->open_gripper('l');

        robot->right_arm.moveGrippertoPose(start_pose_right);
        robot->left_arm.moveGrippertoPose(start_pose_left);

        //------grasp object ideally------------------------------------
        bool rsuccess = robot->right_arm.moveGrippertoPose(preGrasp_Right);
        bool lsuccess = robot->left_arm.moveGrippertoPose(preGrasp_Left);

        if (rsuccess && lsuccess)
        {       ROS_INFO("could grasp the chair with both arms");
                robot->right_arm.moveGrippertoPose(goal_right);
                robot->left_arm.moveGrippertoPose(goal_left);
                gripper->close_gripper('l');
                gripper->close_gripper('r');
        }
        else if(!rsuccess)
        {       if(right_Gripper.point.x > left_Gripper.point.x)
                {       correction(robot->left_arm, 'l', correction_left, preGrasp_Left, goal_left);
                }
                return false;
        }
        else if(!lsuccess)
        {       if(left_Gripper.point.x > right_Gripper.point.x)
                {       correction(robot->right_arm, 'r', correction_right, preGrasp_Right, goal_right);
                }
                return false;
        }
        else //-------couldn't reach a goal--------------------------
        {       ROS_INFO("couldn't grasp object");
                return false;
        }
        return true;
}

bool GraspChair::release(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{       assert(robot && gripper);

                //------start pose--------------------------
                tf::Quaternion q2_start = tf::createQuaternionFromRPY(0, 0, 0);

                tf::StampedTransform start_pose_right (tf::Transform(q2_start,
                                tf::Vector3(0.4, - 0.5, -0.2)),
                                ros::Time::now(),"torso_lift_link","doesnt_matter");

                tf::StampedTransform start_pose_left (tf::Transform(q2_start,
                                tf::Vector3(0.4, 0.5, -0.2)),
                                ros::Time::now(),"torso_lift_link","doesnt_matter");

                //------start_pose------------------------
                gripper->open_gripper('r');
                gripper->open_gripper('l');

                robot->right_arm.moveGrippertoPose(start_pose_right);
                robot->left_arm.moveGrippertoPose(start_pose_left);

                return true;
}

void GraspChair::correction(simple_robot_control::Arm& arm, char lr, tf::StampedTransform corrGrasp, tf::StampedTransform preGrasp, tf::StampedTransform goalGrasp)
{               if(!arm.moveGrippertoPose(preGrasp))
                {       ROS_INFO("can't grasp the chair");
                }
                arm.moveGrippertoPose(goalGrasp);
                gripper->close_gripper(lr);
                arm.moveGrippertoPose(corrGrasp);
                gripper->open_gripper(lr);
}

void GraspChair::setGraspPositions_cb(const PointCloud::ConstPtr& msg)
{
        assert(msg->points.size() > 1);
        right_Gripper.header.frame_id = msg->header.frame_id;
        right_Gripper.header.stamp = msg->header.stamp;

        left_Gripper.header.frame_id = msg->header.frame_id;
        left_Gripper.header.stamp = msg->header.stamp;

        right_Gripper.point.x = msg->points[1].x;
        right_Gripper.point.y = msg->points[1].y;
        right_Gripper.point.z = msg->points[1].z;
        left_Gripper.point.x = msg->points[0].x;
        left_Gripper.point.y = msg->points[0].y;
        left_Gripper.point.z = msg->points[0].z;

        ROS_INFO_STREAM("Gripper_Point right: " << right_Gripper.point.x << "; " << right_Gripper.point.y << "; " << right_Gripper.point.z);
        ROS_INFO_STREAM("Gripper_Point left: " << left_Gripper.point.x << "; " << left_Gripper.point.y << "; " << left_Gripper.point.z);

        position_set = true;
}