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)
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)
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 = [positionMessageFromFrame(tform)]
msg.region_id = [uid]
msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
lcmUtils.publish(self.request_channel, msg)
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 = [positionMessageFromFrame(tform)]
msg.region_id = [uid]
msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
lcmUtils.publish(self.request_channel, msg)