Beispiel #1
0
    def autoIRISSegmentation(self,
                             xy_lb=[-2, -2],
                             xy_ub=[2, 2],
                             max_num_regions=10,
                             default_yaw=np.nan,
                             max_slope_angle=np.nan,
                             max_height_variation=np.nan,
                             plane_distance_tolerance=np.nan,
                             plane_angle_tolerance=np.nan):
        msg = lcmdrc.auto_iris_segmentation_request_t()
        msg.utime = getUtime()
        A = np.vstack((np.eye(2), -np.eye(2)))
        b = np.hstack((xy_ub, -np.asarray(xy_lb)))
        msg.xy_bounds = irisUtils.encodeLinCon(A, b)

        msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
        msg.map_mode = self.map_mode_map[self.params.properties.map_mode]

        msg.num_seed_poses = 0

        msg.max_num_regions = max_num_regions
        msg.region_id = self.getNewUIDs(max_num_regions)
        msg.default_yaw = default_yaw
        msg.max_slope_angle = max_slope_angle
        msg.max_height_variation = max_height_variation
        msg.plane_distance_tolerance = plane_distance_tolerance
        msg.plane_angle_tolerance = plane_angle_tolerance
        lcmUtils.publish('AUTO_IRIS_SEGMENTATION_REQUEST', msg)
Beispiel #2
0
    def autoIRISSegmentation(self,
                             xy_lb=[-2, -2],
                             xy_ub=[2, 2],
                             max_num_regions=10,
                             default_yaw=np.nan,
                             max_slope_angle=np.nan,
                             max_height_variation=np.nan,
                             plane_distance_tolerance=np.nan,
                             plane_angle_tolerance=np.nan):
        msg = lcmdrc.auto_iris_segmentation_request_t()
        msg.utime = getUtime()
        A = np.vstack((np.eye(2), -np.eye(2)))
        b = np.hstack((xy_ub, -np.asarray(xy_lb)))
        msg.xy_bounds = irisUtils.encodeLinCon(A, b)

        msg.initial_state = robotstate.drakePoseToRobotState(
            self.jointController.q)
        msg.map_mode = self.map_mode_map[self.params.properties.map_mode]

        msg.num_seed_poses = 0

        msg.max_num_regions = max_num_regions
        msg.region_id = self.getNewUIDs(max_num_regions)
        msg.default_yaw = default_yaw
        msg.max_slope_angle = max_slope_angle
        msg.max_height_variation = max_height_variation
        msg.plane_distance_tolerance = plane_distance_tolerance
        msg.plane_angle_tolerance = plane_angle_tolerance
        lcmUtils.publish('AUTO_IRIS_SEGMENTATION_REQUEST', msg)
Beispiel #3
0
 def requestIRISRegion(self, tform, uid, bounding_box_width=2):
     start = np.asarray(tform.GetPosition())
     A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width)
     msg = lcmdrc.iris_region_request_t()
     msg.utime = getUtime()
     msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
     msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
     msg.num_seed_poses = 1
     msg.seed_poses = [transformUtils.positionMessageFromFrame(tform)]
     msg.region_id = [uid]
     msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
     lcmUtils.publish(self.request_channel, msg)
Beispiel #4
0
 def requestIRISRegion(self, tform, uid, bounding_box_width=2):
     start = np.asarray(tform.GetPosition())
     A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width)
     msg = lcmdrc.iris_region_request_t()
     msg.utime = getUtime()
     msg.initial_state = robotstate.drakePoseToRobotState(
         self.jointController.q)
     msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
     msg.num_seed_poses = 1
     msg.seed_poses = [transformUtils.positionMessageFromFrame(tform)]
     msg.region_id = [uid]
     msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
     lcmUtils.publish(self.request_channel, msg)