def build_collision_operations(self, object1, object2):
msg = OrderedCollisionOperations()
collision = CollisionOperation()
collision.operation = CollisionOperation.DISABLE
collision.object1 = object1
collision.object2 = object2
msg.collision_operations.append(collision)
return msg
def build_collision_operations(self, object1, object2):
msg = OrderedCollisionOperations()
collision = CollisionOperation()
collision.operation = CollisionOperation.DISABLE
collision.object1 = object1
collision.object2 = object2
msg.collision_operations.append(collision)
return msg
def execute_trajectories(self, result):
rospy.loginfo('There are ' + str(len(result.primitive_names)) +
' segments')
for (i, t) in enumerate(result.primitive_types):
rospy.loginfo('Executing trajectory ' + str(i) + ' of type ' + t +
' and length ' + str(
len(result.primitive_trajectories[i].
joint_trajectory.points)))
#print 'Trajectory is\n', result.primitive_trajectories[i]
splits = t.split('-')
if splits[0] == 'BaseTransit':
self.execute_base_trajectory(result.primitive_trajectories[i])
continue
if splits[0] == 'BaseWarp':
self.arm_tasks.move_arm_to_side('right_arm')
self.arm_tasks.move_arm_to_side('left_arm')
self.execute_warp_trajectory(result.primitive_trajectories[i])
continue
if splits[0] == 'ArmTransit':
self.execute_arm_trajectory(splits[1],
result.primitive_trajectories[i])
continue
#if splits[0] == 'Approach':
#gripper.open()
if splits[0] == 'Pickup':
try:
self.grippers[splits[1]].close()
except ActionFailedError:
pass
self.wi.attach_object_to_gripper(splits[1], splits[2])
#so the trajectory filter doesn't complain
ops = OrderedCollisionOperations()
op = CollisionOperation()
op.operation = op.DISABLE
op.object2 = splits[2]
for t in self.wi.hands[splits[1]].touch_links:
op.object1 = t
ops.collision_operations.append(copy.deepcopy(op))
self.psi.add_ordered_collisions(ops)
self.execute_straight_line_arm_trajectory(
splits[1], result.primitive_trajectories[i])
if splits[0] == 'Place':
self.grippers[splits[1]].open()
self.wi.detach_and_add_back_attached_object(
splits[1], splits[2])
self.psi.reset()
def build_collision_operations(self, arm_name,
penetration_depth,
enable=False):
msg = OrderedCollisionOperations()
if arm_name == "right_arm":
link_names = ["r_gripper_palm_joint",
"r_gripper_l_finger_joint",
"r_gripper_l_finger_tip_joint",
"r_gripper_led_joint",
"r_gripper_motor_accelerometer_joint",
"r_gripper_motor_slider_joint",
"r_gripper_motor_screw_joint",
"r_gripper_r_finger_joint",
"r_gripper_r_finger_tip_joint",
"r_gripper_joint",
"r_gripper_tool_joint",
]
else:
link_names = ["l_gripper_palm_joint",
"l_gripper_l_finger_joint",
"l_gripper_l_finger_tip_joint",
"l_gripper_led_joint",
"l_gripper_motor_accelerometer_joint",
"l_gripper_motor_slider_joint",
"l_gripper_motor_screw_joint",
"l_gripper_r_finger_joint",
"l_gripper_r_finger_tip_joint",
"l_gripper_joint",
"l_gripper_tool_joint",
]
for link in link_names:
collision = CollisionOperation()
if enable:
collision.operation = CollisionOperation.ENABLE
else:
collision.operation = CollisionOperation.DISABLE
collision.object1 = link
collision.object2 = "collision_map"
collision.penetration_distance = penetration_depth
msg.collision_operations.append(collision)
return msg
def get_collisions(self):
'''
creates list of collisions to be ignored.
@rtype OrderedCollisionOperations
@returns list of collisions to be ignored
'''
ignore = OrderedCollisionOperations()
gripper = CollisionOperation()
gripper.object1 = "l_end_effector"
gripper.object2 = CollisionOperation.COLLISION_SET_ALL
gripper.operation = CollisionOperation.DISABLE
wrist = CollisionOperation()
wrist.object1 = "l_wrist_roll_link"
wrist.object2 = CollisionOperation.COLLISION_SET_ALL
wrist.operation = CollisionOperation.DISABLE
obj = CollisionOperation()
obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
obj.object2 = "table"
obj.operation = CollisionOperation.DISABLE
map_obj = CollisionOperation()
map_obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
map_obj.object2 = "collision_map"
map_obj.operation = CollisionOperation.DISABLE
lfing = CollisionOperation()
lfing.object1 = "r_gripper_r_finger_tip_link"
lfing.object2 = "table"
lfing.operation = CollisionOperation.DISABLE
rfingtip = CollisionOperation()
rfingtip.object1 = "r_gripper_l_finger_tip_link"
rfingtip.object2 = "table"
rfingtip.operation = CollisionOperation.DISABLE
rfing = CollisionOperation()
rfing.object1 = "r_gripper_l_finger_link"
rfing.object2 = "table"
rfing.operation = CollisionOperation.DISABLE
forearm = CollisionOperation()
forearm.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
forearm.object2 = "l_forearm_link"
forearm.operation = CollisionOperation.DISABLE
flex = CollisionOperation()
flex.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
flex.object2 = "l_wrist_flex_link"
flex.operation = CollisionOperation.DISABLE
ops = []
op = CollisionOperation()
op.operation = op.DISABLE
op.object1 = op.COLLISION_SET_ATTACHED_OBJECTS
for l in self.my_world.hands['left_arm'].hand_links:
op.object2 = l
ops.append(copy.deepcopy(op))
ignore.collision_operations = [
gripper, wrist, obj, map_obj, lfing, rfing, rfingtip, forearm, flex
] + ops
return ignore
def test_attached_object_collision_operations(self):
global set_allowed_collision_service_name
global get_allowed_collision_service_name
global revert_allowed_collision_service_name
full_name = default_prefix + '/' + set_allowed_collision_service_name
rospy.wait_for_service(full_name)
set_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
full_name = default_prefix + '/' + get_allowed_collision_service_name
rospy.wait_for_service(full_name)
get_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
full_name = default_prefix + '/' + revert_allowed_collision_service_name
rospy.wait_for_service(full_name)
revert_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, std_srvs.srv.Empty)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
rospy.init_node('test_allowed_collisions')
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "base_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.touch_links = [str() for _ in range(1)]
att_object.touch_links[0] = "r_gripper_palm_link"
att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "base_link"
att_object.object.id = "obj3"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.BOX
att_object.object.shapes[0].dimensions = [float() for _ in range(3)]
att_object.object.shapes[0].dimensions[0] = .2
att_object.object.shapes[0].dimensions[1] = 0.3
att_object.object.shapes[0].dimensions[2] = .05
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = .15
att_object.object.poses[0].position.y = 0
att_object.object.poses[0].position.z = 1.0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
get_res = get_allowed_collision_service_proxy(get_mat)
try:
r_gripper_palm_link_index = get_res.matrix.link_names.index(
"r_gripper_palm_link")
except ValueError:
self.fail(
"No r_gripper_palm_link in AllowedCollisionMatrix link names")
try:
r_gripper_r_finger_tip_link_index = get_res.matrix.link_names.index(
"r_gripper_r_finger_tip_link")
except ValueError:
self.fail(
"No r_gripper_r_finger_tip_link in AllowedCollisionMatrix link names"
)
try:
obj3_index = get_res.matrix.link_names.index("obj3")
except ValueError:
self.fail("No obj3 in AllowedCollisionMatrix link names")
self.assertEqual(
True, get_res.matrix.entries[obj3_index].
enabled[r_gripper_r_finger_tip_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_r_finger_tip_link_index].
enabled[obj3_index])
self.assertEqual(
True, get_res.matrix.entries[obj3_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj3_index])
#this should overwrite touch links
set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest(
)
set_allowed_request.ord.collision_operations = [
CollisionOperation() for _ in range(1)
]
set_allowed_request.ord.collision_operations[0].object1 = "obj3"
set_allowed_request.ord.collision_operations[
0].object2 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.ENABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
False, get_res.matrix.entries[obj3_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
False, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj3_index])
set_allowed_request.ord.collision_operations[
0].object1 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].object2 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.DISABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
True, get_res.matrix.entries[obj3_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj3_index])
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 testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll)
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def test_object_collisions(self):
global set_allowed_collision_service_name
global get_allowed_collision_service_name
global revert_allowed_collision_service_name
full_name = default_prefix + '/' + set_allowed_collision_service_name
rospy.wait_for_service(full_name)
set_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
full_name = default_prefix + '/' + get_allowed_collision_service_name
rospy.wait_for_service(full_name)
get_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
full_name = default_prefix + '/' + revert_allowed_collision_service_name
rospy.wait_for_service(full_name)
revert_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, std_srvs.srv.Empty)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
rospy.init_node('test_allowed_collisions')
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(2)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 1.0
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .05
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = 1.0
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .05
obj1.poses = [Pose() for _ in range(2)]
obj1.poses[0].position.x = 1.0
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = .5
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.poses[1].position.x = 1.0
obj1.poses[1].position.y = 0
obj1.poses[1].position.z = .75
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "obj2"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = 1.0
obj2.shapes[0].dimensions[1] = 1.0
obj2.shapes[0].dimensions[2] = .05
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .15
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = .5
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
obj_pub.publish(obj2)
rospy.sleep(2.0)
get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
get_res = get_allowed_collision_service_proxy(get_mat)
try:
r_gripper_palm_link_index = get_res.matrix.link_names.index(
"r_gripper_palm_link")
except ValueError:
self.fail(
"No r_gripper_palm_link in AllowedCollisionMatrix link names")
try:
obj1_index = get_res.matrix.link_names.index("obj1")
except ValueError:
self.fail("No obj1 in AllowedCollisionMatrix link names")
self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
self.failIf(len(get_res.matrix.entries) <= obj1_index)
self.failIf(
len(get_res.matrix.entries[obj1_index].enabled) <=
r_gripper_palm_link_index)
self.failIf(
len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <=
obj1_index)
self.assertEqual(
False, get_res.matrix.entries[obj1_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
False, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj1_index])
#now disable between all objects
#if enable just this link, it should be true
set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest(
)
set_allowed_request.ord.collision_operations = [
CollisionOperation() for _ in range(1)
]
set_allowed_request.ord.collision_operations[
0].object1 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].object2 = CollisionOperation.COLLISION_SET_OBJECTS
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.DISABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
True, get_res.matrix.entries[obj1_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj1_index])
def make_disable_allowed_collisions_with_exclusions(all, exclude):
ret = []
for i in all:
if i not in exclude:
coll = CollisionOperation()
coll.object1 = i
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.DISABLE
ret.append(coll)
coll2 = CollisionOperation()
coll2.object1 = i
coll2.object2 = coll.COLLISION_SET_ATTACHED_OBJECTS
coll2.operation = coll.DISABLE
ret.append(coll2)
for j in all:
if j != i and j not in exclude:
coll3 = CollisionOperation()
coll3.object1 = i
coll3.object2 = j
coll3.operation = coll.DISABLE
ret.append(coll3)
return ret
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 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 run(self): #(goal, mass, prims, obs):
search_start = time.time()
obj_name = sys.argv[1]
table_height = self.get_table_height()
self.goal.pose.position.z = table_height + 0.01
print "goal=" + str(self.goal)
self.make_marker(self.goal,
"goal_pose",
mtype=Marker.ARROW,
color=[0.0, 1.0, 0.0, 0.8])
obj = self.get_obj(obj_name)
shape = self.get_shape(obj)
obs = self.make_gripper_obs()
mobj = Mobject(mass, shape, prims)
pose = PoseStamped()
pose.header.frame_id = "/torso_lift_link"
pose.pose.position.x = goal.pose.position.x
pose.pose.position.y = goal.pose.position.y
pose.pose.position.z = goal.pose.position.z + 0.02 #.1
pose.pose.orientation = goal.pose.orientation
vels = [0.0, 0.0, 0.0]
# self.place('right_arm', obj_name, pose)
print "DIMS=" + str(shape.dimensions)
(release_pose, wrist_release) = self.get_best_release(mobj, obj, goal)
# release_pose = pose
self.make_marker(release_pose, namespace="release_pose")
print "release_pose=" + str(release_pose)
search_end = time.time()
# block_start = time.time()
# dof = mobj.find_dof(goal, release_pose, vels)
# print "DOF="+str(dof.degrees)
# success = False
# order = dof.index_order()
# while not success: # not passive placing
# if len(order) != 0.0:
# next = order.pop(0)
# print "NEXT="+str(next)
# success = self.block(obs, next, dof[next], shape, goal)
# print "SUCCESS="+str(success)
# else:
# print "BLOCK FAILED"
# break
# block_end = time.time()
print "waiting for place:"
# raw_input()
ignore = OrderedCollisionOperations()
gripper = CollisionOperation()
gripper.object1 = CollisionOperation.COLLISION_SET_ALL #"l_end_effector"
gripper.object2 = CollisionOperation.COLLISION_SET_ALL
gripper.operation = CollisionOperation.DISABLE
wrist = CollisionOperation()
wrist.object1 = "l_wrist_roll_link"
wrist.object2 = CollisionOperation.COLLISION_SET_ALL
wrist.operation = CollisionOperation.DISABLE
obj = CollisionOperation()
obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
obj.object2 = "table" #CollisionOperation.COLLISION_SET_ALL
obj.operation = CollisionOperation.DISABLE
ignore.collision_operations = [gripper, wrist, obj]
self.move("right_arm", wrist_release, ignore)
# self.release("right_arm")
# self.place('right_arm', obj_name, release_pose)
search = search_end - search_start
block = 0.0 #block_end - block_start
return (search, block)
def reset_robot(self, tabletop_collision_map_processing_result=None):
rospy.loginfo('resetting robot')
ordered_collision_operations = OrderedCollisionOperations()
if tabletop_collision_map_processing_result:
closest_index = self.info[0][0]
collision_object_name = tabletop_collision_map_processing_result.collision_object_names[
closest_index]
collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name
collision_operation = CollisionOperation()
collision_operation.object1 = collision_support_surface_name
collision_operation.object2 = ARM_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations = [
collision_operation
]
collision_operation = CollisionOperation()
collision_operation.object1 = collision_support_surface_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations.append(
collision_operation)
collision_operation = CollisionOperation()
collision_operation.object1 = collision_object_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations.append(
collision_operation)
else:
self.reset_collision_map()
current_state = find_current_arm_state()
rospy.loginfo('arm is currently in %s state' % current_state)
if current_state not in ['READY', 'TUCK']:
# check if the gripper is empty
grasp_status = self.get_grasp_status_srv()
if grasp_status.is_hand_occupied:
rospy.loginfo(
'arm is not in any of the following states %s, opening gripper'
% str(['READY', 'TUCK']))
self.open_gripper()
if current_state != 'READY' and not self.ready_arm(
ordered_collision_operations):
return False
self.gentle_close_gripper()
return True
def main():
print 'Starting!'
parser = _get_options_parser()
(options, args) = parser.parse_args()
print 'pause =', options.pause
ss = SimSetup(options.world, False)
world = ss.setup()
planner = Planner()
base_poses = get_base_poses(ss.wi, ss.world)
arm_poses = get_arm_poses(ss.wi, ss.world)
time = 0
#note: these should all update the planning scene as we go
#first plan to initial base position
stime = planner.plan_base(base_poses.push)
rospy.loginfo("Initial base move: " + str(stime))
time += stime
print 'pause =', options.pause
if options.pause:
raw_input()
#now plan to approach to push
stime = planner.plan_arm(arm_poses.approach_push)
rospy.loginfo('Gross move to push: ' + str(stime))
time += stime
if options.pause:
raw_input()
ops = OrderedCollisionOperations()
op = CollisionOperation()
op.operation = op.DISABLE
op.object1 = world.object.id
op.object2 = world.object_support_surface
ops.collision_operations.append(op)
op = copy.deepcopy(op)
op.object2 = 'r_end_effector'
ops.collision_operations.append(op)
op = copy.deepcopy(op)
op.object1 = world.object_support_surface
ops.collision_operations.append(op)
stime = planner.plan_arm_interpolated(arm_poses.init_push,
ordered_colls=ops)
rospy.loginfo('Approach to push: ' + str(stime))
time += stime
if options.pause:
raw_input()
stime = planner.plan_arm_interpolated(arm_poses.final_push,
ordered_colls=ops)
rospy.loginfo('Push: ' + str(stime))
time += stime
co = copy.deepcopy(world.object)
co.poses[0].position.x -= PUSH_DIST
ss.wi.add_object(co)
planner.psi.reset()
if options.pause:
raw_input()
stime = planner.plan_arm_interpolated(arm_poses.retreat_push,
ordered_colls=ops)
rospy.loginfo('Retreat: ' + str(stime))
time += stime
if options.pause:
raw_input()
stime = planner.plan_base(base_poses.pick)
rospy.loginfo('Base move to pick: ' + str(stime))
time += stime
if options.pause:
raw_input()
stime = planner.plan_arm(arm_poses.approach_pick)
rospy.loginfo('Gross move to Pick: ' + str(stime))
time += stime
if options.pause:
raw_input()
stime = planner.plan_arm_interpolated(arm_poses.pick, ordered_colls=ops)
rospy.loginfo('Approach to pick: ' + str(stime))
time += stime
if options.pause:
raw_input()
stime = planner.plan_arm_interpolated(arm_poses.lift, ordered_colls=ops)
rospy.loginfo('Lift: ' + str(stime))
time += stime
if options.pause:
raw_input()
for g in base_poses.goal:
stime = planner.plan_base(g)
rospy.loginfo('Base move to goal: ' + str(stime))
time += stime
if options.pause:
raw_input()
stime = planner.plan_arm(arm_poses.goal)
rospy.loginfo('Arm move to goal: ' + str(stime))
time += stime
if options.pause:
raw_input()
rospy.loginfo('TIME = ' + str(time))
def test_self_collisions(self):
global set_allowed_collision_service_name
global get_allowed_collision_service_name
global revert_allowed_collision_service_name
full_name = default_prefix + '/' + set_allowed_collision_service_name
rospy.wait_for_service(full_name)
set_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
full_name = default_prefix + '/' + get_allowed_collision_service_name
rospy.wait_for_service(full_name)
get_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
full_name = default_prefix + '/' + revert_allowed_collision_service_name
rospy.wait_for_service(full_name)
revert_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, std_srvs.srv.Empty)
#first we get the default
get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
get_res = get_allowed_collision_service_proxy(get_mat)
#entry for left gripper and right gripper should be false - just an example
try:
r_gripper_palm_link_index = get_res.matrix.link_names.index(
"r_gripper_palm_link")
except ValueError:
self.fail(
"No r_gripper_palm_link in AllowedCollisionMatrix link names")
try:
l_gripper_palm_link_index = get_res.matrix.link_names.index(
"l_gripper_palm_link")
except ValueError:
self.fail(
"No l_gripper_palm_link in AllowedCollisionMatrix link names")
self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
self.failIf(len(get_res.matrix.entries) <= l_gripper_palm_link_index)
self.failIf(
len(get_res.matrix.entries[l_gripper_palm_link_index].enabled) <=
r_gripper_palm_link_index)
self.failIf(
len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <=
l_gripper_palm_link_index)
self.assertEqual(
False, get_res.matrix.entries[l_gripper_palm_link_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
False, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[l_gripper_palm_link_index])
#if enable just this link, it should be true
set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest(
)
set_allowed_request.ord.collision_operations = [
CollisionOperation() for _ in range(1)
]
set_allowed_request.ord.collision_operations[
0].object1 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].object2 = "l_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.DISABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.0)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
True, get_res.matrix.entries[l_gripper_palm_link_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[l_gripper_palm_link_index])
#now we enable r_end_effector versus l_end_effector, and it should be false again
set_allowed_request.ord.collision_operations[
0].object1 = "r_end_effector"
set_allowed_request.ord.collision_operations[
0].object2 = "l_end_effector"
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.ENABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.0)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
False, get_res.matrix.entries[l_gripper_palm_link_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
False, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[l_gripper_palm_link_index])
def push_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
current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
start_angles = current_state.actual.positions
full_state = rospy.wait_for_message("/joint_states", JointState)
req = GetPositionFKRequest()
req.header.frame_id = "base_link"
req.fk_link_names = [GRIPPER_LINK_FRAME]
req.robot_state.joint_state = full_state
if not self.set_planning_scene_diff_client():
rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME)
self.action_server.set_aborted()
return
pose = self.get_fk_srv(req).pose_stamped[0].pose
frame_id = "base_link"
approachpos = [pose.position.x, pose.position.y, pose.position.z]
approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
push_distance = 0.40
grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z]
graspquat = [0.00000, 0.00000, 0.70711, -0.70711]
# attach object to gripper, they will move together
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)
# 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
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 = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
res = self.check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
frame=frame_id,
ordered_collision_operations=ordered_collision_operations,
)
error_code_dict = {
ArmNavigationErrorCodes.SUCCESS: 0,
ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
ArmNavigationErrorCodes.PLANNING_FAILED: 4,
}
trajectory_len = len(res.trajectory.joint_trajectory.points)
trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)]
vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)]
times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)]
error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes]
# if even one code is not 0, abort
if sum(error_codes) != 0:
for ind in range(len(trajectory)):
rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
rospy.loginfo("")
for ind in range(len(trajectory)):
rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind]))
rospy.logerr("%s: attempted push failed" % ACTION_NAME)
self.action_server.set_aborted()
return
req = FilterJointTrajectoryRequest()
req.trajectory = res.trajectory.joint_trajectory
req.trajectory.points = req.trajectory.points[1:] # skip zero velocity point
req.allowed_time = rospy.Duration(2.0)
filt_res = self.trajectory_filter_srv(req)
goal = FollowJointTrajectoryGoal()
goal.trajectory = filt_res.trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
rospy.sleep(0.5)
self.trajectory_controller.send_goal(goal)
self.trajectory_controller.wait_for_result()
state = self.trajectory_controller.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound))
return
rospy.logerr("%s: attempted push failed" % ACTION_NAME)
self.stop_audio_recording_srv(False)
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
rospy.sleep(1)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'L7_wrist_roll_link'#'base_link'
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = coll_map_res.collision_object_names[0]
obj.link_name = 'L7_wrist_roll_link'
obj.touch_links = ['L7_wrist_roll_link', 'L8_left_finger_link', 'L9_right_finger_link']
attached_object_pub.publish(obj)
rospy.sleep(1)
ordered_collision_operations = OrderedCollisionOperations()
collision_operation = CollisionOperation()
collision_operation.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation.object2 = coll_map_res.collision_support_surface_name
collision_operation.operation = CollisionOperation.DISABLE
ordered_collision_operations.collision_operations = [collision_operation]
if not move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts, gripper_paddings, ordered_collision_operations):
exit(1)
rospy.loginfo('Pickup stage has successfully finished. Will place the object now')
############################################################################
####################### PLACE STAGE START HERE #############################
listener = TransformListener()
def make_disable_allowed_collisions_with_exclusions(all, exclude):
ret = []
for i in all:
if i not in exclude:
coll = CollisionOperation()
coll.object1 = i
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.DISABLE
ret.append(coll)
coll2 = CollisionOperation()
coll2.object1 = i
coll2.object2 = coll.COLLISION_SET_ATTACHED_OBJECTS
coll2.operation = coll.DISABLE
ret.append(coll2)
for j in all:
if j != i and j not in exclude:
coll3 = CollisionOperation()
coll3.object1 = i
coll3.object2 = j
coll3.operation = coll.DISABLE
ret.append(coll3)
return ret
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(
coll)
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
check_cartesian_path_lists(approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
pos_spacing=0.02,
collision_aware=1,
collision_check_resolution=1,
steps_before_abort=-1)
#print "using current start angles"
#check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles = [], pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 1, steps_before_abort = -1)
print "ignoring collisions with all collision points"
collision_oper = CollisionOperation(object1 = "points", \
object2 = CollisionOperation.COLLISION_SET_ALL, \
operation = CollisionOperation.DISABLE)
ordered_collision_operations = OrderedCollisionOperations([
collision_oper,
])
check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
pos_spacing=0.02,
collision_aware=1,
collision_check_resolution=1,
steps_before_abort=-1,
ordered_collision_operations=ordered_collision_operations)
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 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 push_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
current_state = rospy.wait_for_message('l_arm_controller/state',
FollowJointTrajectoryFeedback)
start_angles = current_state.actual.positions
full_state = rospy.wait_for_message('/joint_states', JointState)
req = GetPositionFKRequest()
req.header.frame_id = 'base_link'
req.fk_link_names = [GRIPPER_LINK_FRAME]
req.robot_state.joint_state = full_state
if not self.set_planning_scene_diff_client():
rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME)
self.action_server.set_aborted()
return
pose = self.get_fk_srv(req).pose_stamped[0].pose
frame_id = 'base_link'
approachpos = [pose.position.x, pose.position.y, pose.position.z]
approachquat = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
push_distance = 0.40
grasppos = [
pose.position.x, pose.position.y - push_distance, pose.position.z
]
graspquat = [0.00000, 0.00000, 0.70711, -0.70711]
# attach object to gripper, they will move together
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)
# 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
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 = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [
collision_operation1, collision_operation2
]
res = self.check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
frame=frame_id,
ordered_collision_operations=ordered_collision_operations)
error_code_dict = {
ArmNavigationErrorCodes.SUCCESS: 0,
ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
ArmNavigationErrorCodes.PLANNING_FAILED: 4
}
trajectory_len = len(res.trajectory.joint_trajectory.points)
trajectory = [
res.trajectory.joint_trajectory.points[i].positions
for i in range(trajectory_len)
]
vels = [
res.trajectory.joint_trajectory.points[i].velocities
for i in range(trajectory_len)
]
times = [
res.trajectory.joint_trajectory.points[i].time_from_start
for i in range(trajectory_len)
]
error_codes = [
error_code_dict[error_code.val]
for error_code in res.trajectory_error_codes
]
# if even one code is not 0, abort
if sum(error_codes) != 0:
for ind in range(len(trajectory)):
rospy.loginfo("error code " + str(error_codes[ind]) +
" pos : " + self.pplist(trajectory[ind]))
rospy.loginfo("")
for ind in range(len(trajectory)):
rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() +
"vels: " + self.pplist(vels[ind]))
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.action_server.set_aborted()
return
req = FilterJointTrajectoryRequest()
req.trajectory = res.trajectory.joint_trajectory
req.trajectory.points = req.trajectory.points[
1:] # skip zero velocity point
req.allowed_time = rospy.Duration(2.0)
filt_res = self.trajectory_filter_srv(req)
goal = FollowJointTrajectoryGoal()
goal.trajectory = filt_res.trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
rospy.sleep(0.5)
self.trajectory_controller.send_goal(goal)
self.trajectory_controller.wait_for_result()
state = self.trajectory_controller.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(
PushObjectResult(sound_msg.recorded_sound))
return
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.stop_audio_recording_srv(False)
self.action_server.set_aborted()
def reset_robot(self, tabletop_collision_map_processing_result=None):
rospy.loginfo('resetting robot')
ordered_collision_operations = OrderedCollisionOperations()
if tabletop_collision_map_processing_result:
closest_index = self.info[0][0]
collision_object_name = tabletop_collision_map_processing_result.collision_object_names[closest_index]
collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name
collision_operation = CollisionOperation()
collision_operation.object1 = collision_support_surface_name
collision_operation.object2 = ARM_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations = [collision_operation]
collision_operation = CollisionOperation()
collision_operation.object1 = collision_support_surface_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations.append(collision_operation)
collision_operation = CollisionOperation()
collision_operation.object1 = collision_object_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations.append(collision_operation)
else:
self.reset_collision_map()
current_state = find_current_arm_state()
rospy.loginfo('arm is currently in %s state' % current_state)
if current_state not in ['READY', 'TUCK']:
# check if the gripper is empty
grasp_status = self.get_grasp_status_srv()
if grasp_status.is_hand_occupied:
rospy.loginfo('arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK']))
self.open_gripper()
if current_state != 'READY' and not self.ready_arm(ordered_collision_operations): return False
self.gentle_close_gripper()
return True
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()
