Calling Grasp Adjustment

This tutorial is still under construction

grasp_adjust_client = None
rospy.wait_for_service("/pr2_gripper_grasp_adjust")

grasp_adjust_client = rospy.ServiceProxy("/pr2_gripper_grasp_adjust", GripperStereoGraspAdjust)

                ps = copy.copy(self.target)
                pose_mat = pose_to_mat(ps.pose)
                pose_mat[0:3, 3] -= 0.08*pose_mat[0:3, 0]
                ps.pose  = mat_to_pose(pose_mat)
                (new_goal_list, result) = self.gripper_stereo_grasp_adjust(ps, do_global_search = True)
                
                #grasp is impossible!  
                if result == GripperStereoGraspAdjustResponse.IMPOSSIBLE:
                    rospy.loginfo("gripper stereo adjustment returned grasp impossible!")
                    return self.target 

                if result == GripperStereoGraspAdjustResponse.FAILURE:
                    rospy.loginfo("gripper stereo adjustment returned failed!")
                    return self.target
                
                for i in range(len(new_goal_list)):
                    #TODO account for tool frame -> wrist frame offset
                    new_goal_list[i] = change_pose_stamped_frame(self.tf, new_goal_list[i], 'torso_lift_link')
                 
                self.adjusted_poses = new_goal_list[:]
                print "Received", len(self.adjusted_poses), "adjusted grasps"