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_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 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
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tdr
req.reset_collision_models = True
req.reset_attached_models = True
req.reset_static_map = False
req.take_static_collision_map = False
req.desired_frame = 'base_link'
coll_map_res = collision_map_processing_srv(req)
rospy.loginfo('collision_map update is done')
rospy.loginfo('there are %d graspable objects %s, collision support surface name is "%s"' %
(len(coll_map_res.graspable_objects), coll_map_res.collision_object_names, coll_map_res.collision_support_surface_name))
req = GraspPlanningRequest()
req.arm_name = 'left_arm'
req.target = coll_map_res.graspable_objects[0]
req.collision_object_name = coll_map_res.collision_object_names[0]
req.collision_support_surface_name = coll_map_res.collision_support_surface_name
rospy.loginfo('trying to find a good grasp for graspable object %s' % coll_map_res.collision_object_names[0])
grasping_result = grasp_planning_srv(req)
if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS:
rospy.logerr('unable to find a feasable grasp, aborting')
exit(1)
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
