def find_grasp_pose(self,
target,
collision_object_name='',
collision_support_surface_name=''):
"""
target = GraspableObject
collision_object_name = name of target in collision map
collision_support_surface_name = name of surface target is touching
"""
req = GraspPlanningRequest()
req.arm_name = ARM_GROUP_NAME
req.target = target
req.collision_object_name = collision_object_name
req.collision_support_surface_name = collision_support_surface_name
rospy.loginfo(
'%s: trying to find a good grasp for graspable object %s' %
(ACTION_NAME, collision_object_name))
grasping_result = self.grasp_planning_srv(req)
if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS:
rospy.logerr('%s: unable to find a feasable grasp, aborting' %
ACTION_NAME)
return None
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = grasping_result.grasps[
0].grasp_posture.header.frame_id
pose_stamped.pose = grasping_result.grasps[0].grasp_pose
rospy.loginfo('%s: found good grasp, looking for corresponding IK' %
ACTION_NAME)
return self.find_ik_for_grasping_pose(pose_stamped)
def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''):
"""
target = GraspableObject
collision_object_name = name of target in collision map
collision_support_surface_name = name of surface target is touching
"""
req = GraspPlanningRequest()
req.arm_name = ARM_GROUP_NAME
req.target = target
req.collision_object_name = collision_object_name
req.collision_support_surface_name = collision_support_surface_name
rospy.loginfo('%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name))
grasping_result = self.grasp_planning_srv(req)
if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS:
rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME)
return None
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id
pose_stamped.pose = grasping_result.grasps[0].grasp_pose
rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME)
return self.find_ik_for_grasping_pose(pose_stamped)
def call_plan_point_cluster_grasp(cluster):
req = GraspPlanningRequest()
req.target.cluster = cluster
req.arm_name = "arm"
req.collision_support_surface_name = "table"
service_name = "openrave_grasp_planner"
rospy.loginfo("waiting for /openrave_grasp_planner service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
serv = rospy.ServiceProxy(service_name, GraspPlanning)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling plan_point_cluster_grasp: %s" % e)
return 0
def call_plan_point_cluster_grasp(cluster):
req = GraspPlanningRequest()
req.target.reference_frame_id = "/base_link"
req.target.cluster = cluster
req.arm_name = "right_arm"
req.collision_support_surface_name = "table"
service_name = "plan_point_cluster_grasp"
rospy.loginfo("waiting for plan_point_cluster_grasp service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
serv = rospy.ServiceProxy(service_name, GraspPlanning)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling plan_point_cluster_grasp: %s"%e)
return 0
def call_plan_point_cluster_grasp(cluster):
req = GraspPlanningRequest()
req.target.reference_frame_id = "/base_link"
req.target.cluster = cluster
req.arm_name = "right_arm"
req.collision_support_surface_name = "table"
service_name = "plan_point_cluster_grasp"
rospy.loginfo("waiting for plan_point_cluster_grasp service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
serv = rospy.ServiceProxy(service_name, GraspPlanning)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling plan_point_cluster_grasp: %s" % e)
return 0
def plan_grasp(self, graspable,
which_arm,
graspable_name = "",
table_name = "",
):
"""Picks up a previously detected object.
Parameters:
graspable: an object_manipulation_msgs/GraspableObject msg instance.
This usually comes from a Detector.call_collision_map_processing call.
graspable_name: the name of the object to graps. It is provided by
Detector.call_collision_map_processing.
table_name: the name of the table. Again provided by Detector.call_collision_map_processing.
which_arm: left_arm or right_arm
Return:
a object_manipulation_msgs.GraspPlanningResponse msg
"""
if self.grap_planning_srv is None:
srv_name = "/plan_point_cluster_grasp"
rospy.loginfo("Waiting for service %s", srv_name)
rospy.wait_for_service(srv_name)
self.grap_planning_srv = rospy.ServiceProxy(srv_name,
GraspPlanning)
rospy.loginfo("Calling the grasp planning service")
gp = GraspPlanningRequest()
gp.arm_name = which_arm
gp.target = graspable
gp.collision_object_name = graspable_name
gp.collision_support_surface_name = table_name
res = self.grap_planning_srv(gp)
isinstance(res, GraspPlanningResponse)
if res.error_code.value != res.error_code.SUCCESS:
rospy.logerr("Could not find valid grasps!")
return None
else:
grasps = sorted(res.grasps, key = lambda g:g.success_probability)
res.grasps = grasps
return res
