def plan_point_cluster_grasp_callback(self, req):
rospy.loginfo("planning grasps for a point cluster")
resp = GraspPlanningResponse()
(grasps, error_code) = self.plan_point_cluster_grasps(req.target, req.arm_name)
resp.grasps = grasps
resp.error_code = error_code
return resp
def evaluate_point_cluster_grasps_callback(self, req):
#rospy.loginfo("evaluating grasps for a point cluster")
#find the cluster bounding box and relevant frames, and transform the cluster
if len(req.target.cluster.points) > 0:
self.pcgp.init_cluster_grasper(req.target.cluster)
cluster_frame = req.target.cluster.header.frame_id
else:
self.pcgp.init_cluster_grasper(req.target.region.cloud)
cluster_frame = req.target.region.cloud.header.frame_id
#evaluate the grasps on the cluster
probs = self.pcgp.evaluate_point_cluster_grasps(
req.grasps_to_evaluate, cluster_frame)
#return the same grasps with the qualities added
for (grasp, prob) in zip(req.grasps_to_evaluate, probs):
grasp.success_probability = prob
#fill in the response
resp = GraspPlanningResponse()
resp.error_code.value = 0
resp.grasps = req.grasps_to_evaluate
return resp
def plan_segmented_clutter_grasps_callback(self, req):
rospy.loginfo("planning grasps for a point cloud")
resp = GraspPlanningResponse()
(grasps,
error_code) = self.plan_grasps_for_target(req.target, req.arm_name)
resp.grasps = grasps
resp.error_code = error_code
return resp
def evaluate_point_cluster_grasps_callback(self, req):
#rospy.loginfo("evaluating grasps for a point cluster")
#find the cluster bounding box and relevant frames, and transform the cluster
if len(req.target.cluster.points) > 0:
self.pcgp.init_cluster_grasper(req.target.cluster)
cluster_frame = req.target.cluster.header.frame_id
else:
self.pcgp.init_cluster_grasper(req.target.region.cloud)
cluster_frame = req.target.region.cloud.header.frame_id
#evaluate the grasps on the cluster
probs = self.pcgp.evaluate_point_cluster_grasps(req.grasps_to_evaluate, cluster_frame)
#return the same grasps with the qualities added
for (grasp, prob) in zip(req.grasps_to_evaluate, probs):
grasp.success_probability = prob
#fill in the response
resp = GraspPlanningResponse()
resp.error_code.value = 0
resp.grasps = req.grasps_to_evaluate
return resp
def plan_point_cluster_grasp_callback(self, req):
rospy.loginfo("planning grasps for a point cluster")
resp = GraspPlanningResponse()
#get the hand joint names from the param server (loaded from yaml config file)
joint_names_dict = rospy.get_param('~joint_names')
pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles')
grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles')
pregrasp_joint_efforts_dict = rospy.get_param('~pregrasp_joint_efforts')
grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts')
if req.arm_name:
arm_name = req.arm_name
else:
arm_name = joint_names_dict.keys()[0]
rospy.logerr("point cluster grasp planner: missing arm_name in request! Using "+arm_name)
try:
hand_joints = joint_names_dict[arm_name]
except KeyError:
arm_name = joint_names_dict.keys()[0]
rospy.logerr("arm_name "+req.arm_name+" not found! Using joint names from "+arm_name)
hand_joints = joint_names_dict[arm_name]
pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name]
grasp_joint_angles = grasp_joint_angles_dict[arm_name]
pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name]
grasp_joint_efforts = grasp_joint_efforts_dict[arm_name]
#hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints')
rospy.loginfo("hand_joints:"+str(hand_joints))
#find the cluster bounding box and relevant frames, and transform the cluster
init_start_time = time.time()
if len(req.target.cluster.points) > 0:
self.pcgp.init_cluster_grasper(req.target.cluster)
cluster_frame = req.target.cluster.header.frame_id
else:
self.pcgp.init_cluster_grasper(req.target.region.cloud)
cluster_frame = req.target.region.cloud.header.frame_id
if len(cluster_frame) == 0:
rospy.logerr("region.cloud.header.frame_id was empty!")
resp.error_code.value = resp.error_code.OTHER_ERROR
return resp
init_end_time = time.time()
#print "init time: %.3f"%(init_end_time - init_start_time)
#plan grasps for the cluster (returned in the cluster frame)
grasp_plan_start_time = time.time()
(pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) = self.pcgp.plan_point_cluster_grasps()
grasp_plan_end_time = time.time()
#print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time)
#add error code to service
resp.error_code.value = resp.error_code.SUCCESS
grasp_list = []
if pregrasp_poses == None:
resp.error_code.value = resp.error_code.OTHER_ERROR
return resp
#fill in the list of grasps
for (grasp_pose, quality, pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists):
pre_grasp_joint_state = self.create_joint_state(hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts)
grasp_joint_state = self.create_joint_state(hand_joints, grasp_joint_angles, grasp_joint_efforts)
#if the cluster isn't in the same frame as the graspable object reference frame,
#transform the grasps to be in the reference frame
if cluster_frame == req.target.reference_frame_id:
transformed_grasp_pose = grasp_pose
else:
transformed_grasp_pose = change_pose_stamped_frame(self.pcgp.tf_listener,
stamp_pose(grasp_pose, cluster_frame),
req.target.reference_frame_id).pose
if self.pcgp.pregrasp_just_outside_box:
min_approach_distance = pregrasp_dist
else:
min_approach_distance = max(pregrasp_dist-.05, .05)
grasp_list.append(Grasp(pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state,
grasp_pose=transformed_grasp_pose, success_probability=quality,
desired_approach_distance = pregrasp_dist, min_approach_distance = min_approach_distance))
#if requested, randomize the first few grasps
if self.randomize_grasps:
first_grasps = grasp_list[:30]
random.shuffle(first_grasps)
shuffled_grasp_list = first_grasps + grasp_list[30:]
resp.grasps = shuffled_grasp_list
else:
resp.grasps = grasp_list
return resp
def plan_point_cluster_grasp_callback(self, req):
rospy.loginfo("planning grasps for a point cluster")
resp = GraspPlanningResponse()
#get the hand joint names from the param server (loaded from yaml config file)
joint_names_dict = rospy.get_param('~joint_names')
pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles')
grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles')
pregrasp_joint_efforts_dict = rospy.get_param(
'~pregrasp_joint_efforts')
grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts')
if req.arm_name:
arm_name = req.arm_name
else:
arm_name = joint_names_dict.keys()[0]
rospy.logerr(
"point cluster grasp planner: missing arm_name in request! Using "
+ arm_name)
try:
hand_joints = joint_names_dict[arm_name]
except KeyError:
arm_name = joint_names_dict.keys()[0]
rospy.logerr("arm_name " + req.arm_name +
" not found! Using joint names from " + arm_name)
hand_joints = joint_names_dict[arm_name]
pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name]
grasp_joint_angles = grasp_joint_angles_dict[arm_name]
pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name]
grasp_joint_efforts = grasp_joint_efforts_dict[arm_name]
#hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints')
rospy.loginfo("hand_joints:" + str(hand_joints))
#find the cluster bounding box and relevant frames, and transform the cluster
init_start_time = time.time()
if len(req.target.cluster.points) > 0:
self.pcgp.init_cluster_grasper(req.target.cluster)
cluster_frame = req.target.cluster.header.frame_id
else:
self.pcgp.init_cluster_grasper(req.target.region.cloud)
cluster_frame = req.target.region.cloud.header.frame_id
if len(cluster_frame) == 0:
rospy.logerr("region.cloud.header.frame_id was empty!")
resp.error_code.value = resp.error_code.OTHER_ERROR
return resp
init_end_time = time.time()
#print "init time: %.3f"%(init_end_time - init_start_time)
#plan grasps for the cluster (returned in the cluster frame)
grasp_plan_start_time = time.time()
(pregrasp_poses, grasp_poses, gripper_openings, qualities,
pregrasp_dists) = self.pcgp.plan_point_cluster_grasps()
grasp_plan_end_time = time.time()
#print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time)
#add error code to service
resp.error_code.value = resp.error_code.SUCCESS
grasp_list = []
if pregrasp_poses == None:
resp.error_code.value = resp.error_code.OTHER_ERROR
return resp
#fill in the list of grasps
for (grasp_pose, quality,
pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists):
pre_grasp_joint_state = self.create_joint_state(
hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts)
grasp_joint_state = self.create_joint_state(
hand_joints, grasp_joint_angles, grasp_joint_efforts)
#if the cluster isn't in the same frame as the graspable object reference frame,
#transform the grasps to be in the reference frame
if cluster_frame == req.target.reference_frame_id:
transformed_grasp_pose = grasp_pose
else:
transformed_grasp_pose = change_pose_stamped_frame(
self.pcgp.tf_listener, stamp_pose(grasp_pose,
cluster_frame),
req.target.reference_frame_id).pose
if self.pcgp.pregrasp_just_outside_box:
min_approach_distance = pregrasp_dist
else:
min_approach_distance = max(pregrasp_dist - .05, .05)
grasp_list.append(
Grasp(pre_grasp_posture=pre_grasp_joint_state,
grasp_posture=grasp_joint_state,
grasp_pose=transformed_grasp_pose,
success_probability=quality,
desired_approach_distance=pregrasp_dist,
min_approach_distance=min_approach_distance))
#if requested, randomize the first few grasps
if self.randomize_grasps:
first_grasps = grasp_list[:30]
random.shuffle(first_grasps)
shuffled_grasp_list = first_grasps + grasp_list[30:]
resp.grasps = shuffled_grasp_list
else:
resp.grasps = grasp_list
return resp
