def _do_detection(self, point_head_at): req = TabletopDetectionRequest() req.return_clusters = True req.return_models = True req.num_models = 1 if point_head_at: point_head_at = tl.transform_point_stamped(self._wi.world_frame, point_head_at) zrng = range(0, int(100*self.table_search_max), int(100*self.table_search_resolution)) sgnrng = [-1.0, 1.0] rospy.logdebug('For table finding, trying z offsets for head of:' +str(zrng)) if not point_head_at or not zrng: zrng = [0] sgnrng = [1.0] first_pass=True for z in zrng: for sgn in sgnrng: if point_head_at: new_point = copy.copy(point_head_at.point) new_point.z += sgn*z/100.0 self._head._point_head([new_point.x, new_point.y, new_point.z], point_head_at.header.frame_id) rospy.sleep(0.4) #let the head bobbing settle out res = self._detect_srv(req) if res.detection.result != res.detection.SUCCESS: rospy.logerr('Unable to find table. Error was: '+ str(res.detection.result)) if first_pass: exc = DetectionError('Unable to find table', error_code=res.detection.result) first_pass = False continue if self.allow_vertical_tables: return res table = res.detection.table #is the table somewhat horizontal? table_pose_bl = tl.transform_pose_stamped(self._wi.robot_frame, table.pose) vert = Point() vert.z = 1.0 oripose = Pose() oripose.orientation = table_pose_bl.pose.orientation table_vertical = gt.transform_point(vert, oripose) if table_vertical.z < self.vertical_threshold: rospy.logerr('Table vertical is [%f, %f, %f]. This is not a horizontal table', table_vertical.x, table_vertical.y, table_vertical.z) res.detection.result = res.detection.OTHER_ERROR if first_pass: exc = DetectionError('Unable to find horizontal table', error_code=res.detection.result) first_pass = False continue rospy.loginfo('Found horizontal table. Vertical is [%f, %f, %f]', table_vertical.x, table_vertical.y, table_vertical.z) return res raise exc
def transform_point(self, new_frame_id, old_frame_id, point, robot_state): """ Transforms a point defined in old_frame_id into new_frame_id according to robot state. **Args:** **new_frame_id (string):** new frame id **old_frame_id (string):** old frame id **point (geometry_msgs.msg.Point):** point defined in old_frame_id **robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation **Returns:** A geometry_msgs.msg.Point defined in new_frame_id **Raises:** **exceptions.TransformStateError:** if there is an error getting the transform """ trans = self.get_transform(old_frame_id, new_frame_id, robot_state) return gt.transform_point(point, conv.transform_to_pose(trans.transform))
def transform_point(self, new_frame_id, old_frame_id, point, robot_state): ''' Transforms a point defined in old_frame_id into new_frame_id according to robot state. **Args:** **new_frame_id (string):** new frame id **old_frame_id (string):** old frame id **point (geometry_msgs.msg.Point):** point defined in old_frame_id **robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation **Returns:** A geometry_msgs.msg.Point defined in new_frame_id **Raises:** **exceptions.TransformStateError:** if there is an error getting the transform ''' trans = self.get_transform(old_frame_id, new_frame_id, robot_state) return gt.transform_point(point, conv.transform_to_pose(trans.transform))
def max_angles(self, pose): ''' @type pose: PoseStamped @param pose: the pose the object will be released in (in the frame of the robot) ''' ## get the center of mass in the frame of the object, ## then transform it into the robot frame cm = self.center_of_mass(pose).tolist() ## in the frame of the object tf_cm = self.translate(cm, pose) ## get the corners of the base of the bounding box of the object in robot frame base = self.get_base(pose.pose) # print "base="+str([pivot.position.x, pivot.position.y, pivot.position.z]) print "base=" + str(base) ## corner of the base is the pivot pt, transform to obj frame # pivot_matrix = self.inv_translate([[base[3][0]],[base[3][1]],[base[3][2]]], pose).tolist() # pivot = [pivot_matrix[0][0], pivot_matrix[1][0], pivot_matrix[2][0]] ## get the euler angles of the pose (goal pose) (orig_phi, orig_theta, orig_psi) = geometry_tools.quaternion_to_euler(pose.pose.orientation) ## max rotation of pi/2 max_phi = orig_phi + math.pi / 2 max_theta = orig_theta + math.pi / 2 pivot = Point() pivot.x = base[0][0] pivot.y = base[0][1] pivot.z = base[0][2] cm = Pose() cm.position.x = tf_cm[0][0] cm.position.y = tf_cm[1][0] cm.position.z = tf_cm[2][0] cm.orientation = pose.pose.orientation t = geometry_tools.inverse_transform_point(pivot, cm) tol = 0.01 ## find max angle around x-axis, phi print "cm=" + str(tf_cm.tolist()) for phi in frange(0.0, math.pi / 2, 0.1): transform = Pose() transform.orientation = geometry_tools.euler_to_quaternion( phi, 0.0, 0.0) cm_new = Pose() cm_new.position = cm.position cm_new.orientation = geometry_tools.transform_quaternion( cm.orientation, transform) p_new = geometry_tools.transform_point(t, cm_new) y_diff = abs(tf_cm[1][0] - p_new.y) z_diff = abs(tf_cm[2][0] - p_new.z) # print "phi angle="+str(phi)+" pivot="+str([p_new.x, p_new.y, p_new.z]) if y_diff < tol or z_diff < tol: max_phi = phi break for theta in frange(0.0, math.pi / 2, 0.1): transform = Pose() transform.orientation = geometry_tools.euler_to_quaternion( 0.0, theta, 0.0) cm_new = Pose() cm_new.position = cm.position cm_new.orientation = geometry_tools.transform_quaternion( cm.orientation, transform) p_new = geometry_tools.transform_point(t, cm_new) x_diff = abs(tf_cm[0][0] - p_new.x) z_diff = abs(tf_cm[2][0] - p_new.z) # print "theta angle="+str(theta)+" pivot="+str([p_new.x, p_new.y, p_new.z]) if z_diff < tol or x_diff < tol: max_theta = theta break return (max_phi, max_theta)
def free_spots_from_dimensions(table, object_dims, blocking_objs, res=0.1): ''' Currently works only with a single shape Assumes table is in xy plane ''' #find the lower left corner of the table in the header frame #we want everything relative to this point table_corners = gt.bounding_box_corners(table.shapes[0]) lower_left = Pose() lower_left.position = gt.list_to_point(table_corners[0]) lower_left.orientation.w = 1.0 #rospy.loginfo('Table corners are:') #for c in table_corners: rospy.loginfo('\n'+str(c)) #rospy.loginfo('Lower left =\n'+str(lower_left)) #rospy.loginfo('Table header =\n'+str(table.header)) #rospy.loginfo('Table pose =\n'+str(table.poses[0])) #this is the position of the minimum x, minimum y point in the table's header frame table_box_origin = gt.transform_pose(lower_left, table.poses[0]) tr = gt.inverse_transform_list(gt.transform_list(table_corners[-1], table.poses[0]), table_box_origin) table_dims = (tr[0], tr[1]) tbos = PoseStamped() tbos.header = table.header tbos.pose = table_box_origin marray.markers.append(vt.marker_at(tbos, ns='table_origin', mtype=Marker.CUBE, r=1.0)) max_box = PointStamped() max_box.header = table.header max_box.point = gt.list_to_point(gt.transform_list([table_dims[0], table_dims[1], 0], table_box_origin)) marray.markers.append(vt.marker_at_point(max_box, ns='table_max', mtype=Marker.CUBE, r=1.0)) #rospy.loginfo('Table box origin is '+str(table_box_origin)+' dimensions are '+str(table_dims)) locs_on_table = _get_feasible_locs(table_dims, object_dims, res) for i, l in enumerate(locs_on_table): pt = Point() pt.x = l[0] pt.y = l[1] mpt = PointStamped() mpt.header = table.header mpt.point = gt.transform_point(pt, table_box_origin) marray.markers.append(vt.marker_at_point(mpt, mid=i, ns='locations', r=1.0, g=1.0, b=0.0)) feasible_locs = [] #check that these points really are on the table for i, l in enumerate(locs_on_table): pt = Point() pt.x = l[0] pt.y = l[1] #this point is now defined relative to the origin of the table (rather than its minimum x, minimum y point) table_pt = gt.inverse_transform_point(gt.transform_point(pt, table_box_origin), table.poses[0]) if point_on_table(table_pt, table.shapes[0]): feasible_locs.append(l) marray.markers[i+2].color.r = 0.0 marray.markers[i+2].color.b = 1.0 rospy.loginfo('Testing '+str(len(feasible_locs))+' locations') if not feasible_locs: return feasible_locs forbidden=[] for i, o in enumerate(blocking_objs): ofcs = gt.bounding_box_corners(o.shapes[0]) objpose = tl.transform_pose(table.header.frame_id, o.header.frame_id, o.poses[0]) hfcs = [gt.transform_list(c, objpose) for c in ofcs] tfcs = [gt.inverse_transform_list(c, table_box_origin) for c in hfcs] oxmax = max([c[0] for c in tfcs]) oxmin = min([c[0] for c in tfcs]) oymax = max([c[1] for c in tfcs]) oymin = min([c[1] for c in tfcs]) forbidden.append(((oxmin, oymin), (oxmax - oxmin, oymax - oymin))) #rospy.loginfo('\n'+str(forbidden[-1])) ps = PoseStamped() ps.header = table.header ps.pose = objpose ps = PoseStamped() ps.header = table.header ps.pose.position = gt.list_to_point(gt.transform_list([oxmin, oymin, 0], table_box_origin)) ps.pose.orientation.w = 1.0 marray.markers.append(vt.marker_at(ps, ns='forbidden', mid=i, r=1.0, b=0.0)) # Remove forbidden rectangles for (bottom_left, dims) in forbidden: _remove_feasible_locs(feasible_locs, object_dims, bottom_left, _add(bottom_left, dims), res) rospy.loginfo('There are '+str(len(feasible_locs))+' possible locations') obj_poses = [] for i, fl in enumerate(feasible_locs): table_frame_pose = Pose() table_frame_pose.position.x = fl[0] + object_dims[0]/2.0 table_frame_pose.position.y = fl[1] + object_dims[1]/2.0 table_frame_pose.orientation.w = 1.0 pose = PoseStamped() pose.header = copy.deepcopy(table.header) pose.pose = gt.transform_pose(table_frame_pose, table_box_origin) obj_poses.append(pose) pt = PointStamped() pt.header = table.header pt.point = pose.pose.position marray.markers.append(vt.marker_at_point(pt, mid=i, ns='final_locations', g=1.0, b=0.0)) #rospy.loginfo('Object poses are:') #for op in obj_poses: rospy.loginfo(str(op)) for i in range(10): vpub.publish(marray) rospy.sleep(0.1) return obj_poses