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(2 * math.pi / step_angle)):
00114 grasp_base_pose = Pose2D()
00115 grasp_base_pose.x = target_obj_x + dist_to_obj * math.cos(th) - 0.1 * math.sin(th) - 2 * math.sqrt(dist_to_obj ** 2 + 0.1 ** 2) * math.sin(0.5 * n * step_angle) * math.sin(0.5 * n * step_angle + math.atan(0.1 / dist_to_obj) + th)
00116 grasp_base_pose.y = target_obj_y + dist_to_obj * math.sin(th) + 0.1 * math.cos(th) + 2 * math.sqrt(dist_to_obj ** 2 + 0.1 ** 2) * math.sin(0.5 * n * step_angle) * math.cos(0.5 * n * step_angle + math.atan(0.1 / dist_to_obj) + th)
00117 grasp_base_pose.theta = th + n * step_angle
00118
00119 if grasp_base_pose.theta > math.pi:
00120 grasp_base_pose.theta -= 2 * math.pi
00121 grasp_base_pose_list.append(grasp_base_pose)
00122
00123 return grasp_base_pose_list
00124
00125
00126
00127
00128 def obstacleCheck(gbpl, po_x, po_y, po_th, po_w, po_l, fgl):
00129 parent_obj_checked_grasp_base_pose_list = list()
00130 obstacle_checked_grasp_base_pose_list = list()
00131 wall_checked_grasp_base_pose_list = list()
00132 grasp_base_pose_list = gbpl
00133 parent_obj_x = po_x
00134 parent_obj_y = po_y
00135 parent_obj_th = po_th
00136 parent_obj_w = po_w
00137 parent_obj_l = po_l
00138 furniture_geometry_list = fgl
00139
00140
00141 dist_to_table = 0.45
00142 index_1 = 0
00143 while index_1 < len(grasp_base_pose_list):
00144 th = math.atan((grasp_base_pose_list[index_1].y - parent_obj_y) / (grasp_base_pose_list[index_1].x - parent_obj_x))
00145 if grasp_base_pose_list[index_1].x < parent_obj_x and grasp_base_pose_list[index_1].y > parent_obj_y:
00146 th = math.pi + th
00147 if grasp_base_pose_list[index_1].x < parent_obj_x and grasp_base_pose_list[index_1].y < parent_obj_y:
00148 th = -math.pi + th
00149 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)
00150 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)
00151
00152 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)):
00153 parent_obj_checked_grasp_base_pose_list.append(grasp_base_pose_list[index_1])
00154 index_1 += 1
00155
00156
00157
00158 if parent_obj_checked_grasp_base_pose_list:
00159
00160
00161 dist_to_obstacles = 0.45
00162 index_2 = 0
00163 while index_2 < len(parent_obj_checked_grasp_base_pose_list):
00164 index_3 = 0
00165 while index_3 < len(furniture_geometry_list):
00166 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))
00167 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:
00168 th = math.pi + th
00169 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:
00170 th = -math.pi + th
00171 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)
00172 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)
00173 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)):
00174 index_3 += 1
00175 else:
00176 break
00177 if index_3 == len(furniture_geometry_list):
00178 obstacle_checked_grasp_base_pose_list.append(parent_obj_checked_grasp_base_pose_list[index_2])
00179 index_2 += 1
00180
00181
00182 if obstacle_checked_grasp_base_pose_list:
00183
00184
00185 data = getMapClient()
00186
00187 dist_to_walls = 0.5
00188 threshold = 20
00189 step_angle = 5.0
00190
00191 index_4 = 0
00192 while index_4 < len(obstacle_checked_grasp_base_pose_list):
00193 map_index_list = list()
00194 n = 0
00195 while n < int(360.0 / step_angle):
00196 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)
00197 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)
00198 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)
00199 map_index_list.append(map_index)
00200 n += 1
00201
00202 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)
00203 map_index_list.append(map_index)
00204
00205
00206 index_5 = 0
00207 while index_5 < len(map_index_list):
00208 if -1 < data.map.data[map_index_list[index_5]] < threshold:
00209 index_5 += 1
00210 else:
00211 break
00212 if index_5 == len(map_index_list):
00213 wall_checked_grasp_base_pose_list.append(obstacle_checked_grasp_base_pose_list[index_4])
00214 index_4 += 1
00215
00216 return wall_checked_grasp_base_pose_list
00217
00218
00219
00220 def handle_symbol_grounding_grasp_base_region(req):
00221
00222 '''
00223 #get the robot's current pose from tf
00224 rb_pose = Pose2D()
00225 listener = tf.TransformListener()
00226 listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0)) #wait for 4secs for the coordinate to be transformed
00227 (trans,rot) = listener.lookupTransform("/map", "/base_link", rospy.Time(0))
00228 rb_pose.x = trans[0]
00229 rb_pose.y = trans[1]
00230 rb_pose_rpy = tf.transformations.euler_from_quaternion(rot)
00231 rb_pose.theta = rb_pose_rpy[2]
00232 rospy.sleep(0.5)
00233 #rospy.loginfo(rb_pose)
00234 '''
00235 rb_pose = Pose2D()
00236 rb_pose.x = -1.06
00237 rb_pose.y = -1.08
00238 rb_pose.theta = 0.0
00239
00240
00241 mf1_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0]
00242 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]
00243 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]
00244
00245 '''
00246 mf2_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0]
00247 mf2_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.84, 0.67, 0.49, 0.33, 0]
00248 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]
00249
00250 mf3_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0]
00251 mf3_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.84, 0.67, 0.49, 0.33, 0]
00252 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]
00253 '''
00254
00255
00256
00257 target_obj_x = req.target_obj_pose.position.x
00258 target_obj_y = req.target_obj_pose.position.y
00259 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])
00260 target_obj_th = target_obj_rpy[2]
00261
00262
00263 robot_base_pose_x = rb_pose.x
00264 robot_base_pose_y = rb_pose.y
00265 robot_base_pose_th = rb_pose.theta
00266
00267 parent_obj_x = req.parent_obj_geometry.pose.position.x
00268 parent_obj_y = req.parent_obj_geometry.pose.position.y
00269 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])
00270 parent_obj_th = parent_obj_rpy[2]
00271 parent_obj_l = req.parent_obj_geometry.l
00272 parent_obj_w = req.parent_obj_geometry.w
00273 parent_obj_h = req.parent_obj_geometry.h
00274
00275
00276 '''
00277 #get furniture list from the knowledge base (part 2)
00278
00279 workspace_info = getWorkspaceOnMap()
00280 furniture_geometry_list = list()
00281 furniture_geometry_list = workspace_info.objectsInfo
00282
00283
00284 #transfrom list
00285 index = 0
00286 furniture_geometry_list = list()
00287 while index < len(furniture_geometry_list):
00288 furniture_geometry = FurnitureGeometry()
00289 furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x
00290 furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y
00291 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])
00292 furniture_geometry.pose.theta = furniture_pose_rpy[2]
00293 furniture_geometry.l = furniture_geometry_list[index].l
00294 furniture_geometry.w = furniture_geometry_list[index].w
00295 furniture_geometry.h = furniture_geometry_list[index].h
00296 furniture_geometry_list.append(furniture_geometry)
00297 index += 1
00298 '''
00299
00300 index = 0
00301 furniture_geometry_list = list()
00302 while index < len(req.furniture_geometry_list):
00303 furniture_geometry = FurnitureGeometry()
00304 furniture_geometry.pose.x = req.furniture_geometry_list[index].pose.position.x
00305 furniture_geometry.pose.y = req.furniture_geometry_list[index].pose.position.y
00306 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])
00307 furniture_geometry.pose.theta = furniture_pose_rpy[2]
00308 furniture_geometry.l = req.furniture_geometry_list[index].l
00309 furniture_geometry.w = req.furniture_geometry_list[index].w
00310 furniture_geometry.h = req.furniture_geometry_list[index].h
00311 furniture_geometry_list.append(furniture_geometry)
00312 index += 1
00313
00314
00315
00316
00317
00318
00319
00320 best_gripper_pose_x = robot_base_pose_x - 0.8 * math.cos(robot_base_pose_th) + 0.1 * math.sin(robot_base_pose_th)
00321 best_gripper_pose_y = robot_base_pose_y - 0.8 * math.sin(robot_base_pose_th) - 0.1 * math.cos(robot_base_pose_th)
00322
00323
00324 th = math.atan(target_obj_y - best_gripper_pose_y / target_obj_x - best_gripper_pose_x)
00325 if target_obj_x < best_gripper_pose_x and target_obj_y > best_gripper_pose_y:
00326 th += math.pi
00327 elif target_obj_x < best_gripper_pose_x and target_obj_y < best_gripper_pose_y:
00328 th -= math.pi
00329 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)
00330 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)
00331 th = math.atan((rb_pose.y - target_obj_y) / (rb_pose.x - target_obj_x))
00332 if rb_pose.x < target_obj_x and rb_pose.y > target_obj_y:
00333 th += math.pi
00334 elif rb_pose.x < target_obj_x and rb_pose.y < target_obj_y:
00335 th -= math.pi
00336 delta_th = robot_base_pose_th - th
00337 if delta_th < 0:
00338 delta_th = -1.0 * delta_th
00339
00340
00341 if delta_x > 0.1 or delta_x < -0.15:
00342 reach = 0
00343 elif delta_y > 0.15 or delta_y < -0.1:
00344 reach = 0
00345 elif delta_th < -10.0 / 180.0 * math.pi or delta_th > 10.0 / 180 * math.pi:
00346 reach = 0
00347 else:
00348
00349 index_x = int(round(delta_x / 0.025 + 6))
00350 index_y = int(round(delta_y / 0.025 + 6))
00351 index_th = int(round(delta_th / (1.0 / 180.0 * math.pi) + 10))
00352 member_x = mf1_x[index_x]
00353 member_y = mf1_y[index_y]
00354 member_th = mf1_th[index_th]
00355
00356 reach = min(member_x, member_y, member_th)
00357
00358
00359
00360
00361
00362
00363 step_angle_1 = 5.0 / 180.0 * math.pi
00364 dist_1 = 0.8
00365 grasp_base_pose_list_1 = list()
00366 grasp_base_pose_list_1 = getRobotBasePoseList(step_angle_1, dist_1, rb_pose, target_obj_x, target_obj_y)
00367
00368
00369
00370 step_angle_2 = 5.0 / 180.0 * math.pi
00371 dist_2 = 0.9
00372 grasp_base_pose_list_2 = list()
00373 grasp_base_pose_list_2 = getRobotBasePoseList(step_angle_2, dist_2, rb_pose, target_obj_x, target_obj_y)
00374
00375
00376
00377
00378 obstacle_check = 1
00379
00380 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)
00381
00382 grasp_base_pose = Pose2D()
00383
00384 if grasp_base_pose_list:
00385 obstacle_check = 0
00386 grasp_base_pose = grasp_base_pose_list[0]
00387
00388 else:
00389
00390 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)
00391
00392
00393
00394 if grasp_base_pose_list:
00395
00396 obstacle_check = 0
00397 grasp_base_pose = grasp_base_pose_list[0]
00398
00399
00400
00401 min_dist = 0.40
00402 hdz_size = 0.05
00403 dist_list = list()
00404 index = 0
00405 while index < len(furniture_geometry_list):
00406 th = math.atan((grasp_base_pose.y - furniture_geometry_list[index].pose.y) / (grasp_base_pose.x - furniture_geometry_list[index].pose.x))
00407 if grasp_base_pose.x < furniture_geometry_list[index].pose.x and grasp_base_pose.y > furniture_geometry_list[index].pose.y:
00408 th = math.pi + th
00409 if grasp_base_pose.x < furniture_geometry_list[index].pose.x and grasp_base_pose.y < furniture_geometry_list[index].pose.y:
00410 th = -math.pi + th
00411 delta_x = math.sqrt((grasp_base_pose.x - furniture_geometry_list[index].pose.x) ** 2 + (grasp_base_pose.y - furniture_geometry_list[index].pose.y) ** 2) * math.cos(th - furniture_geometry_list[index].pose.theta)
00412 delta_y = math.sqrt((grasp_base_pose.x - furniture_geometry_list[index].pose.x) ** 2 + (grasp_base_pose.y - furniture_geometry_list[index].pose.y) ** 2) * math.sin(th - furniture_geometry_list[index].pose.theta)
00413 delta_x = abs(delta_x)
00414 delta_y = abs(delta_y)
00415 dist = max((delta_x - furniture_geometry_list[index].w / 2.0), (delta_y - furniture_geometry_list[index].l / 2.0))
00416 dist_list.append(dist)
00417 index += 1
00418 R = min(min(dist_list) - min_dist, hdz_size)
00419
00420
00421
00422
00423 return obstacle_check, reach, grasp_base_pose, R
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433 def symbol_grounding_grasp_base_region_server():
00434 rospy.init_node('symbol_grounding_grasp_base_region_server')
00435 s = rospy.Service('symbol_grounding_grasp_base_region', SymbolGroundingGraspBaseRegion, handle_symbol_grounding_grasp_base_region)
00436 print "Ready to receive requests."
00437 rospy.spin()
00438
00439
00440
00441
00442 if __name__ == "__main__":
00443 symbol_grounding_grasp_base_region_server()