00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 import roslib; roslib.load_manifest('srs_symbolic_grounding')
00061
00062 from srs_symbolic_grounding.srv import *
00063
00064 from std_msgs.msg import *
00065 from geometry_msgs.msg import *
00066 from nav_msgs.msg import *
00067 from nav_msgs.srv import *
00068
00069 import rospy
00070 import math
00071 import tf
00072 from tf.transformations import euler_from_quaternion
00073
00074
00075
00076
00077 '''
00078 #get furniture list from the knowledge base (part 1)
00079 def getWorkspaceOnMap():
00080 print 'test get all workspace (furnitures basically here) from map'
00081 try:
00082 requestNewTask = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap)
00083 res = requestNewTask('ipa-kitchen-map', True)
00084 return res
00085 except rospy.ServiceException, e:
00086 print "Service call failed: %s"%e
00087 '''
00088
00089
00090 def getMapClient():
00091
00092 try:
00093 reqMap = rospy.ServiceProxy('static_map', GetMap)
00094 res = reqMap()
00095 return res
00096 except rospy.ServiceException, e:
00097 print "Service call failed: %s"%e
00098
00099
00100 def getRobotBasePoseList(angle, dist, rbp, obj_x, obj_y):
00101 grasp_base_pose_list = list()
00102 step_angle = angle
00103 dist_to_obj = dist
00104 rb_pose = rbp
00105 target_obj_x = obj_x
00106 target_obj_y = obj_y
00107 th = math.atan((rb_pose.y - target_obj_y) / (rb_pose.x - target_obj_x))
00108 if rb_pose.x < target_obj_x and rb_pose.y > target_obj_y:
00109 th = math.pi + th
00110 if rb_pose.x < target_obj_x and rb_pose.y < target_obj_y:
00111 th = -math.pi + th
00112
00113 for n in range (0, int(360.0 / step_angle)):
00114 grasp_base_pose = Pose2D()
00115 grasp_base_pose.x = target_obj_x + dist_to_obj * math.cos(th + n * step_angle / 180.0 * math.pi)
00116 grasp_base_pose.y = target_obj_y + dist_to_obj * math.sin(th + n * step_angle / 180.0 * math.pi)
00117 grasp_base_pose.theta = th + n * step_angle / 180.0 * math.pi
00118 if grasp_base_pose.theta > math.pi:
00119 grasp_base_pose.theta -= 2 * math.pi
00120 grasp_base_pose_list.append(grasp_base_pose)
00121
00122 return grasp_base_pose_list
00123
00124
00125
00126
00127 def obstacleCheck(gbpl, po_x, po_y, po_th, po_w, po_l, fgl, to_h):
00128 parent_obj_checked_grasp_base_pose_list = list()
00129 obstacle_checked_grasp_base_pose_list = list()
00130 wall_checked_grasp_base_pose_list = list()
00131 grasp_base_pose_list = gbpl
00132 parent_obj_x = po_x
00133 parent_obj_y = po_y
00134 parent_obj_th = po_th
00135 parent_obj_w = po_w
00136 parent_obj_l = po_l
00137 furniture_geometry_list = fgl
00138 target_obj_h = to_h
00139
00140
00141 dist_to_table = 0.6
00142 print target_obj_h
00143 if target_obj_h > 1.2 or target_obj_h < 0.85:
00144 dist_to_table -= 0.05
00145 index_1 = 0
00146 while index_1 < len(grasp_base_pose_list):
00147 th = math.atan((grasp_base_pose_list[index_1].y - parent_obj_y) / (grasp_base_pose_list[index_1].x - parent_obj_x))
00148 if grasp_base_pose_list[index_1].x < parent_obj_x and grasp_base_pose_list[index_1].y > parent_obj_y:
00149 th = math.pi + th
00150 if grasp_base_pose_list[index_1].x < parent_obj_x and grasp_base_pose_list[index_1].y < parent_obj_y:
00151 th = -math.pi + th
00152 delta_x = math.sqrt((grasp_base_pose_list[index_1].x - parent_obj_x) ** 2 + (grasp_base_pose_list[index_1].y - parent_obj_y) ** 2) * math.cos(th - parent_obj_th)
00153 delta_y = math.sqrt((grasp_base_pose_list[index_1].x - parent_obj_x) ** 2 + (grasp_base_pose_list[index_1].y - parent_obj_y) ** 2) * math.sin(th - parent_obj_th)
00154
00155 if (delta_x <= -(parent_obj_w / 2.0 + dist_to_table) or delta_x >= (parent_obj_w / 2.0 + dist_to_table)) or (delta_y <= -(parent_obj_l / 2.0 + dist_to_table) or delta_y >= (parent_obj_l / 2.0 + dist_to_table)):
00156 parent_obj_checked_grasp_base_pose_list.append(grasp_base_pose_list[index_1])
00157 index_1 += 1
00158
00159
00160
00161 if parent_obj_checked_grasp_base_pose_list:
00162
00163
00164 dist_to_obstacles = 0.6
00165 if target_obj_h > 1.2 or target_obj_h < 0.85:
00166 dist_to_obstacles -= 0.05
00167 index_2 = 0
00168 while index_2 < len(parent_obj_checked_grasp_base_pose_list):
00169 index_3 = 0
00170 while index_3 < len(furniture_geometry_list):
00171 th = math.atan((parent_obj_checked_grasp_base_pose_list[index_2].y - furniture_geometry_list[index_3].pose.y) / (parent_obj_checked_grasp_base_pose_list[index_2].x - furniture_geometry_list[index_3].pose.x))
00172 if parent_obj_checked_grasp_base_pose_list[index_2].x < furniture_geometry_list[index_3].pose.x and parent_obj_checked_grasp_base_pose_list[index_2].y > furniture_geometry_list[index_3].pose.y:
00173 th = math.pi + th
00174 if parent_obj_checked_grasp_base_pose_list[index_2].x < furniture_geometry_list[index_3].pose.x and parent_obj_checked_grasp_base_pose_list[index_2].y < furniture_geometry_list[index_3].pose.y:
00175 th = -math.pi + th
00176 delta_x = math.sqrt((parent_obj_checked_grasp_base_pose_list[index_2].x - furniture_geometry_list[index_3].pose.x) ** 2 + (parent_obj_checked_grasp_base_pose_list[index_2].y - furniture_geometry_list[index_3].pose.y) ** 2) * math.cos(th - furniture_geometry_list[index_3].pose.theta)
00177 delta_y = math.sqrt((parent_obj_checked_grasp_base_pose_list[index_2].x - furniture_geometry_list[index_3].pose.x) ** 2 + (parent_obj_checked_grasp_base_pose_list[index_2].y - furniture_geometry_list[index_3].pose.y) ** 2) * math.sin(th - furniture_geometry_list[index_3].pose.theta)
00178 if (delta_x <= -(furniture_geometry_list[index_3].w / 2.0 + dist_to_obstacles) or delta_x >= (furniture_geometry_list[index_3].w / 2.0 + dist_to_obstacles)) or (delta_y <= -(furniture_geometry_list[index_3].l / 2.0 + dist_to_obstacles) or delta_y >= (furniture_geometry_list[index_3].l / 2.0 + dist_to_obstacles)):
00179 index_3 += 1
00180 else:
00181 break
00182 if index_3 == len(furniture_geometry_list):
00183 obstacle_checked_grasp_base_pose_list.append(parent_obj_checked_grasp_base_pose_list[index_2])
00184 index_2 += 1
00185
00186
00187 if obstacle_checked_grasp_base_pose_list:
00188
00189
00190 data = getMapClient()
00191
00192 dist_to_walls = 0.5
00193 threshold = 20
00194 step_angle = 5.0
00195
00196 index_4 = 0
00197 while index_4 < len(obstacle_checked_grasp_base_pose_list):
00198 map_index_list = list()
00199 n = 0
00200 while n < int(360.0 / step_angle):
00201 wall_check_point_x = obstacle_checked_grasp_base_pose_list[index_4].x + dist_to_walls * math.cos(n * step_angle / 180.0 * math.pi)
00202 wall_check_point_y = obstacle_checked_grasp_base_pose_list[index_4].y + dist_to_walls * math.sin(n * step_angle / 180.0 * math.pi)
00203 map_index = int((wall_check_point_y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((wall_check_point_x - data.map.info.origin.position.x) / data.map.info.resolution)
00204 map_index_list.append(map_index)
00205 n += 1
00206
00207 map_index = int((obstacle_checked_grasp_base_pose_list[index_4].y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((obstacle_checked_grasp_base_pose_list[index_4].x - data.map.info.origin.position.x) / data.map.info.resolution)
00208 map_index_list.append(map_index - 1)
00209
00210
00211 index_5 = 0
00212 while index_5 < len(map_index_list):
00213 if -1 < data.map.data[map_index_list[index_5]] < threshold:
00214 index_5 += 1
00215 else:
00216 break
00217 if index_5 == len(map_index_list):
00218 wall_checked_grasp_base_pose_list.append(obstacle_checked_grasp_base_pose_list[index_4])
00219 index_4 += 1
00220
00221 return wall_checked_grasp_base_pose_list
00222
00223
00224
00225 def handle_symbol_grounding_grasp_base_pose_experimental(req):
00226
00227 '''
00228 #get the robot's current pose from tf
00229 rb_pose = Pose2D()
00230 listener = tf.TransformListener()
00231 listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0)) #wait for 4secs for the coordinate to be transformed
00232 (trans,rot) = listener.lookupTransform("/map", "/base_link", rospy.Time(0))
00233 rb_pose.x = trans[0]
00234 rb_pose.y = trans[1]
00235 rb_pose_rpy = tf.transformations.euler_from_quaternion(rot)
00236 rb_pose.theta = rb_pose_rpy[2]
00237 rospy.sleep(0.5)
00238 #rospy.loginfo(rb_pose)
00239 '''
00240
00241
00242 rb_pose = Pose2D()
00243 rb_pose.x = -1.06
00244 rb_pose.y = 1.08
00245 rb_pose.theta = 0.0
00246
00247
00248 mf1_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0]
00249 mf1_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.875, 0.75, 0.625, 0.5, 0.375, 0.25, 0.125, 0]
00250 mf1_th = [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0]
00251
00252 '''
00253 mf2_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0]
00254 mf2_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.84, 0.67, 0.49, 0.33, 0]
00255 mf2_th = [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0]
00256
00257 mf3_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0]
00258 mf3_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.84, 0.67, 0.49, 0.33, 0]
00259 mf3_th = [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0]
00260 '''
00261
00262
00263
00264 target_obj_x = req.target_obj_pose.position.x
00265 target_obj_y = req.target_obj_pose.position.y
00266 target_obj_h = req.target_obj_pose.position.z
00267 target_obj_rpy = tf.transformations.euler_from_quaternion([req.target_obj_pose.orientation.x, req.target_obj_pose.orientation.y, req.target_obj_pose.orientation.z, req.target_obj_pose.orientation.w])
00268 target_obj_th = target_obj_rpy[2]
00269
00270
00271
00272 robot_base_pose_x = rb_pose.x
00273 robot_base_pose_y = rb_pose.y
00274 robot_base_pose_th = rb_pose.theta
00275
00276 parent_obj_x = req.parent_obj_geometry.pose.position.x
00277 parent_obj_y = req.parent_obj_geometry.pose.position.y
00278 parent_obj_rpy = tf.transformations.euler_from_quaternion([req.parent_obj_geometry.pose.orientation.x, req.parent_obj_geometry.pose.orientation.y, req.parent_obj_geometry.pose.orientation.z, req.parent_obj_geometry.pose.orientation.w])
00279 parent_obj_th = parent_obj_rpy[2]
00280 parent_obj_l = req.parent_obj_geometry.l
00281 parent_obj_w = req.parent_obj_geometry.w
00282 parent_obj_h = req.parent_obj_geometry.h
00283
00284
00285 '''
00286 #get furniture list from the knowledge base (part 2)
00287
00288 workspace_info = getWorkspaceOnMap()
00289 furniture_geometry_list = list()
00290 furniture_geometry_list = workspace_info.objectsInfo
00291
00292
00293 #transfrom list
00294 index = 0
00295 furniture_geometry_list = list()
00296 while index < len(furniture_geometry_list):
00297 furniture_geometry = FurnitureGeometry()
00298 furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x
00299 furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y
00300 furniture_pose_rpy = tf.transformations.euler_from_quaternion([furniture_geometry_list[index].pose.orientation.x, furniture_geometry_list[index].pose.orientation.y, furniture_geometry_list[index].pose.orientation.z, furniture_geometry_list[index].pose.orientation.w])
00301 furniture_geometry.pose.theta = furniture_pose_rpy[2]
00302 furniture_geometry.l = furniture_geometry_list[index].l
00303 furniture_geometry.w = furniture_geometry_list[index].w
00304 furniture_geometry.h = furniture_geometry_list[index].h
00305 furniture_geometry_list.append(furniture_geometry)
00306 index += 1
00307 '''
00308
00309 index = 0
00310 furniture_geometry_list = list()
00311 while index < len(req.furniture_geometry_list):
00312 furniture_geometry = FurnitureGeometry()
00313 furniture_geometry.pose.x = req.furniture_geometry_list[index].pose.position.x
00314 furniture_geometry.pose.y = req.furniture_geometry_list[index].pose.position.y
00315 furniture_pose_rpy = tf.transformations.euler_from_quaternion([req.furniture_geometry_list[index].pose.orientation.x, req.furniture_geometry_list[index].pose.orientation.y, req.furniture_geometry_list[index].pose.orientation.z, req.furniture_geometry_list[index].pose.orientation.w])
00316 furniture_geometry.pose.theta = furniture_pose_rpy[2]
00317 furniture_geometry.l = req.furniture_geometry_list[index].l
00318 furniture_geometry.w = req.furniture_geometry_list[index].w
00319 furniture_geometry.h = req.furniture_geometry_list[index].h
00320 furniture_geometry_list.append(furniture_geometry)
00321 index += 1
00322
00323
00324
00325
00326
00327
00328
00329 best_gripper_pose_x = robot_base_pose_x - 0.75 * math.cos(robot_base_pose_th) + 0.1 * math.sin(robot_base_pose_th)
00330 best_gripper_pose_y = robot_base_pose_y - 0.75 * math.sin(robot_base_pose_th) - 0.1 * math.cos(robot_base_pose_th)
00331
00332
00333 th = math.atan(target_obj_y - best_gripper_pose_y / target_obj_x - best_gripper_pose_x)
00334 if target_obj_x < best_gripper_pose_x and target_obj_y > best_gripper_pose_y:
00335 th += math.pi
00336 elif target_obj_x < best_gripper_pose_x and target_obj_y < best_gripper_pose_y:
00337 th -= math.pi
00338 delta_x = math.sqrt((target_obj_x - best_gripper_pose_x) ** 2 + (target_obj_y - best_gripper_pose_y) ** 2) * math.cos(th - robot_base_pose_th)
00339 delta_y = math.sqrt((target_obj_x - best_gripper_pose_x) ** 2 + (target_obj_y - best_gripper_pose_y) ** 2) * math.sin(th - robot_base_pose_th)
00340 th = math.atan((rb_pose.y - target_obj_y) / (rb_pose.x - target_obj_x))
00341 if rb_pose.x < target_obj_x and rb_pose.y > target_obj_y:
00342 th += math.pi
00343 elif rb_pose.x < target_obj_x and rb_pose.y < target_obj_y:
00344 th -= math.pi
00345 delta_th = robot_base_pose_th - th
00346 if delta_th < 0:
00347 delta_th = -1.0 * delta_th
00348
00349
00350 if delta_x > 0.1 or delta_x < -0.15:
00351 reach = 0
00352 elif delta_y > 0.15 or delta_y < -0.1:
00353 reach = 0
00354 elif delta_th < -10.0 / 180.0 * math.pi or delta_th > 10.0 / 180 * math.pi:
00355 reach = 0
00356 else:
00357
00358 index_x = int(round(delta_x / 0.025 + 6))
00359 index_y = int(round(delta_y / 0.025 + 6))
00360 index_th = int(round(delta_th / (1.0 / 180.0 * math.pi) + 10))
00361 member_x = mf1_x[index_x]
00362 member_y = mf1_y[index_y]
00363 member_th = mf1_th[index_th]
00364
00365 reach = min(member_x, member_y, member_th)
00366
00367
00368
00369
00370
00371 step_angle_1 = 1.0
00372 dist_1 = 0.75
00373 step_angle_2 = 1.0
00374 dist_2 = 0.8
00375
00376 if target_obj_h > 1.2 or target_obj_h < 0.85:
00377 dist_1 -= 0.05
00378 dist_2 -= 0.05
00379 grasp_base_pose_list_1 = list()
00380 grasp_base_pose_list_1 = getRobotBasePoseList(step_angle_1, dist_1, rb_pose, target_obj_x, target_obj_y)
00381
00382
00383
00384 grasp_base_pose_list_2 = list()
00385 grasp_base_pose_list_2 = getRobotBasePoseList(step_angle_2, dist_2, rb_pose, target_obj_x, target_obj_y)
00386
00387
00388
00389
00390 obstacle_check = 1
00391
00392 grasp_base_pose_list = obstacleCheck(grasp_base_pose_list_1, parent_obj_x, parent_obj_y, parent_obj_th, parent_obj_w, parent_obj_l, furniture_geometry_list, target_obj_h)
00393
00394 grasp_base_pose = Pose2D()
00395
00396 if grasp_base_pose_list:
00397 obstacle_check = 0
00398 grasp_base_pose = grasp_base_pose_list[0]
00399
00400 else:
00401
00402 grasp_base_pose_list = obstacleCheck(grasp_base_pose_list_2, parent_obj_x, parent_obj_y, parent_obj_th, parent_obj_w, parent_obj_l, furniture_geometry_list, target_obj_h)
00403
00404
00405
00406 if grasp_base_pose_list:
00407
00408 obstacle_check = 0
00409 grasp_base_pose = grasp_base_pose_list[0]
00410
00411
00412
00413
00414
00415
00416
00417
00418 return obstacle_check, reach, grasp_base_pose
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 def symbol_grounding_grasp_base_pose_experimental_server():
00429 rospy.init_node('symbol_grounding_grasp_base_pose_experimental_server')
00430 s = rospy.Service('symbol_grounding_grasp_base_pose_experimental', SymbolGroundingGraspBasePoseExperimental, handle_symbol_grounding_grasp_base_pose_experimental)
00431 print "Ready to receive requests."
00432 rospy.spin()
00433
00434
00435
00436
00437 if __name__ == "__main__":
00438 symbol_grounding_grasp_base_pose_experimental_server()