gripper_stereo_reactive_grasp.py

Go to the documentation of this file.

#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the Willow Garage nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# author: Kaijen Hsiao

## @package gripper_stereo_reactive_grasp
#Reactive grasping based on fingertip readings and gripper stereo

from __future__ import division
import roslib
roslib.load_manifest('pr2_gripper_stereo')
import time
import rospy
import os
import pr2_gripper_reactive_approach.controller_manager as controller_manager
import sys
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Vector3
from object_manipulation_msgs.msg import GripperTranslation, ManipulationPhase
import tf
import scipy
import scipy.linalg
import math
import pdb
import pr2_gripper_reactive_approach.hand_sensor_listeners as hand_sensor_listeners
from object_manipulator.convert_functions import *
import copy
from actionlib_msgs.msg import GoalStatus
from pr2_gripper_reactive_approach.reactive_grasp import ReactiveGrasper, Aborted
from pr2_gripper_stereo.srv import QueryGripperStereo, QueryGripperStereoRequest, GripperStereoGraspAdjust, GripperStereoGraspAdjustRequest, GripperStereoGraspAdjustResponse
import object_manipulator.draw_functions as draw_functions


##reactive/guarded movement and grasping with the gripper stereo integrated into the approach
class GripperStereoReactiveGrasper(ReactiveGrasper):

  def __init__(self, cm):

    ReactiveGrasper.__init__(self, cm)

    #client for gripper stereo
    rospy.loginfo("waiting for gripper stereo service")
    rospy.wait_for_service("/gripper_stereo_grasp_adjust")
    rospy.loginfo("gripper service found")
    self._gripper_stereo_grasp_adjust_client = rospy.ServiceProxy("/gripper_stereo_grasp_adjust", GripperStereoGraspAdjust)

    self.stereo_adjusted = 0

    #class for drawing poses
    self.draw_functions = draw_functions.DrawFunctions('grasp_markers')


#   #query the gripper stereo camera to figure out which direction to move (simple version)
#   def query_gripper_stereo_simple(self, dist_to_obj):
#     req = QueryGripperStereoRequest()
#     req.roi_frame = create_pose_stamped([dist_to_obj+.185-.035, 0, 0, 0, 0, 0, 1], self.whicharm+'_wrist_roll_link')
#     #req.roi_box_dims = Vector3(.1, .4, .4)
#     req.roi_box_dims = Vector3(.08, .15, .1)

