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 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 generate_scored_grasps(self, pc = None, whicharm = "leftarm",
pullup = False):
if pc is None:
rospy.loginfo("Waiting for BOLT pointcloud")
pc = rospy.wait_for_message("/bolt/vision/pcl_robot", PointCloud2)
rospy.loginfo("Ok got it!")
xyz, rgb = utils.pc2xyzrgb(pc)
weights = rgb[0,:,0]
sorted_indexes = np.argsort(weights)[::-1]
weights = weights[sorted_indexes]
xyz = xyz[0,:,:]
xyz = xyz[sorted_indexes,:]
rospy.loginfo("Initially The maximum weight is %f", weights.max())
rospy.loginfo("Initially The min weight is %f", weights.min())
trim_weights = 5
rospy.loginfo("Taking only the maximum %d weights", trim_weights)
weights = weights[:trim_weights]
xyz = xyz[:trim_weights,:]
rospy.loginfo("XYZ shape %s, W shape: %s", xyz.shape, weights.shape)
rospy.loginfo("After trimming The maximum weight is %f", weights.max())
rospy.loginfo("After trimming The min weight is %f", weights.min())
rospy.wait_for_service('evaluate_point_cluster_grasps')
evaluate_grasps = rospy.ServiceProxy('evaluate_point_cluster_grasps',
GraspPlanning)
grasp_planning = GraspPlanningRequest()
grasps = []
for coords in xyz:
_g = utils.create_spaced_downward_grasps(coords, pc,40)
grasps.extend(_g)
rospy.loginfo("Testing a total of %d grasps", len(grasps))
graspable = utils.pc2graspable(pc)
grasp_planning.target = graspable
grasp_planning.grasps_to_evaluate = grasps
res = evaluate_grasps(grasp_planning)
grasps = res.grasps
non_zero_grasps = sorted([g for g in grasps
if g.success_probability > 0],
key = lambda g:g.success_probability,
reverse=True)
if len(non_zero_grasps) == 0:
rospy.logerr("Error: no valid grasps found!")
return False
probs = [g.success_probability
for g in non_zero_grasps]
rospy.loginfo("probabilities are: %s", probs)
rospy.loginfo("Number of non-zero %d grasps", len(non_zero_grasps))
#self.publish_grasps(res)
for g in non_zero_grasps:
rospy.loginfo("Testing a grasp")
ps = PoseStamped()
ps.header.frame_id = pc.header.frame_id
ps.pose = g.grasp_pose
self.publish_gripper_pose(ps)
approach = copy.deepcopy(ps)
approach.pose.position.z += 0.1
if whicharm == "leftarm":
self.robot.move_left_arm(approach)
self.robot.controller.open_left_gripper(True)
success = self.robot.move_left_arm(ps)
if success:
self.robot.controller.close_left_gripper(True)
if pullup:
self.robot.move_left_arm(approach)
else:
self.robot.move_right_arm(approach)
self.robot.controller.open_right_gripper(True)
success = self.robot.move_right_arm(ps)
if success:
self.robot.controller.close_right_gripper(True)
if pullup:
self.robot.move_right_arm(approach)
if success:
break
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