def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
def lift_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo("%s: preempted" % ACTION_NAME)
self.action_server.set_preempted()
collision_support_surface_name = goal.collision_support_surface_name
# disable collisions between grasped object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = GRIPPER_GROUP_NAME
collision_operation2.object2 = collision_support_surface_name
collision_operation2.operation = CollisionOperation.DISABLE
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
# this is a hack to make arm lift an object faster
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.REMOVE
obj.object.id = collision_support_surface_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
joint_names = current_state.joint_names
joint_positions = current_state.actual.positions
start_angles = [joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS]
start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit
if not move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
start_angles,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations,
):
self.action_server.set_aborted()
return
self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id)
if move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
LIFT_POSITION,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations,
):
# check if are still holding an object after lift is done
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(LiftObjectResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr("%s: attempted lift failed" % ACTION_NAME)
self.action_server.set_aborted()
return
def put_down_at(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
bbox_center = self.find_cluster_bounding_box_center(
goal.graspable_object.cluster)
if not bbox_center: self.action_server.set_aborted()
goal.target_location.z = bbox_center.z
x_dist = goal.target_location.x - bbox_center.x
y_dist = goal.target_location.y - bbox_center.y
target = goal.graspable_object
target.cluster.points = [
Point32(p.x + x_dist, p.y + y_dist, p.z)
for p in target.cluster.points
]
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
ik_solution = self.find_grasp_pose(target, collision_object_name,
collision_support_surface_name)
if ik_solution:
# disable collisions between gripper and target object
ordered_collision_operations = OrderedCollisionOperations()
collision_operation = CollisionOperation()
collision_operation.object1 = collision_object_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.01
# disable collisions between arm and table
collision_operation3 = CollisionOperation()
collision_operation3.object1 = collision_support_surface_name
collision_operation3.object2 = ARM_GROUP_NAME
collision_operation3.operation = CollisionOperation.DISABLE
collision_operation3.penetration_distance = 0.01
ordered_collision_operations.collision_operations = [
collision_operation, collision_operation2, collision_operation3
]
# set gripper padding to 0
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS])
# move into pre-grasp pose
if not move_arm_joint_goal(
self.move_arm_client,
ik_solution.joint_state.name,
ik_solution.joint_state.position,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
return
# record put down sound with 0.5 second padding before and after
self.start_audio_recording_srv(InfomaxAction.GRASP,
goal.category_id)
rospy.sleep(0.5)
self.open_gripper()
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
self.action_server.set_succeeded(
PutDownAtResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted put down failed' % ACTION_NAME)
self.action_server.set_aborted()
def grasp_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
target = goal.graspable_object
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
ik_solution = self.find_grasp_pose(target, collision_object_name,
collision_support_surface_name)
if ik_solution:
# disable collisions between gripper and target object
ordered_collision_operations = OrderedCollisionOperations()
collision_operation1 = CollisionOperation()
collision_operation1.object1 = collision_object_name
collision_operation1.object2 = GRIPPER_GROUP_NAME
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
# collision_operation2.penetration_distance = 0.02
ordered_collision_operations.collision_operations = [
collision_operation1, collision_operation2
]
# set gripper padding to 0
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS])
self.open_gripper()
# move into pre-grasp pose
if not move_arm_joint_goal(
self.move_arm_client,
ik_solution.joint_state.name,
ik_solution.joint_state.position,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
return
# record grasping sound with 0.5 second padding before and after
self.start_audio_recording_srv(InfomaxAction.GRASP,
goal.category_id)
rospy.sleep(0.5)
grasp_successful = self.close_gripper()
rospy.sleep(0.5)
# if grasp was successful, attach it to the gripper
if grasp_successful:
sound_msg = self.stop_audio_recording_srv(True)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
self.action_server.set_succeeded(
GraspObjectResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
def place_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
# check that we have something in hand before placing it
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if not grasp_status.is_hand_occupied:
rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME)
self.action_server.set_aborted()
return
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.01
# disable collisions between arm and table
collision_operation3 = CollisionOperation()
collision_operation3.object1 = collision_support_surface_name
collision_operation3.object2 = ARM_GROUP_NAME
collision_operation3.operation = CollisionOperation.DISABLE
collision_operation3.penetration_distance = 0.01
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [
collision_operation1, collision_operation2, collision_operation3
]
self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id)
if move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
PLACE_POSITION,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
sound_msg = None
grasp_status = self.get_grasp_status_srv()
self.open_gripper()
rospy.sleep(0.5)
# if after lowering arm gripper is still holding an object life's good
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
else:
self.stop_audio_recording_srv(False)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
if sound_msg:
self.action_server.set_succeeded(
PlaceObjectResult(sound_msg.recorded_sound))
return
else:
self.action_server.set_aborted()
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted place failed' % ACTION_NAME)
self.action_server.set_aborted()
def place_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
# check that we have something in hand before placing it
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if not grasp_status.is_hand_occupied:
rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME)
self.action_server.set_aborted()
return
gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.01
# disable collisions between arm and table
collision_operation3 = CollisionOperation()
collision_operation3.object1 = collision_support_surface_name
collision_operation3.object2 = ARM_GROUP_NAME
collision_operation3.operation = CollisionOperation.DISABLE
collision_operation3.penetration_distance = 0.01
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2, collision_operation3]
self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id)
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
PLACE_POSITION,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
sound_msg = None
grasp_status = self.get_grasp_status_srv()
self.open_gripper()
rospy.sleep(0.5)
# if after lowering arm gripper is still holding an object life's good
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
else:
self.stop_audio_recording_srv(False)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
if sound_msg:
self.action_server.set_succeeded(PlaceObjectResult(sound_msg.recorded_sound))
return
else:
self.action_server.set_aborted()
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted place failed' % ACTION_NAME)
self.action_server.set_aborted()
def grasp_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
target = goal.graspable_object
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name)
if ik_solution:
# disable collisions between gripper and target object
ordered_collision_operations = OrderedCollisionOperations()
collision_operation1 = CollisionOperation()
collision_operation1.object1 = collision_object_name
collision_operation1.object2 = GRIPPER_GROUP_NAME
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
# collision_operation2.penetration_distance = 0.02
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
# set gripper padding to 0
gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
self.open_gripper()
# move into pre-grasp pose
if not move_arm_joint_goal(self.move_arm_client,
ik_solution.joint_state.name,
ik_solution.joint_state.position,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
return
# record grasping sound with 0.5 second padding before and after
self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id)
rospy.sleep(0.5)
grasp_successful = self.close_gripper()
rospy.sleep(0.5)
# if grasp was successful, attach it to the gripper
if grasp_successful:
sound_msg = self.stop_audio_recording_srv(True)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
self.action_server.set_succeeded(GraspObjectResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
def lift_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
collision_support_surface_name = goal.collision_support_surface_name
# disable collisions between grasped object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = GRIPPER_GROUP_NAME
collision_operation2.object2 = collision_support_surface_name
collision_operation2.operation = CollisionOperation.DISABLE
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [
collision_operation1, collision_operation2
]
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
# this is a hack to make arm lift an object faster
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.REMOVE
obj.object.id = collision_support_surface_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
current_state = rospy.wait_for_message('l_arm_controller/state',
FollowJointTrajectoryFeedback)
joint_names = current_state.joint_names
joint_positions = current_state.actual.positions
start_angles = [
joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS
]
start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit
if not move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
start_angles,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
self.action_server.set_aborted()
return
self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id)
if move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
LIFT_POSITION,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
# check if are still holding an object after lift is done
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(
LiftObjectResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted lift failed' % ACTION_NAME)
self.action_server.set_aborted()
return
def put_down_at(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
bbox_center = self.find_cluster_bounding_box_center(goal.graspable_object.cluster)
if not bbox_center: self.action_server.set_aborted()
goal.target_location.z = bbox_center.z
x_dist = goal.target_location.x - bbox_center.x
y_dist = goal.target_location.y - bbox_center.y
target = goal.graspable_object
target.cluster.points = [Point32(p.x+x_dist, p.y+y_dist, p.z) for p in target.cluster.points]
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name)
if ik_solution:
# disable collisions between gripper and target object
ordered_collision_operations = OrderedCollisionOperations()
collision_operation = CollisionOperation()
collision_operation.object1 = collision_object_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.01
# disable collisions between arm and table
collision_operation3 = CollisionOperation()
collision_operation3.object1 = collision_support_surface_name
collision_operation3.object2 = ARM_GROUP_NAME
collision_operation3.operation = CollisionOperation.DISABLE
collision_operation3.penetration_distance = 0.01
ordered_collision_operations.collision_operations = [collision_operation, collision_operation2, collision_operation3]
# set gripper padding to 0
gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
# move into pre-grasp pose
if not move_arm_joint_goal(self.move_arm_client,
ik_solution.joint_state.name,
ik_solution.joint_state.position,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
return
# record put down sound with 0.5 second padding before and after
self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id)
rospy.sleep(0.5)
self.open_gripper()
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
self.action_server.set_succeeded(PutDownAtResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted put down failed' % ACTION_NAME)
self.action_server.set_aborted()
