class ObjectChooser(QWidget):
"""
Display the list of found objects in a table. Make PickUp calls to
manipulator action server on selected objects.
"""
def __init__(self, parent, gui, title):
QWidget.__init__(self)
self.gui = gui
self.grasp = None
self.title = QLabel()
self.title.setText(title)
self.draw_functions = draw_functions.DrawFunctions('grasp_markers')
self.pickup_result = None
self.listener = tf.TransformListener()
self.grasp_display_publisher = rospy.Publisher("/grasp_display", Grasp)
self.pickupservice = Execution()
def draw(self):
self.frame = QFrame(self)
self.tree = QTreeWidget()
self.connect(self.tree,
SIGNAL('itemDoubleClicked (QTreeWidgetItem *, int)'),
self.double_click)
self.tree.setHeaderLabels(["Object Name", "Maker", "tags"])
self.tree.resizeColumnToContents(0)
self.tree.resizeColumnToContents(1)
self.tree.resizeColumnToContents(2)
self.layout = QVBoxLayout()
self.layout.addWidget(self.title)
self.layout.addWidget(self.tree)
self.frame.setLayout(self.layout)
layout = QVBoxLayout()
layout.addWidget(self.frame)
self.frame.show()
self.setLayout(layout)
self.show()
def double_click(self, item, value):
self.object_name = str(item.text(0))
self.object = self.gui.found_objects[self.object_name]
graspable_object = self.object.graspable_object
# draw a bounding box around the selected object
(box_pose, box_dims) = self.call_find_cluster_bounding_box(
graspable_object.cluster)
if box_pose == None:
return
box_mat = pose_to_mat(box_pose.pose)
rospy.logerr("box_pose %f %f %f q: %f %f %f %f",
box_pose.pose.position.x, box_pose.pose.position.y,
box_pose.pose.position.z, box_pose.pose.orientation.x,
box_pose.pose.orientation.y, box_pose.pose.orientation.z,
box_pose.pose.orientation.w)
box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2],
[box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]]
self.box_pose = box_pose
self.draw_functions.draw_rviz_box(box_mat,
box_ranges,
'/world',
ns='bounding box',
color=[0, 0, 1],
opaque=0.25,
duration=60)
# call the pickup service
res = self.pickup(graspable_object, self.object.graspable_object_name,
self.object_name)
if res == 0: #correctly picked up
#TODO: set up place_location
executed_grasp = self.pickup_result.grasp
initial_pose = PoseStamped()
initial_pose.header.stamp = rospy.get_rostime()
initial_pose.header.frame_id = "/world"
initial_pose.pose.position.x = self.box_pose.pose.position.x + 0.1
initial_pose.pose.position.y = self.box_pose.pose.position.y - 0.3
initial_pose.pose.position.z = self.box_pose.pose.position.z - box_dims.z / 2 # graspable object is from bottom but bounding box is at center !
q = transformations.quaternion_about_axis(-0.05, (0, 0, 1))
initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x #q[0]
initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y #q[1]
initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z #q[2]
initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w #q[3]
self.list_of_poses = self.compute_list_of_poses(
initial_pose, graspable_object, executed_grasp)
#print "list of pose",self.list_of_poses
self.place_object(graspable_object,
self.object.graspable_object_name,
self.object_name, self.list_of_poses)
def place_click(self):
if (self.pickup_result):
grasp = self.pickup_result.grasp
self.pickupservice.pre_grasp_exec(grasp, 10.0)
else:
self.pickupservice.grasp_release_exec(10.0)
initial_pose = PoseStamped()
initial_pose.header.stamp = rospy.get_rostime()
initial_pose.header.frame_id = "/world" #"/fixed"
#fake hand to world pose
initial_pose.pose.position.x = 0.4 #self.box_pose.pose.position.x+0.0
initial_pose.pose.position.y = 0.091 # self.box_pose.pose.position.y-0.2
initial_pose.pose.position.z = 1.25 #self.box_pose.pose.position.z+0.05
q = transformations.quaternion_about_axis(-0.05, (0, 0, 1))
initial_pose.pose.orientation.x = 0.41 #self.box_pose.pose.orientation.x#q[0]
initial_pose.pose.orientation.y = 0.695 #self.box_pose.pose.orientation.y#q[1]
initial_pose.pose.orientation.z = 0.52 #self.box_pose.pose.orientation.z#q[2]
initial_pose.pose.orientation.w = 0.284 #self.box_pose.pose.orientation.w#q[3]
executed_grasp = Grasp()
executed_grasp.grasp_pose = copy.deepcopy(initial_pose.pose)
#fake obj world origin pose
initial_pose.pose.position.x = 0.43
initial_pose.pose.position.y = 0.149
initial_pose.pose.position.z = 1.088
initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0]
initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1]
initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2]
initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3]
#fake graspable object
graspable_object = GraspableObject()
graspable_object.reference_frame_id = "/world"
mypotentialmodels = DatabaseModelPose()
mypotentialmodels.model_id = 18744
mypotentialmodels.confidence = 1.0
mypotentialmodels.pose = copy.deepcopy(initial_pose)
graspable_object.potential_models.append(mypotentialmodels)
#fake obj world destination pose
initial_pose.pose.position.x = 0.43
initial_pose.pose.position.y = 0.0
initial_pose.pose.position.z = 1.088
initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0]
initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1]
initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2]
initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3]
list_of_poses = self.compute_list_of_poses(initial_pose,
graspable_object,
executed_grasp)
#print "list of pose",list_of_poses
#self.place_object(self.object.graspable_object, self.object.graspable_object_name, self.object_name, self.list_of_poses)
def pickup(self, graspable_object, graspable_object_name, object_name):
"""
Try to pick up the given object. Sends a message (PickupGoal from
actionlib_msgs) to the manipularior action server on
/object_manipulator/object_manipulator_pickup/goal
"""
info_tmp = "Picking up " + object_name
rospy.loginfo(info_tmp)
pickup_goal = PickupGoal()
pickup_goal.target = graspable_object
pickup_goal.collision_object_name = graspable_object_name
pickup_goal.collision_support_surface_name = self.gui.collision_support_surface_name
#pickup_goal.additional_link_padding
pickup_goal.ignore_collisions = True
pickup_goal.arm_name = "right_arm"
#pickup_goal.desired_approach_distance = 0.08 This does not exist anymore in the message
#pickup_goal.min_approach_distance = 0.02 This does not exist anymore in the message
direction = Vector3Stamped()
direction.header.stamp = rospy.get_rostime()
direction.header.frame_id = "/base_link"
direction.vector.x = 0
direction.vector.y = 0
direction.vector.z = 1
pickup_goal.lift.direction = direction
#request a vertical lift of 15cm after grasping the object
pickup_goal.lift.desired_distance = 0.1
pickup_goal.lift.min_distance = 0.07
#do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = True
pickup_goal.use_reactive_execution = True
self.pickup_result = self.pickupservice.pick(pickup_goal)
#pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#pickup_client.wait_for_server()
#rospy.loginfo("Pickup server ready")
#pickup_client.send_goal(pickup_goal)
#TODO: change this when using the robot
#pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0))
loginfo("Got Pickup results")
#self.pickup_result = pickup_client.get_result()
#print "Pickup result: "+str(self.pickup_result)
'''
if pickup_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("The pickup action has failed: " + str(self.pickup_result.manipulation_result.value) )
QMessageBox.warning(self, "Warning",
"Pickup action failed: "+str(self.pickup_result.manipulation_result.value))
for tested_grasp,tested_grasp_result in zip(self.pickup_result.attempted_grasps,self.pickup_result.attempted_grasp_results):
if tested_grasp_result.result_code==7:
self.grasp_display_publisher.publish(tested_grasp)
return -1
'''
if self.pickup_result.manipulation_result.value == ManipulationResult.SUCCESS:
loginfo("Pick succeeded, now lifting")
self.pickupservice.lift(0.07)
else:
loginfo("Pick failed")
return 1
return 0
def place_object(self, graspable_object, graspable_object_name,
object_name, list_of_poses):
"""
Place the given object in the given pose
"""
if self.pickup_result == None:
rospy.logwarn(
"No objects where picked up. Aborting place object action.")
return
info_tmp = "Placing " + object_name
rospy.loginfo(info_tmp)
place_goal = PlaceGoal()
#place at the prepared location
place_goal.place_locations = list_of_poses
place_goal.collision_object_name = graspable_object_name
place_goal.collision_support_surface_name = self.gui.collision_support_surface_name
print "collision support surface name: ", self.gui.collision_support_surface_name
#information about which grasp was executed on the object,
#returned by the pickup action
place_goal.grasp = self.pickup_result.grasp
#use the right rm to place
place_goal.arm_name = "right_arm"
#padding used when determining if the requested place location
#would bring the object in collision with the environment
place_goal.place_padding = 0.01
#how much the gripper should retreat after placing the object
place_goal.desired_retreat_distance = 0.1
place_goal.min_retreat_distance = 0.05
#we will be putting down the object along the "vertical" direction
#which is along the z axis in the fixed frame
direction = Vector3Stamped()
direction.header.stamp = rospy.get_rostime()
direction.header.frame_id = "/world"
direction.vector.x = 0
direction.vector.y = 0
direction.vector.z = 1
place_goal.approach.direction = direction
place_goal.approach.min_distance = 0.01
place_goal.approach.desired_distance = 0.03
#request a vertical put down motion of 10cm before placing the object
place_goal.desired_retreat_distance = 0.1
place_goal.min_retreat_distance = 0.05
#we are not using tactile based placing
place_goal.use_reactive_place = False
placeresult = self.pickupservice.place(place_goal)
'''place_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_place', PlaceAction)
place_client.wait_for_server()
rospy.loginfo("Place server ready")
place_client.send_goal(place_goal)
#timeout after 1sec
#TODO: change this when using the robot
place_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0))
rospy.loginfo("Got Place results")
place_result = place_client.get_result()
if place_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("The place action has failed: " + str(place_result.manipulation_result.value) )
print place_result
'''
if placeresult.manipulation_result.value == ManipulationResult.SUCCESS:
loginfo("Place succeeded, now retreating")
self.pickupservice.retreat(0.07)
else:
loginfo("Place failed")
return 1
return 0
def compute_list_of_poses(self, destination_pose, graspable_object,
executed_grasp):
list_of_poses = []
# find hand to object transform
# executed_grasp is hand pose in world frame
# graspable_object is object in world frame
hand_world_pose = executed_grasp.grasp_pose
#print "hand_world", hand_world_pose
object_world_pose = graspable_object.potential_models[0].pose.pose
# print "object world" , object_world_pose
tmathandworld = pose_to_mat(hand_world_pose)
tmatobjworld = pose_to_mat(object_world_pose)
hand_object_pose = mat_to_pose(
np.dot(np.linalg.inv(tmatobjworld), tmathandworld))
#print "hand_object_pose" , hand_object_pose
# generate rotational_symmetric hand to object frames
hand_object_poses = self.generate_rotational_symmetric_poses(
hand_object_pose, 16)
target_pose = PoseStamped()
target_pose = copy.deepcopy(destination_pose)
target_pose.header.stamp = rospy.get_rostime()
#print "target_object_world pose" , target_pose
tmattarget_world = pose_to_mat(target_pose.pose)
for hand_object_pose in hand_object_poses:
tmathandobj = pose_to_mat(hand_object_pose)
target_hand_posestamped_world = PoseStamped()
target_hand_posestamped_world.header.frame_id = "/world"
target_hand_posestamped_world.pose = mat_to_pose(
np.dot(tmattarget_world, tmathandobj))
target_hand_posestamped_world.header.stamp = rospy.get_rostime()
list_of_poses.append(target_hand_posestamped_world)
self.draw_place_area(list_of_poses, graspable_object)
return list_of_poses
def generate_rotational_symmetric_poses(self, hand_object_pose, step=8):
'''
Generate a list of 20 pregrasps around the z axis of the object
'''
generated_poses = []
pose = hand_object_pose
#We'll generate 20 poses centered in 0,0,0 with orientations in 360 degrees around z
for i in range(0, step):
angle = (6.28318532 * i) / step
q = tf.transformations.quaternion_from_euler(0.0, 0.0, angle)
rotation_pose = Pose(Point(0, 0, 0),
Quaternion(0.0, 0.0, 0.0, 1.0))
rotation_pose.orientation = Quaternion(*q)
tmatrotz = pose_to_mat(rotation_pose)
# premultiply by the rotation matrix
tmatpose = pose_to_mat(pose)
rotated_pose = mat_to_pose(np.dot(tmatrotz, tmatpose))
generated_poses.append(rotated_pose)
return generated_poses
def draw_place_area(self, list_of_poses, graspable_object):
'''
Displays all the possible placing locations that are going to be tried.
'''
#try:
(trans_palm,
rot_palm) = self.listener.lookupTransform('/world', '/palm',
rospy.Time(0))
#except:
# return
for index, pose_stamped in enumerate(list_of_poses):
pose_tmp = Pose()
pose_tmp.position.x = pose_stamped.pose.position.x
pose_tmp.position.y = pose_stamped.pose.position.y
pose_tmp.position.z = pose_stamped.pose.position.z
pose_tmp.orientation.x = pose_stamped.pose.orientation.x
pose_tmp.orientation.y = pose_stamped.pose.orientation.y
pose_tmp.orientation.z = pose_stamped.pose.orientation.z
pose_tmp.orientation.w = pose_stamped.pose.orientation.w
mat = pose_to_mat(pose_tmp)
self.draw_functions.draw_rviz_box(mat, [.01, .01, .01],
frame='/world',
ns='place_' + str(index),
id=1000 + index,
duration=90,
color=[0.5, 0.5, 0.0],
opaque=1.0)
def call_find_cluster_bounding_box(self, cluster):
req = FindClusterBoundingBoxRequest()
req.cluster = cluster
service_name = "find_cluster_bounding_box"
rospy.loginfo("waiting for find_cluster_bounding_box service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling find_cluster_bounding_box: %s" %
e)
return 0
if not res.error_code:
return (res.pose, res.box_dims)
else:
return (None, None)
class PickServer(object):
def __init__(self):
self.draw_functions = draw_functions.DrawFunctions('grasp_markers')
self.pickupservice = Execution()
self.object = None
self.object_name = None
self.server = actionlib.SimpleActionServer('pick_object',
PickObjectAction,
self.execute, False)
self.server.start()
loginfo("Step actionlib servers: pick server ready")
def execute(self, goal):
(res, initial_pose, executed_grasp) = self.pick_object(
goal.object_to_pick, goal.collision_support_surface_name)
if self.server.is_active():
if res == 0:
self.server.publish_feedback(
PickObjectFeedback(100, "Pickup succeeded"))
self.server.set_succeeded(
PickObjectResult(res, initial_pose, executed_grasp))
else:
rospy.logwarn("Pickup failed")
self.server.publish_feedback(
PickObjectFeedback(100, "Error: Pickup failed"))
self.server.set_succeeded(
PickObjectResult(res, initial_pose, executed_grasp))
def pick_object(self, object_to_pick, collision_support_surface_name):
self.server.publish_feedback(
PickObjectFeedback(1, "Finding object bounding box"))
self.object_name = object_to_pick.model_description.name
self.object = object_to_pick
graspable_object = self.object.graspable_object
# draw a bounding box around the selected object
(box_pose, box_dims) = self.call_find_cluster_bounding_box(
graspable_object.cluster)
if box_pose == None:
return (1, None, None)
box_mat = pose_to_mat(box_pose.pose)
rospy.logerr("box_pose %f %f %f q: %f %f %f %f",
box_pose.pose.position.x, box_pose.pose.position.y,
box_pose.pose.position.z, box_pose.pose.orientation.x,
box_pose.pose.orientation.y, box_pose.pose.orientation.z,
box_pose.pose.orientation.w)
box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2],
[box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]]
self.box_pose = box_pose
self.draw_functions.draw_rviz_box(box_mat,
box_ranges,
'/world',
ns='bounding box',
color=[0, 0, 1],
opaque=0.25,
duration=60)
if self.server.is_preempt_requested():
self.server.publish_feedback(
PickObjectFeedback(50, "Pick action cancelled"))
self.server.set_preempted()
return (1, None, None)
self.server.publish_feedback(
PickObjectFeedback(10, "Calling pick up service"))
# call the pickup service
res = self.pickup(graspable_object, self.object.graspable_object_name,
self.object_name, collision_support_surface_name)
initial_pose = PoseStamped()
initial_pose.header.stamp = rospy.get_rostime()
initial_pose.header.frame_id = "/world"
initial_pose.pose.position.x = self.box_pose.pose.position.x
initial_pose.pose.position.y = self.box_pose.pose.position.y
initial_pose.pose.position.z = self.box_pose.pose.position.z - box_dims.z / 2 # graspable object is from bottom but bounding box is at center !
initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x
initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y
initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z
initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w
if res == 0: #correctly picked up
executed_grasp = self.pickup_result.grasp
else:
executed_grasp = Grasp()
return (res, initial_pose, executed_grasp)
def pickup(self, graspable_object, graspable_object_name, object_name,
collision_support_surface_name):
"""
Try to pick up the given object. Sends a message (PickupGoal from
actionlib_msgs) to the manipularior action server on
/object_manipulator/object_manipulator_pickup/goal
"""
info_tmp = "Picking up " + object_name
rospy.loginfo(info_tmp)
pickup_goal = PickupGoal()
pickup_goal.target = graspable_object
pickup_goal.collision_object_name = graspable_object_name
pickup_goal.collision_support_surface_name = collision_support_surface_name
#pickup_goal.additional_link_padding
pickup_goal.ignore_collisions = True
pickup_goal.arm_name = "right_arm"
#pickup_goal.desired_approach_distance = 0.08 This does not exist anymore in the message
#pickup_goal.min_approach_distance = 0.02 This does not exist anymore in the message
direction = Vector3Stamped()
direction.header.stamp = rospy.get_rostime()
direction.header.frame_id = "/base_link"
direction.vector.x = 0
direction.vector.y = 0
direction.vector.z = 1
pickup_goal.lift.direction = direction
#request a vertical lift of 15cm after grasping the object
pickup_goal.lift.desired_distance = 0.1
pickup_goal.lift.min_distance = 0.07
#do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = True
pickup_goal.use_reactive_execution = True
self.pickup_result = self.pickupservice.pick(pickup_goal)
#pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#pickup_client.wait_for_server()
#rospy.loginfo("Pickup server ready")
#pickup_client.send_goal(pickup_goal)
#TODO: change this when using the robot
#pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0))
loginfo("Got Pickup results")
#self.pickup_result = pickup_client.get_result()
#print "Pickup result: "+str(self.pickup_result)
'''
if pickup_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("The pickup action has failed: " + str(self.pickup_result.manipulation_result.value) )
QMessageBox.warning(self, "Warning",
"Pickup action failed: "+str(self.pickup_result.manipulation_result.value))
for tested_grasp,tested_grasp_result in zip(self.pickup_result.attempted_grasps,self.pickup_result.attempted_grasp_results):
if tested_grasp_result.result_code==7:
self.grasp_display_publisher.publish(tested_grasp)
return -1
'''
if self.pickup_result.manipulation_result.value == ManipulationResult.SUCCESS:
loginfo("Pick succeeded, now lifting")
self.pickupservice.lift(0.07)
else:
loginfo("Pick failed")
return 1
return 0
def call_find_cluster_bounding_box(self, cluster):
req = FindClusterBoundingBoxRequest()
req.cluster = cluster
service_name = "find_cluster_bounding_box"
rospy.loginfo("waiting for find_cluster_bounding_box service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox)
try:
res = serv(req)
except rospy.ServiceException, e:
rospy.logerr("error when calling find_cluster_bounding_box: %s" %
e)
return 0
if not res.error_code:
return (res.pose, res.box_dims)
else:
return (None, None)
