def convertFrameShelfToRobot(self, pose):
shelf_position = get_shelf_pose().pose.position
pose.position.x += (shelf_position.x)
pose.position.y += (shelf_position.y)
pose.position.z += (shelf_position.z)
return pose
def convertFrameRobotToShelf(self, pose):
shelf_stamped_pose = get_shelf_pose()
pose.position.x += -(shelf_stamped_pose.pose.position.x)
pose.position.y += -(shelf_stamped_pose.pose.position.y)
pose.position.z += -(shelf_stamped_pose.pose.position.z)
return pose
def convertFrameShelfToRobot(self, pose):
shelf_position = get_shelf_pose().pose.position
pose.position.x += (shelf_position.x)
pose.position.y += (shelf_position.y)
pose.position.z += (shelf_position.z)
# pose.position.x += -1.3535731570096812
# pose.position.y += -0.08215183129781853
# pose.position.z += 0.135
# pose.position.x += -1.39485775456
# pose.position.y += -0.0744959997413
# pose.position.z += 0.045
# pose.position.x += -1.40009583376
# pose.position.y += -0.0841733373195
# pose.position.z += 0.045
return pose
def execute(self, userdata):
rospy.loginfo("Trying to pick '"+userdata.item+"'...")
self.points.publish(userdata.points)
pose = get_shelf_pose().pose
# Note: Orientation for STL shelf differs from true shelf
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
grasps, success = generate_grasps(userdata.item, userdata.pose, pose,
userdata.points, userdata.bin)
if not success:
return 'Failure'
# self.show_grasps(grasps)
#with PADDED_SHELF:
with BIN(userdata.bin):
grasps = plan_grasps(self.arm, grasps)
try:
grasp, plan_to_approach, plan_to_grasp, plan_to_retreat = grasps.next()
rospy.loginfo("Grasp: %s" % grasp)
except StopIteration:
rospy.logwarn("No online grasps found.")
#
min_x, max_x, min_y, max_y, min_z, _ = rospy.get_param("/shelf"+"/bins/"+userdata.bin)
# TODO: Convert to base frame
Tbase_shelf = numpy.array([PoseToMatrix(pose)]) # Transform form base to shelf
min_y_base = numpy.dot(Tbase_shelf, numpy.array([[0], [min_y], [0], [1]])) [1]
max_y_base = numpy.dot(Tbase_shelf, numpy.array([[0], [max_y], [0], [1]])) [1]
min_x_base = numpy.dot(Tbase_shelf, numpy.array([[min_x], [0], [0], [1]])) [0]
max_x_base = numpy.dot(Tbase_shelf, numpy.array([[max_x], [0], [0], [1]])) [0]
min_z_base = numpy.dot(Tbase_shelf, numpy.array([[0], [0], [min_z], [1]])) [2]
rospy.logerr('>>>>>>>>>>>>>>>>>>>>>> after conver bin bound>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>')
cutoff1, cutoff2 = min_y_base + (0.14 * sqrt(2)/2), max_y_base - (0.14 * sqrt(2)/2)
if userdata.pose.position.y < cutoff1:
success = execute_wallgrasp_left(self.arm, min_x_base, max_x_base, min_y_base, max_y_base, min_z_base)
if not success:
return 'Failure'
elif userdata.pose.position.y < cutoff1:
pass # TODO: center grasp
else:
success = execute_wallgrasp_right(self.arm, min_x_base, max_x_base, min_y_base, max_y_base, min_z_base)
if not success:
return 'Failure'
return "Failure"
success, gripper_status = execute_grasp(self.arm, grasp, plan_to_approach, plan_to_grasp, plan_to_retreat, shelf=NO_SHELF)
userdata.gripper_status = gripper_status
if not success:
return "Failure"
pose = PoseStamped()
pose.header.frame_id = "/arm_left_link_7_t"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = -0.35
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
scene.attach_box("arm_left_link_7_t", "Object", pose, [0.15, 0.23, 0.16],
["hand_left_finger_1_link_1", "hand_left_finger_1_link_2", "hand_left_finger_1_link_3", "hand_left_finger_1_link_3_tip",
"hand_left_finger_2_link_1", "hand_left_finger_2_link_2", "hand_left_finger_2_link_3", "hand_left_finger_2_link_3_tip",
"hand_left_finger_middle_link_1", "hand_left_finger_middle_link_2", "hand_left_finger_middle_link_3", "hand_left_finger_middle_link_3_tip"])
return 'Success'