$search
00001 #!/usr/bin/env python 00002 00003 ################################################################# 00004 ##\file 00005 # 00006 # \note 00007 # Copyright (c) 2012 \n 00008 # University of Bedfordshire \n\n 00009 # 00010 ################################################################# 00011 # 00012 # \note 00013 # Project name: care-o-bot 00014 # \note 00015 # ROS stack name: srs_public 00016 # \note 00017 # ROS package name: srs_symbolic_grounding 00018 # 00019 # \author 00020 # Author: Beisheng Liu, email:beisheng.liu@beds.ac.uk 00021 # \author 00022 # Supervised by: Dayou Li, email:dayou.li@beds.ac.uk 00023 # 00024 # \date Date of creation: April 2012 00025 # 00026 # \brief 00027 # Grounding grasp object commands 00028 # 00029 ################################################################# 00030 # 00031 # Redistribution and use in source and binary forms, with or without 00032 # modification, are permitted provided that the following conditions are met: 00033 # 00034 # - Redistributions of source code must retain the above copyright 00035 # notice, this list of conditions and the following disclaimer. \n 00036 # - Redistributions in binary form must reproduce the above copyright 00037 # notice, this list of conditions and the following disclaimer in the 00038 # documentation and/or other materials provided with the distribution. \n 00039 # - Neither the name of the University of Bedfordshire nor the names of its 00040 # contributors may be used to endorse or promote products derived from 00041 # this software without specific prior written permission. \n 00042 # 00043 # This program is free software: you can redistribute it and/or modify 00044 # it under the terms of the GNU Lesser General Public License LGPL as 00045 # published by the Free Software Foundation, either version 3 of the 00046 # License, or (at your option) any later version. 00047 # 00048 # This program is distributed in the hope that it will be useful, 00049 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00050 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00051 # GNU Lesser General Public License LGPL for more details. 00052 # 00053 # You should have received a copy of the GNU Lesser General Public 00054 # License LGPL along with this program. 00055 # If not, see <http://www.gnu.org/licenses/>. 00056 # 00057 ################################################################# 00058 00059 00060 import roslib; roslib.load_manifest('srs_symbolic_grounding') 00061 00062 from srs_symbolic_grounding.srv import * 00063 from srs_symbolic_grounding.msg import * 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 #from srs_knowledge.msg import * 00069 import rospy 00070 import math 00071 import tf 00072 from tf.transformations import euler_from_quaternion 00073 #import csv 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 #get the occupancy grid map from the navigation services 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 #this function is used to calculate a list of canidate poses around the target obj. the number of candidate poses is 360/step_angle 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 #rospy.loginfo(th) 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 #rospy.loginfo(grasp_base_pose_list) 00122 return grasp_base_pose_list 00123 00124 00125 00126 #function for checking if a pose is obstacle free. 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 #to check if a pose is too close to the edge of the parent obj. the valid poses will be stored in the parent_obj_checked_grasp_base_pose_list. 00141 dist_to_table = 0.6 #minimum distance to the edge of the parent obj 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 #rospy.loginfo([delta_x, delta_y]) 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 #rospy.loginfo(parent_obj_checked_grasp_base_pose_list) 00159 00160 00161 if parent_obj_checked_grasp_base_pose_list: #if there is a parent obj free pose 00162 00163 #to check if a pose is too close to or blocked by the furnitures in the room 00164 dist_to_obstacles = 0.6 #minimum disatnce to the furnitures in the room 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 #rospy.loginfo(obstacle_checked_grasp_base_pose_list) 00186 00187 if obstacle_checked_grasp_base_pose_list: 00188 00189 #to check if a pose is too near to or blocked by the wall 00190 data = getMapClient() #get occupancy grid map 00191 #rospy.loginfo(data.map.info) 00192 dist_to_walls = 0.5 #set the minimum distance to the walls 00193 threshold = 20 #>20:occupied 00194 step_angle = 5.0 #(360 / step_angle) points will be put around the robot for the wall check 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 #rospy.loginfo(map_index_list) 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 #rospy.loginfo(wall_checked_grasp_base_pose_list) 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 #test value near table 00242 rb_pose = Pose2D() 00243 rb_pose.x = -1.06 00244 rb_pose.y = 1.08 00245 rb_pose.theta = 0.0 00246 00247 #predefined membership function 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 #transfrom the parent obj data from database 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 robot_base_pose_x = rb_pose.x 00272 robot_base_pose_y = rb_pose.y 00273 robot_base_pose_th = rb_pose.theta 00274 00275 parent_obj_x = req.parent_obj_geometry.pose.position.x 00276 parent_obj_y = req.parent_obj_geometry.pose.position.y 00277 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]) 00278 parent_obj_th = parent_obj_rpy[2] 00279 parent_obj_l = req.parent_obj_geometry.l 00280 parent_obj_w = req.parent_obj_geometry.w 00281 parent_obj_h = req.parent_obj_geometry.h 00282 00283 00284 ''' 00285 #get furniture list from the knowledge base (part 2) 00286 00287 workspace_info = getWorkspaceOnMap() 00288 furniture_geometry_list = list() 00289 furniture_geometry_list = workspace_info.objectsInfo 00290 00291 00292 #transfrom list 00293 index = 0 00294 furniture_geometry_list = list() 00295 while index < len(furniture_geometry_list): 00296 furniture_geometry = FurnitureGeometry() 00297 furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x 00298 furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y 00299 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]) 00300 furniture_geometry.pose.theta = furniture_pose_rpy[2] 00301 furniture_geometry.l = furniture_geometry_list[index].l 00302 furniture_geometry.w = furniture_geometry_list[index].w 00303 furniture_geometry.h = furniture_geometry_list[index].h 00304 furniture_geometry_list.append(furniture_geometry) 00305 index += 1 00306 ''' 00307 #transfrom furniture geometry data from database 00308 index = 0 00309 furniture_geometry_list = list() 00310 while index < len(req.furniture_geometry_list): 00311 furniture_geometry = FurnitureGeometry() 00312 furniture_geometry.pose.x = req.furniture_geometry_list[index].pose.position.x 00313 furniture_geometry.pose.y = req.furniture_geometry_list[index].pose.position.y 00314 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]) 00315 furniture_geometry.pose.theta = furniture_pose_rpy[2] 00316 furniture_geometry.l = req.furniture_geometry_list[index].l 00317 furniture_geometry.w = req.furniture_geometry_list[index].w 00318 furniture_geometry.h = req.furniture_geometry_list[index].h 00319 furniture_geometry_list.append(furniture_geometry) 00320 index += 1 00321 00322 00323 00324 00325 #calculate the reachability 00326 00327 #calculate the best grippier pose from the robot's current base pose 00328 best_gripper_pose_x = robot_base_pose_x - 0.75 * math.cos(robot_base_pose_th) + 0.1 * math.sin(robot_base_pose_th) 00329 best_gripper_pose_y = robot_base_pose_y - 0.75 * math.sin(robot_base_pose_th) - 0.1 * math.cos(robot_base_pose_th) 00330 00331 #calculate the difference between the best gripper pose and the target obj pose 00332 th = math.atan(target_obj_y - best_gripper_pose_y / target_obj_x - best_gripper_pose_x) 00333 if target_obj_x < best_gripper_pose_x and target_obj_y > best_gripper_pose_y: 00334 th += math.pi 00335 elif target_obj_x < best_gripper_pose_x and target_obj_y < best_gripper_pose_y: 00336 th -= math.pi 00337 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) 00338 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) 00339 th = math.atan((rb_pose.y - target_obj_y) / (rb_pose.x - target_obj_x)) 00340 if rb_pose.x < target_obj_x and rb_pose.y > target_obj_y: 00341 th += math.pi 00342 elif rb_pose.x < target_obj_x and rb_pose.y < target_obj_y: 00343 th -= math.pi 00344 delta_th = robot_base_pose_th - th 00345 if delta_th < 0: 00346 delta_th = -1.0 * delta_th 00347 00348 #if target obj pose is out side hdz, reach = 0 00349 if delta_x > 0.1 or delta_x < -0.15: 00350 reach = 0 00351 elif delta_y > 0.15 or delta_y < -0.1: 00352 reach = 0 00353 elif delta_th < -10.0 / 180.0 * math.pi or delta_th > 10.0 / 180 * math.pi: 00354 reach = 0 00355 else: 00356 #calculate membership value according to the membership functions 00357 index_x = int(round(delta_x / 0.025 + 6)) 00358 index_y = int(round(delta_y / 0.025 + 6)) 00359 index_th = int(round(delta_th / (1.0 / 180.0 * math.pi) + 10)) 00360 member_x = mf1_x[index_x] 00361 member_y = mf1_y[index_y] 00362 member_th = mf1_th[index_th] 00363 #Apply the fuzzy rule. 00364 reach = min(member_x, member_y, member_th) 00365 #rospy.loginfo(reach) 00366 00367 00368 #calculate two lists of candidate base poses for grasping 00369 #the first candidate pose list 00370 step_angle_1 = 1.0 #360 / step_angle of candidate poses will be put around the target obj 00371 dist_1 = 0.75 #distance to the target obj 00372 step_angle_2 = 1.0 00373 dist_2 = 0.8 00374 #print target_obj_h 00375 if target_obj_h > 1.2 or target_obj_h < 0.85: 00376 dist_1 -= 0.05 00377 dist_2 -= 0.05 00378 grasp_base_pose_list_1 = list() 00379 grasp_base_pose_list_1 = getRobotBasePoseList(step_angle_1, dist_1, rb_pose, target_obj_x, target_obj_y) 00380 #rospy.loginfo(grasp_base_pose_list) 00381 00382 #calculate another list of candidate grasping poses with lower reachability 00383 grasp_base_pose_list_2 = list() 00384 grasp_base_pose_list_2 = getRobotBasePoseList(step_angle_2, dist_2, rb_pose, target_obj_x, target_obj_y) 00385 #rospy.loginfo(grasp_base_pose_list) 00386 00387 00388 #optimisation 00389 obstacle_check = 1 00390 #obstacle check 00391 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) 00392 00393 grasp_base_pose = Pose2D() 00394 #rospy.loginfo(grasp_base_pose_list) 00395 if grasp_base_pose_list: 00396 obstacle_check = 0 00397 grasp_base_pose = grasp_base_pose_list[0] 00398 00399 else: 00400 #obstacle check for the grasping poses with lower reachability when no valid pose can be found from the first candidate pose list 00401 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) 00402 00403 #rospy.loginfo(grasp_base_pose_list) 00404 #check if there is valid pose 00405 if grasp_base_pose_list: 00406 00407 obstacle_check = 0 00408 grasp_base_pose = grasp_base_pose_list[0] 00409 00410 00411 00412 00413 00414 00415 00416 #return value 00417 return obstacle_check, reach, grasp_base_pose 00418 00419 00420 00421 00422 00423 00424 00425 00426 00427 def symbol_grounding_grasp_base_pose_experimental_server(): 00428 rospy.init_node('symbol_grounding_grasp_base_pose_experimental_server') 00429 s = rospy.Service('symbol_grounding_grasp_base_pose_experimental', SymbolGroundingGraspBasePoseExperimental, handle_symbol_grounding_grasp_base_pose_experimental) 00430 print "Ready to receive requests." 00431 rospy.spin() 00432 00433 00434 00435 00436 if __name__ == "__main__": 00437 symbol_grounding_grasp_base_pose_experimental_server()