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 __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 __init__(self):
self.draw_functions = draw_functions.DrawFunctions('grasp_markers')
#We're creating another Execution object here (there's another one in pick_server.py)
#We need to make sure that they don't interfere with each other
#(Execution doesn't provide any service, so it shouldn't be a problem)
self.pickupservice = Execution()
self.listener = self.pickupservice.listener
self.server = actionlib.SimpleActionServer('place_object',
PlaceObjectAction,
self.execute, False)
self.server.start()
rospy.loginfo("Step actionlib servers: place server ready")
def __init__(self,):
"""
"""
self.markers_pub_ = rospy.Publisher("~markers", Marker)
self.execution = Execution(use_database=False)
# direct control of the elbow swing, WRJ1 and THJ5 for throwing the lid open
self.publishers = {}
self.publishers["ElbowJSwing"] = rospy.Publisher("/sa_es_position_controller/command", Float64, latch=True)
self.publishers["WRJ1"] = rospy.Publisher(
"/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
)
self.publishers["THJ5"] = rospy.Publisher(
"/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
)
self.suitcase_src_ = rospy.Service("~open_suitcase", OpenSuitcase, self.open_lid)
class PlaceServer(object):
def __init__(self):
self.draw_functions = draw_functions.DrawFunctions('grasp_markers')
#We're creating another Execution object here (there's another one in pick_server.py)
#We need to make sure that they don't interfere with each other
#(Execution doesn't provide any service, so it shouldn't be a problem)
self.pickupservice = Execution()
self.listener = self.pickupservice.listener
self.server = actionlib.SimpleActionServer('place_object',
PlaceObjectAction,
self.execute, False)
self.server.start()
rospy.loginfo("Step actionlib servers: place server ready")
def execute(self, goal):
res = self.place(goal.place_pose, goal.object_to_place,
goal.executed_grasp,
goal.collision_support_surface_name)
if self.server.is_active():
if res == 0:
self.server.publish_feedback(
PlaceObjectFeedback(100, "Place object succeeded"))
self.server.set_succeeded(PlaceObjectResult(res))
else:
rospy.logwarn("Place failed")
self.server.publish_feedback(
PlaceObjectFeedback(100, "Error: Place object failed"))
self.server.set_succeeded(PlaceObjectResult(res))
def place(self, place_pose, object_to_place, executed_grasp,
collision_support_surface_name):
self.server.publish_feedback(
PlaceObjectFeedback(1, "Computing list of poses"))
list_of_poses = self.compute_list_of_poses(
place_pose, object_to_place.graspable_object, executed_grasp)
if self.server.is_preempt_requested():
self.server.publish_feedback(
PlaceObjectFeedback(50, "Place action cancelled"))
self.server.set_preempted()
return 1
self.server.publish_feedback(PlaceObjectFeedback(25, "Placing object"))
res = self.place_object(object_to_place.graspable_object,
object_to_place.graspable_object_name,
object_to_place.model_description.name,
list_of_poses, executed_grasp,
collision_support_surface_name)
return res
def place_object(self, graspable_object, graspable_object_name,
object_name, list_of_poses, executed_grasp,
collision_support_surface_name):
"""
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 = collision_support_surface_name
print "collision support surface name: ", collision_support_surface_name
#information about which grasp was executed on the object,
#returned by the pickup action
place_goal.grasp = executed_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:
rospy.loginfo("Place succeeded, now retreating")
self.pickupservice.retreat(0.07)
else:
rospy.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())
#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)
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 SrOpenSuitcase(object):
"""
"""
# The xyz distance we want the palm link to be from the grasping point
DISTANCE_TO_GRASP = [-0.1, -0.01, 0.04]
# The distance used when computing the approach
APPROACH_DISTANCE = 0.03
def __init__(self,):
"""
"""
self.markers_pub_ = rospy.Publisher("~markers", Marker)
self.execution = Execution(use_database=False)
# direct control of the elbow swing, WRJ1 and THJ5 for throwing the lid open
self.publishers = {}
self.publishers["ElbowJSwing"] = rospy.Publisher("/sa_es_position_controller/command", Float64, latch=True)
self.publishers["WRJ1"] = rospy.Publisher(
"/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
)
self.publishers["THJ5"] = rospy.Publisher(
"/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
)
self.suitcase_src_ = rospy.Service("~open_suitcase", OpenSuitcase, self.open_lid)
def open_lid(self, suitcase_req):
suitcase = suitcase_req.suitcase
self.display_suitcase_(suitcase)
semi_circle = self.go_to_mechanism_and_grasp_(suitcase)
if semi_circle == None:
rospy.logerr("Failed to approach.")
sys.exit(1)
time.sleep(1.0)
self.lift_lid_(suitcase, semi_circle)
# throw the lid open
self.throw_lid_open()
def go_to_mechanism_and_grasp_(self, suitcase):
# compute the full trajectory
semi_circle = self.compute_semi_circle_traj_(suitcase)
# compute the pregrasp and grasp
grasp = Grasp()
grasp_pose_ = PoseStamped()
# the grasp is the first item of the semi circle
grasp_pose_ = semi_circle[0]
# 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
# TODO: use the suitcase axis to approach from the perpendicular
pre_grasp_pose_.pose.position.x = pre_grasp_pose_.pose.position.x - self.APPROACH_DISTANCE
# TODO: find better postures
grasp.pre_grasp_posture.name = [
"FFJ0",
"FFJ3",
"FFJ4",
"LFJ0",
"LFJ3",
"LFJ4",
"LFJ5",
"MFJ0",
"MFJ3",
"MFJ4",
"RFJ0",
"RFJ3",
"RFJ4",
"THJ1",
"THJ2",
"THJ3",
"THJ4",
"THJ5",
"WRJ1",
"WRJ2",
]
grasp.pre_grasp_posture.position = [0.0] * 18
grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ4")] = 58.0
grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ5")] = -50.0
grasp.grasp_posture.name = [
"FFJ0",
"FFJ3",
"FFJ4",
"LFJ0",
"LFJ3",
"LFJ4",
"LFJ5",
"MFJ0",
"MFJ3",
"MFJ4",
"RFJ0",
"RFJ3",
"RFJ4",
"THJ1",
"THJ2",
"THJ3",
"THJ4",
"THJ5",
"WRJ1",
"WRJ2",
]
grasp.grasp_posture.position = [0.0] * 18
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ1")] = 57.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ2")] = 30.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ3")] = -15.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ4")] = 58.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ5")] = 40.0
grasp.grasp_pose = grasp_pose_.pose
# 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
motion_plan_res = GetMotionPlanResponse()
interpolated_motion_plan_res = self.execution.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 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.execution.plan.plan_arm_motion("right_arm", "jointspace", pre_grasp_pose_)
# execution part
if motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS:
# put hand in pre-grasp posture
if self.execution.pre_grasp_exec(grasp) < 0:
rospy.logerr("Failed to go in pregrasp.")
sys.exit()
# go there
# filter the trajectory
filtered_traj = self.execution.filter_traj_(motion_plan_res)
self.execution.display_traj_(filtered_traj)
# reach pregrasp pose
if self.execution.send_traj_(filtered_traj) < 0:
time.sleep(20) # TODO use actionlib here
# approach
if self.execution.send_traj_(interpolated_motion_plan_res.trajectory.joint_trajectory) < 0:
rospy.logerr("Failed to approach.")
sys.exit()
time.sleep(20) # TODO use actionlib here
# grasp
if self.execution.grasp_exec(grasp) < 0:
rospy.logerr("Failed to grasp.")
sys.exit()
time.sleep(20) # TODO use actionlib here
else:
# Failed, don't return the computed traj
return None
# return the full traj
return semi_circle
def lift_lid_(self, suitcase, semi_circle):
while len(semi_circle) > 0:
self.execution.plan_and_execute_step_(semi_circle)
time.sleep(0.5)
return OpenSuitcaseResponse(OpenSuitcaseResponse.SUCCESS)
def throw_lid_open(self):
# get the current joint states
res = self.execution.plan.get_joint_state_.call()
current_joint_states = res.joint_state
# flex the elbow, flip the wrist
elbow_target = current_joint_states.position[current_joint_states.name.index("ElbowJSwing")] + 0.5
wrist_target = current_joint_states.position[current_joint_states.name.index("WRJ1")] - 0.5
thj5_target = -0.80
self.publishers["ElbowJSwing"].publish(elbow_target)
self.publishers["WRJ1"].publish(wrist_target)
self.publishers["THJ5"].publish(thj5_target)
def compute_semi_circle_traj_(self, suitcase, nb_steps=50):
poses = []
# compute a semi-circular trajectory, starting
# from the suitcase mechanism, rotating
# around the suitcase axis
target = PoseStamped()
target.header.frame_id = suitcase.header.frame_id
target.pose = suitcase.opening_mechanism.pose_stamped.pose
# axis_x and axis_z are the projection of the mechanism model
# onto the suitcase axis (point around which we want to rotate)
mechanism = [
suitcase.opening_mechanism.pose_stamped.pose.position.x,
suitcase.opening_mechanism.pose_stamped.pose.position.y,
suitcase.opening_mechanism.pose_stamped.pose.position.z,
]
axis = [
suitcase.lid_axis_a.x + (suitcase.lid_axis_b.x - suitcase.lid_axis_a.x) / 2.0,
suitcase.lid_axis_a.y + (suitcase.lid_axis_b.y - suitcase.lid_axis_a.y) / 2.0,
suitcase.lid_axis_a.z + (suitcase.lid_axis_b.z - suitcase.lid_axis_a.z) / 2.0,
]
# We're always starting with the palm straight along the x axis
# TODO: use real suitcase axis instead?
suitcase_axis = (1, 0, 0)
for i in range(0, nb_steps + 1):
# we're rotating from this angle around the suitcase axis to point towards the suitcase
rotation_angle = float(i) * math.pi / 2.0 / float(nb_steps)
####
# POSITION
target.pose.position.x = axis[0] - (
(axis[0] - mechanism[0] - self.DISTANCE_TO_GRASP[0]) * math.cos(rotation_angle)
)
target.pose.position.y = suitcase.opening_mechanism.pose_stamped.pose.position.y + self.DISTANCE_TO_GRASP[1]
target.pose.position.z = (
axis[2]
+ ((axis[0] - mechanism[0] - self.DISTANCE_TO_GRASP[0]) * math.sin(rotation_angle))
+ self.DISTANCE_TO_GRASP[2]
)
####
# ORIENTATION
# limit the wrj1 axis
rotation_angle = min(rotation_angle, math.radians(15.0))
# add 90 degrees to point the axis toward the suitcase
rotation_angle += math.pi / 2.0
# orientation, palm z axis pointing towards the suitcase axes
# z toward suitcase
toward_z = transformations.quaternion_about_axis(math.pi / 2.0, (0, 0, 1))
# then rotate as we go up to continue pointing at the axis
step_rotation = transformations.quaternion_about_axis(rotation_angle, suitcase_axis)
# combine those transform:
orientation = transformations.quaternion_multiply(toward_z, step_rotation)
target.pose.orientation.x = orientation[0]
target.pose.orientation.y = orientation[1]
target.pose.orientation.z = orientation[2]
target.pose.orientation.w = orientation[3]
poses.append(copy.deepcopy(target))
return poses
def display_suitcase_(self, suitcase):
# display axes
axes_marker = Marker()
axes_marker.header.frame_id = suitcase.header.frame_id
axes_marker.ns = "suitcase"
axes_marker.id = 0
axes_marker.type = Marker.LINE_STRIP
axes_marker.action = Marker.ADD
axes_marker.points.append(suitcase.lid_axis_a)
axes_marker.points.append(suitcase.lid_axis_b)
axes_marker.scale.x = 0.02
axes_marker.color.a = 0.4
axes_marker.color.r = 0.1
axes_marker.color.g = 0.47
axes_marker.color.b = 0.88
self.markers_pub_.publish(axes_marker)
# display mechanism
mechanism_marker = Marker()
mechanism_marker.header.frame_id = suitcase.header.frame_id
mechanism_marker.ns = "suitcase"
mechanism_marker.id = 1
mechanism_marker.type = Marker.CUBE
mechanism_marker.action = Marker.ADD
mechanism_marker.pose = suitcase.opening_mechanism.pose_stamped.pose
mechanism_marker.scale = suitcase.opening_mechanism.dimensions
mechanism_marker.color.a = 0.4
mechanism_marker.color.r = 0.1
mechanism_marker.color.g = 0.47
mechanism_marker.color.b = 0.88
self.markers_pub_.publish(mechanism_marker)
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)
