old_point.point.y = objects.recognized_objects.objects[
0].pose.pose.pose.position.y
old_point.point.z = objects.recognized_objects.objects[
0].pose.pose.pose.position.z
new_point = listener.transformPoint('base_link', old_point)
arm_mover.set_pose_reference_frame('base_link')
euler = euler_from_quaternion([0.94, -0.3, -0.035, 0.03])
#go to pre-grasp pose
pose = [
new_point.point.x, new_point.point.y, new_point.point.z + 0.45,
euler[0], euler[1], euler[2]
]
print "I want to move to: %s" % (str(pose))
arm_mover.set_pose_target(pose, "wrist_roll_link")
if not arm_mover.go():
continue
rospy.sleep(1.0)
#move down to grasp
new_pose = pose
new_pose[2] = new_pose[2] - 0.18
new_pose[1] = new_pose[1] + 0.02
new_pose[0] = new_pose[0] + 0.02
arm_mover.set_pose_target(new_pose, "wrist_roll_link")
if not arm_mover.go():
continue
#hack to close gripper because pr2_controllers_msgs is nto catkinized
listener.waitForTransform("/head_camera_rgb_optical_frame", "/base_link", rospy.Time(0), rospy.Duration(5.0))
old_point = PointStamped()
old_point.header.frame_id = "head_camera_rgb_optical_frame"
old_point.point.x = objects.recognized_objects.objects[0].pose.pose.pose.position.x
old_point.point.y = objects.recognized_objects.objects[0].pose.pose.pose.position.y
old_point.point.z = objects.recognized_objects.objects[0].pose.pose.pose.position.z
new_point = listener.transformPoint("base_link", old_point)
arm_mover.set_pose_reference_frame("base_link")
euler = euler_from_quaternion([0.94, -0.3, -0.035, 0.03])
# go to pre-grasp pose
pose = [new_point.point.x, new_point.point.y, new_point.point.z + 0.45, euler[0], euler[1], euler[2]]
print "I want to move to: %s" % (str(pose))
arm_mover.set_pose_target(pose, "wrist_roll_link")
if not arm_mover.go():
continue
rospy.sleep(1.0)
# move down to grasp
new_pose = pose
new_pose[2] = new_pose[2] - 0.18
new_pose[1] = new_pose[1] + 0.02
new_pose[0] = new_pose[0] + 0.02
arm_mover.set_pose_target(new_pose, "wrist_roll_link")
if not arm_mover.go():
continue
# hack to close gripper because pr2_controllers_msgs is nto catkinized
