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(self,
cluster,
arm_name,
reference_frame_id,
side=False):
req = GraspPlanningRequest()
req.target.reference_frame_id = reference_frame_id
req.target.region.cloud = cluster
req.arm_name = arm_name #not that cluster planner cares right now which arm
try:
res = self._cluster_grasp_srv(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.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 call_plan_segmented_clutter_grasps(point_cloud, box_pose, box_dims,
arm_name):
req = GraspPlanningRequest()
req.arm_name = arm_name
req.target.reference_frame_id = box_pose.header.frame_id
req.target.region.cloud = point_cloud
req.target.region.roi_box_pose = box_pose
req.target.region.roi_box_dims = box_dims
service_name = "plan_segmented_clutter_grasps"
rospy.loginfo("waiting for %s" % service_name)
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_segmented_clutter_grasps: %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
def get_grasps(self, model_id, model_pose):
'''
Get the set of grasps for a model, given a particular model pose
**Args:**
**model_id:** The model id corresponding to the database being used
*model_pose:* The pose of the model
'''
request = GraspPlanningRequest()
request.arm_name = 'right_arm'
db_pose = DatabaseModelPose()
db_pose.pose = model_pose
db_pose.model_id = model_id
db_pose.confidence = 0.5
request.target.potential_models.append(db_pose)
request.target.reference_frame_id = model_pose.header.frame_id
response = self.database_grasp_planner(request)
rospy.loginfo('Found = ' + str(len(response.grasps)) + ' grasps')
self._visualize_grasps(response.grasps, model_pose.header.frame_id)
return response.grasps
def get_grasps(self, model_id, model_pose):
'''
Get the set of grasps for a model, given a particular model pose
**Args:**
**model_id:** The model id corresponding to the database being used
*model_pose:* The pose of the model
'''
request = GraspPlanningRequest()
request.arm_name = 'right_arm'
db_pose = DatabaseModelPose()
db_pose.pose = model_pose
db_pose.model_id = model_id
db_pose.confidence = 0.5
request.target.potential_models.append(db_pose)
request.target.reference_frame_id = model_pose.header.frame_id
response = self.database_grasp_planner(request)
rospy.loginfo('Found = '+str(len(response.grasps))+' grasps')
self._visualize_grasps(response.grasps,model_pose.header.frame_id)
return response.grasps
def pick(self,pickup_goal):
#prepare result
pickresult = PickupResult()
#get grasps for the object
# fill up a grasp planning request
grasp_planning_req = GraspPlanningRequest()
grasp_planning_req.arm_name = pickup_goal.arm_name
grasp_planning_req.target = pickup_goal.target
object_to_attach = pickup_goal.collision_object_name
# call grasp planning service
grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
#print grasp_planning_res
# process grasp planning result
if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
else:
rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")
# for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)
#for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
motion_plan_res=GetMotionPlanResponse()
grasp_to_execute_=Grasp()
for index, grasp in enumerate(grasp_planning_res.grasps):
# extract grasp_pose
grasp_pose_ = PoseStamped()
grasp_pose_.header.frame_id = "/world";
grasp_pose_.pose = grasp.grasp_pose
grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# currently add this to Z because approach vector needs to be computed somehow first (TODO)
pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
#print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )
#if this pre-grasp pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
# copy the grasp to execute for the following steps
grasp_to_execute_ = copy.deepcopy(grasp)
break
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#put hand in pre-grasp posture
if self.pre_grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Pre-grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach pregrasp pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#time.sleep(20) # TODO use actionlib here
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#grasp
if self.grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#attach the collision object to the hand (should be cleaned-up)
rospy.loginfo("Now we attach the object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attach object published")
else:
rospy.logerr("None of the grasps tested is possible")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
pickresult.manipulation_result.value = ManipulationResult.SUCCESS
pickresult.grasp= grasp_to_execute_
return pickresult