#     try:
#       resp = self._gripper_stereo_simple_client(req)
#     except rospy.ServiceException, e:
#       rospy.logerr("error when querying gripper stereo, %s"%e)
#       return None
#     return resp.object_frame


  #adjust a grasp using the point cloud from the gripper stereo camera 
  #returns adjusted grasp pose, result (0=success, 1=failed to adjust, 2=grasp impossible)
  def gripper_stereo_grasp_adjust(self, grasp_pose, do_global_search = 1):

    req = GripperStereoGraspAdjustRequest()
    req.grasp_pose = grasp_pose
    req.roi_dims = Vector3(.1, .3, .3)
    req.do_global_search = do_global_search

    try:
      resp = self._gripper_stereo_grasp_adjust_client(req)
    except rospy.ServiceException, e:
      rospy.logerr("error when asking gripper stereo to adjust grasp, %s"%e)
      return ([grasp_pose], 1)
    return (resp.adjusted_grasp_pose, resp.result)


  #simple reactive far-approach using the gripper stereo (move to dist away from grasp_pose)
  #stop if the fingertip sensors hit something
  def gripper_stereo_reactive_approach(self, approach_dir, grasp_pose, stop_at_dist = 0.05):

    left_touching = right_touching = palm_touching = 0
    new_goal = grasp_pose
    new_approach_dir = approach_dir.copy()

    if self.stereo_adjusted == 1:
      rospy.loginfo("gripper stereo adjustment already done")
      return (0, 0, 0, grasp_pose, approach_dir, 1)

    #compute the distance of the gripper's current pose from the new_goal along approach_dir
    (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(new_goal, approach_dir)
    dist_to_obj = scipy.linalg.norm(proj_vect)

    #keep moving toward the object, adjusting the grasp pose with each 1 cm step inwards
    #while not rospy.is_shutdown():
    
    #adjust the grasp in a direction orthogonal to approach_dir
    grasp_feasible = True
#    if use_simple:
#      (left_touching, right_touching, palm_touching, new_goal) = self.adjust_using_gripper_stereo_simple(approach_dir, .10, new_goal, dist_to_obj)
#    else:
    for i in range(2):
      do_global_search = 1
      if i > 0:
        print "Pausing 1.0 sec..."
        rospy.sleep(1.0)
        do_global_search = 0
      (left_touching, right_touching, palm_touching, new_goal, new_approach_dir, grasp_feasible) = self.adjust_using_gripper_stereo(new_goal, new_approach_dir, do_global_search)

      #return if we hit something or if the grasp is infeasible
      if left_touching or right_touching or palm_touching or not grasp_feasible:
        break

#     #compute the distance of the gripper's current pose from the new_goal along approach_dir
#     (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(new_goal, approach_dir)
#     dist_to_obj = scipy.linalg.norm(proj_vect)

#     #how far to move towards the grasp
#     dist_to_move = dist_to_obj - stop_at_dist
# #       if dist_to_move > .01:
# #         dist_to_move = .01
# #       elif dist_to_move < .005:
# #         break

#     #move in another step along approach_dir
#     move_in_vect = new_approach_dir * dist_to_move
#     pose = self.return_rel_pose(move_in_vect, 'base_link')
#     (left_touching, right_touching, palm_touching) = self.guarded_move_ik(pose)

#     #return if we hit something
#     if left_touching or right_touching or palm_touching:
#       return (left_touching, right_touching, palm_touching, new_goal, grasp_feasible)

    self.stereo_adjusted = 1

    return (left_touching, right_touching, palm_touching, new_goal, new_approach_dir, grasp_feasible)


  #move to dist_to_obj away along the gripper x-axis from the new grasp goal
  def adjust_using_gripper_stereo(self, grasp_goal, approach_dir, do_global_search = 1):

    print "grasp_goal: "+str(grasp_goal)

    #try to adjust the grasp up to 3 times (stop if we found one in reach and tried it)
    for tries in range(3):
      rospy.loginfo("asking gripper stereo to adjust the grasp, try number %d"%tries)

      #where the gripper stereo thinks we should grasp
      (new_goal_list, result) = self.gripper_stereo_grasp_adjust(grasp_goal, do_global_search)

      #grasp is impossible!  
      if result == GripperStereoGraspAdjustResponse.IMPOSSIBLE:
        rospy.loginfo("gripper stereo adjustment returned grasp impossible!")
        continue

      if result == GripperStereoGraspAdjustResponse.FAILURE:
        rospy.loginfo("gripper stereo adjustment returned failed!")
        continue

      #try to reach each goal in turn
      for new_goal in new_goal_list:

        print "new_goal: "+str(new_goal)

        #run interpolated IK to see if new_goal is reachable
        current_angles = self.cm.get_current_arm_angles()
        current_pose = self.cm.get_current_wrist_pose_stamped()
        (trajectory, error_codes) = self.cm.ik_utilities.check_cartesian_path(current_pose, new_goal, current_angles, collision_aware = 0)
        print "error_codes:", error_codes
        if any(error_codes):
          rospy.loginfo("can't execute an interpolated IK trajectory to new_goal, moving onto the next")
          continue
        else:
          rospy.loginfo("found interpolated IK trajectory")

        #convert to base link
        new_goal = change_pose_stamped_frame(self.cm.tf_listener, new_goal, 'base_link')

        #separate the vector from our current pose to new_goal into projected and orthogonal components
        (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(grasp_goal, approach_dir)

        #current distance to the goal pose along approach_dir
        dist_to_obj = scipy.linalg.norm(proj_vect)

        #compute the pose dist_to_obj away from new_goal along the gripper x-axis (turns into base_link)
        new_goal_mat = pose_to_mat(new_goal.pose)
        dist_to_obj_mat = new_goal_mat.copy()
        dist_to_obj_mat[0:3, 3] += new_goal_mat[0:3, 0] * -dist_to_obj
        dist_to_obj_pose = stamp_pose(mat_to_pose(dist_to_obj_mat), 'base_link')

        #return the new approach dir in base_link frame
        new_approach_dir = scipy.array(new_goal_mat[0:3, 0].T)[0]

    #     print "approach_dir: "+pplist(approach_dir)
    #     print "proj_vect: "+pplist(proj_vect)
    #     print "orthogonal_vect: "+pplist(orthogonal_vect)
        print "dist_to_obj: %0.3f"%dist_to_obj
        print "dist_to_obj_pose: "+str(dist_to_obj_pose)

        #draw the desired pose
        self.draw_functions.draw_rviz_axes(dist_to_obj_mat, 'base_link', id = 0, duration = 30)

        #use IK to move the gripper to that pose
        (left_touching, right_touching, palm_touching) = self.guarded_move_ik(dist_to_obj_pose, max_joint_vel = .15)
        break

      #didn't find a reachable grasp pose on this try
      else:
        continue

      #executed a grasp adjustment, stop
      break

    #no tries succeeded in finding a reachable grasp pose
    else:
        return (0, 0, 0, grasp_goal, approach_dir, False)

    return (left_touching, right_touching, palm_touching, new_goal, approach_dir, True)


  #vector from our current pose origin to the goal origin, separated into projected/orthogonal vectors to approach_dir
  def projected_vect_to_obj(self, goal, approach_dir):

    #convert to base_link
    bl_goal_frame = change_pose_stamped_frame(self.cm.tf_listener, goal, 'base_link')

    #vector from our current pose to the goal frame
    current_pose = self.cm.get_current_wrist_pose_stamped()
    goal_frame_pos = scipy.array(get_xyz(bl_goal_frame.pose.position))
    current_frame_pos = scipy.array(get_xyz(current_pose.pose.position))
    toward_goal_frame = goal_frame_pos - current_frame_pos

    #projection of toward_goal_frame onto approach_dir
    proj = self.vect_proj(toward_goal_frame, approach_dir)

    #subtract out the projection to find the orthogonal component
    orthogonal_vect = toward_goal_frame - proj

#     print "bl_goal_frame: "+str(bl_goal_frame)
#     print "current_pose: "+str(current_pose)
#     print "toward_goal_frame "+pplist(toward_goal_frame)

    return (proj, orthogonal_vect)


#   #make a small adjustment in the gripper's position in a direction orthogonal to approach_dir, based on the gripper stereo's object frame
#   def adjust_using_gripper_stereo_simple(self, approach_dir, max_dist, grasp_goal, dist_to_obj):
    
#     #where the gripper stereo thinks the object is
#     object_frame = self.query_gripper_stereo_simple(dist_to_obj)

#     #find the vector from our current pose to the object frame projected onto approach_dir
#     (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(object_frame, approach_dir)

#     #clip the magnitude at max_dist
#     orthogonal_vect_mag = scipy.linalg.norm(orthogonal_vect)
#     if orthogonal_vect_mag > max_dist:
#       orthogonal_vect = orthogonal_vect / orthogonal_vect_mag * max_dist

#     #move the gripper
#     pose = self.return_rel_pose(orthogonal_vect, 'base_link')
#     (left_touching, right_touching, palm_touching) = self.guarded_move_ik(pose, max_joint_vel = .15)

#     #update the grasp_goal
#     new_goal = self.return_rel_pose(orthogonal_vect, 'base_link', grasp_goal)

# #     print "approach_dir: "+pplist(approach_dir)
# #     print "object_frame_pos: "+pplist(object_frame_pos)
# #     print "current_frame_pos: "+pplist(current_frame_pos)
# #     print "toward_object_frame: "+pplist(toward_object_frame)
# #     print "proj: "+pplist(proj)
# #     print "orthogonal_vect: "+pplist(orthogonal_vect)

#     return (left_touching, right_touching, palm_touching, new_goal)
  

  ##reactive approach (stop if unexpected fingertip contact and go around)
  #assumes we are already at the approach pose, going to grasp_pose along the direction approach_dir
  #grasp_pose should be in base_link frame
  #and have found and checked the joint-angle path
  #side_step is how far to move right or left when you hit something
  #num_tries is how many bump-and-moves to try before quitting
  def reactive_approach(self, approach_dir, grasp_pose, joint_path = None, 
                        side_step = .015, back_step = .03, \
                          num_tries = 10, goal_pos_thres = 0.01):
    
    rospy.loginfo("running gripper stereo reactive approach")
    (left_touching, right_touching, palm_touching, grasp_pose, approach_dir, grasp_feasible) = self.gripper_stereo_reactive_approach(approach_dir, grasp_pose, stop_at_dist = 0.05)

    if not grasp_feasible:
      rospy.loginfo("grasp infeasible!")
      return self.reactive_approach_result_dict["grasp infeasible"]      

    rospy.loginfo("running normal approach")
    rospy.loginfo("new grasp pose: "+str(grasp_pose))
    ReactiveGrasper.reactive_approach(self, approach_dir, grasp_pose, joint_path, side_step,
                                      back_step, num_tries, goal_pos_thres)

  #overloaded to only do gripper stereo adjust once
  def reactive_grasp(self, approach_pose, grasp_pose, joint_path = None, side_step = .015, back_step = .03, \
                       approach_num_tries = 10, goal_pos_thres = 0.01, min_gripper_opening = 0.0021, max_gripper_opening = 0.1, \
                       grasp_num_tries = 2, forward_step = 0.03, min_contact_row = 1, object_name = "points", table_name = "table", grasp_adjust_x_step = .02, grasp_adjust_z_step = .015, grasp_adjust_num_tries = 3):
    
    self.stereo_adjusted = 0

    result = ReactiveGrasper.reactive_grasp(self, approach_pose, grasp_pose, joint_path, side_step, back_step, 
                                   approach_num_tries, goal_pos_thres, min_gripper_opening, max_gripper_opening,
                                   grasp_num_tries, forward_step, min_contact_row, object_name, table_name,
                                   grasp_adjust_x_step, grasp_adjust_z_step, grasp_adjust_num_tries)

    return result