def attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
This does NOT "snap" the object to the end effector.
Rather, now when the robot moves, the object is assumed to remain stationary with respect to the robot's
end effector instead of the world. Collisions will be checked between the object and the world as the
object moves, but collisions between the object and the end effector will be ignored. The link the object
is attached to and the links with which collisions are ignored are defined by the hand description
(see hand_description.py).
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object ot
**object_id (string):** The ID of the object to attach
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.object.operation.operation = obj.object.operation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.object.id = object_id
obj.touch_links = self.hands[arm_name].touch_links
self._publish(obj, self._attached_object_pub)
def attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
This does NOT "snap" the object to the end effector.
Rather, now when the robot moves, the object is assumed to remain stationary with respect to the robot's
end effector instead of the world. Collisions will be checked between the object and the world as the
object moves, but collisions between the object and the end effector will be ignored. The link the object
is attached to and the links with which collisions are ignored are defined by the hand description
(see hand_description.py).
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object ot
**object_id (string):** The ID of the object to attach
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.object.operation.operation = obj.object.operation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.object.id = object_id
obj.touch_links = self.hands[arm_name].touch_links
self._publish(obj, self._attached_object_pub)
def detach_all_objects_from_gripper(self, whicharm='r'):
rospy.loginfo("detaching all objects from gripper %s" % whicharm)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = "base_link"
obj.link_name = whicharm + "_gripper_r_finger_tip_link"
obj.object.operation.operation = CollisionObjectOperation.REMOVE
self.attached_object_pub.publish(obj)
def detach_all_objects_from_gripper(self, whicharm = 'r'):
rospy.loginfo("detaching all objects from gripper %s"%whicharm)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = "base_link"
obj.link_name = whicharm+"_gripper_r_finger_tip_link"
obj.object.operation.operation = CollisionObjectOperation.REMOVE
self.attached_object_pub.publish(obj)
def reset_attached_objects(self):
'''
Removes all collision objects that are currently attached to the robot.
'''
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = 'all'
reset_attached_objects.object.header.frame_id = 'base_link'
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = self._get_reset_object()
self._publish(reset_attached_objects, self._attached_object_pub)
rospy.logdebug('Attached object reset')
def attach_object_to_gripper(self, object_name, whicharm = 'r'):
obj = AttachedCollisionObject()
obj.link_name = whicharm+"_gripper_r_finger_tip_link"
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = "base_link"
obj.object.id = object_name
touch_links = ["_gripper_palm_link", "_gripper_r_finger_tip_link", "_gripper_l_finger_tip_link", "_gripper_l_finger_link", "_gripper_r_finger_link"]
obj.touch_links = [whicharm+link for link in touch_links]
self.attached_object_pub.publish(obj)
def reset_attached_objects(self):
'''
Removes all collision objects that are currently attached to the robot.
'''
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = 'all'
reset_attached_objects.object.header.frame_id = 'base_link'
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = self._get_reset_object()
self._publish(reset_attached_objects, self._attached_object_pub)
rospy.logdebug('Attached object reset')
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
The object must have been attached in the world or planning scene diff previously. This removes the object
from the planning scene even if it currently exists in the world.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not previously attached in the world or the diff and therefore cannot be
detached; True otherwise.
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
#did the object ever exist in the world?
#if so, we need to continually remove it after detaching
#but it will always be in
#corresponding collision object if it exists
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
#if the planning scene originally added this object, we could
#just remove it from the list of collision objects but we have
#no way of knowing that at this point
co.operation.operation = co.operation.REMOVE
for p in range(len(co.poses)):
co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
co.header.frame_id = self.world_frame
break
for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
if ao.object.id == object_id:
rospy.loginfo('Object was attached by the planning scene interface')
self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
self.send_diff()
return True
rospy.loginfo('Object was not attached by the planning scene interface')
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
obj.object = ao.object
obj.object.operation.operation = obj.object.operation.REMOVE
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
self.send_diff()
return True
rospy.logwarn('Object '+object_id+' not attached to gripper')
return False
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
The object must have been attached in the world or planning scene diff previously. This removes the object
from the planning scene even if it currently exists in the world.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not previously attached in the world or the diff and therefore cannot be
detached; True otherwise.
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
#did the object ever exist in the world?
#if so, we need to continually remove it after detaching
#but it will always be in
#corresponding collision object if it exists
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
#if the planning scene originally added this object, we could
#just remove it from the list of collision objects but we have
#no way of knowing that at this point
co.operation.operation = co.operation.REMOVE
for p in range(len(co.poses)):
co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
co.header.frame_id = self.world_frame
break
for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
if ao.object.id == object_id:
rospy.loginfo('Object was attached by the planning scene interface')
self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
self.send_diff()
return True
rospy.loginfo('Object was not attached by the planning scene interface')
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
obj.object = ao.object
obj.object.operation.operation = obj.object.operation.REMOVE
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
self.send_diff()
return True
rospy.logwarn('Object '+object_id+' not attached to gripper')
return False
def detach_all_objects_from_gripper(self, arm_name):
'''
Detaches all objects and removes them from the collision space entirely.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach all objects.
'''
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.link_name = self.hands[arm_name].attach_link
obj.object.id = 'all'
obj.object.operation.operation = obj.object.operation.REMOVE
self._publish(obj, self._attached_object_pub)
def detach_all_objects_from_gripper(self, arm_name):
'''
Detaches all objects and removes them from the collision space entirely.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach all objects.
'''
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.link_name = self.hands[arm_name].attach_link
obj.object.id = 'all'
obj.object.operation.operation = obj.object.operation.REMOVE
self._publish(obj, self._attached_object_pub)
def detach_and_add_back_objects_attached_to_gripper(self, whicharm = 'r', object_collision_name = None):
rospy.loginfo("detaching all objects from gripper %s and adding them back to the collision map"%whicharm)
if object_collision_name == None:
rospy.loginfo("need to specify the object name! Detaching and not adding back object.")
self.detach_all_objects_from_gripper(whicharm)
return
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = "base_link"
obj.link_name = whicharm+"_gripper_r_finger_tip_link"
obj.object.id = object_collision_name
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
self.attached_object_pub.publish(obj)
def attach_object_to_gripper(self, object_name, whicharm='r'):
obj = AttachedCollisionObject()
obj.link_name = whicharm + "_gripper_r_finger_tip_link"
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = "base_link"
obj.object.id = object_name
touch_links = [
"_gripper_palm_link", "_gripper_r_finger_tip_link",
"_gripper_l_finger_tip_link", "_gripper_l_finger_link",
"_gripper_r_finger_link"
]
obj.touch_links = [whicharm + link for link in touch_links]
self.attached_object_pub.publish(obj)
def drop_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
# check that we have something in hand before dropping 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 drop' % ACTION_NAME)
self.action_server.set_aborted()
return
# record sound padded by 0.5 seconds from start and 1.5 seconds from the end
self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id)
rospy.sleep(0.5)
self.open_gripper()
rospy.sleep(1.5)
# check if gripper actually opened first
sound_msg = None
grasp_status = self.get_grasp_status_srv()
# if there something in the gripper - drop failed
if grasp_status.is_hand_occupied:
self.stop_audio_recording_srv(False)
else:
sound_msg = self.stop_audio_recording_srv(True)
# delete the object that we just dropped, we don't really know where it will land
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 = goal.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(
DropObjectResult(sound_msg.recorded_sound))
else:
self.action_server.set_aborted()
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
'''
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.link_name = self.hands[arm_name].attach_link
obj.object.id = object_id
obj.object.operation.operation = obj.object.operation.REMOVE
self._publish(obj, self._attached_object_pub)
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
'''
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.link_name = self.hands[arm_name].attach_link
obj.object.id = object_id
obj.object.operation.operation = obj.object.operation.REMOVE
self._publish(obj, self._attached_object_pub)
def drop_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
# check that we have something in hand before dropping 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 drop' % ACTION_NAME)
self.action_server.set_aborted()
return
# record sound padded by 0.5 seconds from start and 1.5 seconds from the end
self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id)
rospy.sleep(0.5)
self.open_gripper()
rospy.sleep(1.5)
# check if gripper actually opened first
sound_msg = None
grasp_status = self.get_grasp_status_srv()
# if there something in the gripper - drop failed
if grasp_status.is_hand_occupied:
self.stop_audio_recording_srv(False)
else:
sound_msg = self.stop_audio_recording_srv(True)
# delete the object that we just dropped, we don't really know where it will land
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 = goal.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(DropObjectResult(sound_msg.recorded_sound))
else:
self.action_server.set_aborted()
def detach_and_add_back_attached_object(self, arm_name, object_id):
'''
Detaches a single object from the gripper and adds it back to the world at its current location.
From here on, it is assumed that the object remains stationary in the world.
**Args:**
**arm_name (string):** The arm ('right_arm' or 'left_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
'''
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.link_name = self.hands[arm_name].attach_link
obj.object.id = object_id
obj.object.operation.operation = obj.object.operation.DETACH_AND_ADD_AS_OBJECT
self._publish(obj, self._attached_object_pub)
def detach_and_add_back_attached_object(self, arm_name, object_id):
'''
Detaches a single object from the gripper and adds it back to the world at its current location.
From here on, it is assumed that the object remains stationary in the world.
**Args:**
**arm_name (string):** The arm ('right_arm' or 'left_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
'''
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = 'base_link'
obj.link_name = self.hands[arm_name].attach_link
obj.object.id = object_id
obj.object.operation.operation = obj.object.operation.DETACH_AND_ADD_AS_OBJECT
self._publish(obj, self._attached_object_pub)
def detach_and_add_back_objects_attached_to_gripper(
self, whicharm='r', object_collision_name=None):
rospy.loginfo(
"detaching all objects from gripper %s and adding them back to the collision map"
% whicharm)
if object_collision_name == None:
rospy.loginfo(
"need to specify the object name! Detaching and not adding back object."
)
self.detach_all_objects_from_gripper(whicharm)
return
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = "base_link"
obj.link_name = whicharm + "_gripper_r_finger_tip_link"
obj.object.id = object_collision_name
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
self.attached_object_pub.publish(obj)
def get_virtual_gloves():
r_glove = AttachedCollisionObject()
r_glove.object.header.stamp = rospy.get_rostime()
r_glove.object.header.frame_id = '/r_gripper_palm_link'
r_glove.link_name = 'r_gripper_palm_link'
r_glove.object.id = 'r_gripper_glove'
r_glove.object.operation.operation = CollisionObjectOperation.ADD
glove_shape = Shape()
glove_shape.type = Shape.BOX
glove_shape.dimensions = [ 0.25, 0.18, 0.1 ]
glove_pose = Pose()
glove_pose.orientation.w = 1
glove_pose.position.x = 0.1
# Pose will be zero
r_glove.touch_links = ['r_end_effector',
'r_wrist_roll_link',
'r_wrist_flex_link',
'r_forearm_link']
r_glove.object.shapes.append(glove_shape)
r_glove.object.poses.append(glove_pose)
l_glove = copy.deepcopy(r_glove)
l_glove.object.id = 'l_gripper_glove'
l_glove.object.header.frame_id = '/l_gripper_palm_link'
l_glove.link_name = 'l_gripper_palm_link'
l_glove.touch_links = ['l_end_effector',
'l_wrist_roll_link',
'l_wrist_flex_link',
'l_forearm_link']
return r_glove, l_glove
def reset_collision_map(self):
#remove all objects
reset_object = CollisionObject()
reset_object.operation.operation = CollisionObjectOperation.REMOVE
reset_object.header.frame_id = "base_link"
reset_object.header.stamp = rospy.Time.now()
reset_object.id = "all"
self.object_in_map_pub.publish(reset_object)
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = "all"
reset_attached_objects.object.header.frame_id = "base_link"
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = reset_object
self.attached_object_pub.publish(reset_attached_objects)
#and clear the octomap
self.clear_octomap()
rospy.loginfo("collision objects reset")
self.object_in_map_current_id = 0.
return 1
def reset_collision_map(self):
#remove all objects
reset_object = CollisionObject()
reset_object.operation.operation = CollisionObjectOperation.REMOVE
reset_object.header.frame_id = "base_link"
reset_object.header.stamp = rospy.Time.now()
reset_object.id = "all"
self.object_in_map_pub.publish(reset_object)
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = "all"
reset_attached_objects.object.header.frame_id = "base_link"
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = reset_object
self.attached_object_pub.publish(reset_attached_objects)
#and clear the octomap
self.clear_octomap()
rospy.loginfo("collision objects reset")
self.object_in_map_current_id = 0.
return 1
rospy.sleep(1)
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
posture_controller.send_goal(pg)
posture_controller.wait_for_result()
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)
def place(self,place_goal):
placeresult = PlaceResult()
target_pose_to_execute_ = PoseStamped()
#for location, check path from approach pose to release pose first and then check motion to approach pose
motion_plan_res=GetMotionPlanResponse()
object_to_attach = place_goal.collision_object_name
# get current hand pose
self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0))
try:
(trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time())
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr("Cannot get current palm pose")
placeresult.manipulation_result.value = ManipulationResult.ERROR
return placeresult
current_pose_= PoseStamped()
current_pose_.header.frame_id = "/world"
current_pose_.pose.position = Point(trans[0],trans[1],trans[2])
current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3])
# for each place location
for index, target_pose_ in enumerate(place_goal.place_locations):
#compute straight trajectory to approach distance
target_approach_pose_= PoseStamped()
target_approach_pose_.header.frame_id = "/world"
target_approach_pose_.pose.position = Point(target_pose_.pose.position.x,target_pose_.pose.position.y,target_pose_.pose.position.z+place_goal.approach.desired_distance)
target_approach_pose_.pose.orientation = Quaternion(target_pose_.pose.orientation.x,target_pose_.pose.orientation.y,target_pose_.pose.orientation.z,target_pose_.pose.orientation.w) #keep same orientation for the first test
# for distance from 0 (release_pose) to desired_approach distance (approach_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(target_approach_pose_, target_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to approach pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Place pose number "+str(index)+" approach is possible, checking motion plan to approach pose")
#print interpolated_motion_plan_res
# check and plan motion to this approach pose
motion_plan_res = self.plan.plan_arm_motion( place_goal.arm_name, "jointspace" ,target_approach_pose_)#,object_to_attach)
#if this approach pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Place pose number "+str(index)+" is possible, executing it")
# copy the pose to execute for the following steps
target_pose_to_execute_ = copy.deepcopy(target_pose_)
break
else:
rospy.logerr("Place pose number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Place pose number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach approach pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
#put hand in pre-grasp posture (to gently release)
if self.pre_grasp_exec(place_goal.grasp)<0:
#QMessageBox.warning(self, "Warning",
# "Release action failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
#detach the object from the hand
rospy.loginfo("Now we detach the attached object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attached object to be detached published")
else:
rospy.logerr("None of the place pose tested is possible")
placeresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return placeresult
placeresult.manipulation_result.value = ManipulationResult.SUCCESS
placeresult.place_location= target_pose_to_execute_
return placeresult
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
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.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
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)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 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)
def pick(self,pickup_goal):
#prepare result
pickresult = PickupResult()
#get grasps for the object
# fill up a grasp planning request
grasp_planning_req = GraspPlanningRequest()
grasp_planning_req.arm_name = pickup_goal.arm_name
grasp_planning_req.target = pickup_goal.target
object_to_attach = pickup_goal.collision_object_name
# call grasp planning service
grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
#print grasp_planning_res
# process grasp planning result
if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
else:
rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")
# for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)
#for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
motion_plan_res=GetMotionPlanResponse()
grasp_to_execute_=Grasp()
for index, grasp in enumerate(grasp_planning_res.grasps):
# extract grasp_pose
grasp_pose_ = PoseStamped()
grasp_pose_.header.frame_id = "/world";
grasp_pose_.pose = grasp.grasp_pose
grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# currently add this to Z because approach vector needs to be computed somehow first (TODO)
pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
#print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )
#if this pre-grasp pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
# copy the grasp to execute for the following steps
grasp_to_execute_ = copy.deepcopy(grasp)
break
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#put hand in pre-grasp posture
if self.pre_grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Pre-grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach pregrasp pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#time.sleep(20) # TODO use actionlib here
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#grasp
if self.grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#attach the collision object to the hand (should be cleaned-up)
rospy.loginfo("Now we attach the object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attach object published")
else:
rospy.logerr("None of the grasps tested is possible")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
pickresult.manipulation_result.value = ManipulationResult.SUCCESS
pickresult.grasp= grasp_to_execute_
return pickresult
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 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 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 setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object',CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
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.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
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)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject();
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 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)
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 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 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 attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
The object must already exist in the planning scene
(although it does not need to exist in the current state of the world). This does NOT "snap" the object to
the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect
to the robot's end effector instead of the world. Collisions will be checked between the object and the
world as the object moves, but collisions between the object and the end effector will be ignored.
The link the object is attached to and the links with which collisions are ignored are defined by the hand
description (see hand_description.py).
The object will be attached to the robot according to the current state of the robot and object in the
planning scene as maintained by this class (i.e. with any prior changes you made to the diff).
To undo this function, you can use detach_object and add_object but you must remember the position of the
object where it was originally attached! If you call detach_and_add_back_attached_object, it will add
the object back at its current location in the planning scene, NOT at the location at which it was
originally attached.
The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing
an attached object with operation ADD and a collision object with operation REMOVE.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to
**object_id (string):** The ID of the object to attach
**Returns:**
False if the object did not previously exist in the world or planning scene diff and therefore cannot be
attached; True otherwise.
'''
#Note: It is important to send the full collision object (not just the id) in the attached collision object.
#This was quite complicated to figure out the first time.
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.touch_links = self.hands[arm_name].touch_links
diff_obj = None
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
rospy.loginfo('Object was added to or modified in planning scene.')
diff_obj = co
break
if not diff_obj:
rospy.loginfo('Object was not previously added to or modified in planning scene.')
diff_obj = self.collision_object(object_id)
if not diff_obj:
rospy.logerr('Cannot attach object '+object_id+'. This object does not exist')
return False
self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
#convert it into the frame of the hand - it remains stationary with respect to this
for p in range(len(diff_obj.poses)):
diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
diff_obj.header.frame_id = obj.link_name
diff_obj.operation.operation = diff_obj.operation.REMOVE
obj.object = copy.deepcopy(diff_obj)
obj.object.operation.operation = obj.object.operation.ADD
self.send_diff()
return True
def attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
The object must already exist in the planning scene
(although it does not need to exist in the current state of the world). This does NOT "snap" the object to
the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect
to the robot's end effector instead of the world. Collisions will be checked between the object and the
world as the object moves, but collisions between the object and the end effector will be ignored.
The link the object is attached to and the links with which collisions are ignored are defined by the hand
description (see hand_description.py).
The object will be attached to the robot according to the current state of the robot and object in the
planning scene as maintained by this class (i.e. with any prior changes you made to the diff).
To undo this function, you can use detach_object and add_object but you must remember the position of the
object where it was originally attached! If you call detach_and_add_back_attached_object, it will add
the object back at its current location in the planning scene, NOT at the location at which it was
originally attached.
The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing
an attached object with operation ADD and a collision object with operation REMOVE.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to
**object_id (string):** The ID of the object to attach
**Returns:**
False if the object did not previously exist in the world or planning scene diff and therefore cannot be
attached; True otherwise.
'''
#Note: It is important to send the full collision object (not just the id) in the attached collision object.
#This was quite complicated to figure out the first time.
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.touch_links = self.hands[arm_name].touch_links
diff_obj = None
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
rospy.loginfo('Object was added to or modified in planning scene.')
diff_obj = co
break
if not diff_obj:
rospy.loginfo('Object was not previously added to or modified in planning scene.')
diff_obj = self.collision_object(object_id)
if not diff_obj:
rospy.logerr('Cannot attach object '+object_id+'. This object does not exist')
return False
self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
#convert it into the frame of the hand - it remains stationary with respect to this
for p in range(len(diff_obj.poses)):
diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
diff_obj.header.frame_id = obj.link_name
diff_obj.operation.operation = diff_obj.operation.REMOVE
obj.object = copy.deepcopy(diff_obj)
obj.object.operation.operation = obj.object.operation.ADD
self.send_diff()
return True
def test_add_convert_objects():
obj_pub = rospy.Publisher('collision_object',CollisionObject)
att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
rospy.init_node('test_collision_objects')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = .1
obj1.shapes[0].dimensions[2] = .75
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .375
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
att_obj = AttachedCollisionObject()
att_obj.link_name = "r_gripper_palm_link"
att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "r_gripper_palm_link"
obj2.id = "obj2";
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.CYLINDER
obj2.shapes[0].dimensions = [float() for _ in range(2)]
obj2.shapes[0].dimensions[0] = .025
obj2.shapes[0].dimensions[1] = .5
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .12
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
att_obj.object = obj2
r = rospy.Rate(.1)
while(True):
obj1.header.stamp = rospy.Time.now()
obj_pub.publish(obj1)
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
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()