def get_end_eff_xy_basestate_coords_from_xy_yaw(self, xy_yaw_deg): """ :param xy_yaw_deg: :return: """ yaw_rad = np.deg2rad(xy_yaw_deg[2]) x = xy_yaw_deg[0] y = xy_yaw_deg[1] base = (parameters.BASE_STATE_END_EFF_DX_FROM_TORSO, parameters.BASE_STATE_END_EFF_DY_FROM_TORSO) fl = MathUtils._2d_rotation_transformation(base[0], base[1], yaw_rad) fr = MathUtils._2d_rotation_transformation(base[0], -base[1], yaw_rad) bl = MathUtils._2d_rotation_transformation(-base[0], base[1], yaw_rad) br = MathUtils._2d_rotation_transformation(-base[0], -base[1], yaw_rad) fl_x = x + fl[0] fl_y = y + fl[1] fr_x = x + fr[0] fr_y = y + fr[1] bl_x = x + bl[0] bl_y = y + bl[1] br_x = x + br[0] br_y = y + br[1] fl_z = self.height_map.height_at_xy(fl_x, fl_y) fr_z = self.height_map.height_at_xy(fr_x, fr_y) bl_z = self.height_map.height_at_xy(bl_x, bl_y) br_z = self.height_map.height_at_xy(br_x, br_y) return (fl_x, fl_y, fl_z), (fr_x, fr_y, fr_z), (br_x, br_y, br_z), (bl_x, bl_y, bl_z)
def save_high_density_xy_yaw_path(self): num_to_add_between_points = parameters.HLTRAJ_NB_POINTS_BTWN_PATH_NODES extended_xy_yaw_path = MathUtils.list_extender( self.xy_yaw_path, num_to_add_between_points) self.higher_density_xy_yaw_path = extended_xy_yaw_path ave_dist = 0 for i in range(0, len(extended_xy_yaw_path) - 2): xy_yaw1 = extended_xy_yaw_path[i] xy_yaw2 = extended_xy_yaw_path[i + 1] ave_dist += MathUtils._2d_euclidian_distance(xy_yaw1, xy_yaw2) ave_dist /= len(extended_xy_yaw_path) self.ave_higher_density_xy_yaw_path_distance_change = ave_dist
def save_smoothed_path(self): if settings.STEPSEQ_VERBOSITY >= 3: for xy_yaw in self.xy_yaw_path: print(" ", Logger.pp_list(xy_yaw)) try: smoothed_path = MathUtils._3d_pointlist_cubic_interprolation( self.xy_yaw_path) except TypeError: Logger.log("Error: 'm > k must hold'", "FAIL") smoothed_path = self.xy_yaw_path self.smoothed_path = smoothed_path ave_dist = 0 for i in range(0, len(smoothed_path) - 2): xy_yaw1 = smoothed_path[i] xy_yaw2 = smoothed_path[i + 1] ave_dist += MathUtils._2d_euclidian_distance(xy_yaw1, xy_yaw2) ave_dist /= len(smoothed_path) self.ave_smooth_path_distance_change = ave_dist
def __init__(self, height_map_obj): # super(self.__class__, self).__init__() Map.__init__(self) OutputSuperclass.__init__(self, "gradient map") self.x_vars = height_map_obj.x_vars self.y_vars = height_map_obj.y_vars self.x_start = height_map_obj.x_vars[0] self.x_end = height_map_obj.x_vars[1] self.x_granularity = height_map_obj.x_vars[2] self.y_start = height_map_obj.y_vars[0] self.y_end = height_map_obj.y_vars[1] self.y_granularity = height_map_obj.y_vars[2] self.x_indices = int((self.x_end - self.x_start) / self.x_granularity) self.y_indices = int((self.y_end - self.y_start) / self.y_granularity) self.hm_x_gradient, self.hm_y_gradient = MathUtils.xy_gradient( height_map_obj.get_np_hm_array()) self.hm_x_slope, self.hm_y_slope = MathUtils.gradient_to_slope_arr( self.hm_x_gradient), MathUtils.gradient_to_slope_arr( self.hm_y_gradient)
def parse_hm(self, pcd_fname, visualize=False, debug=False): coordinate_frame_mesh = o3d.geometry.create_mesh_coordinate_frame( size=0.6, origin=[0, 0, 0]) voxel_size = 0.01 # pcd: o3d.geometry.PointCloud = o3d.io.read_point_cloud(pcd_fname) pcd: o3d.geometry.PointCloud = o3d.io.read_point_cloud(pcd_fname) pcd: o3d.geometry.PointCloud = o3d.geometry.voxel_down_sample( pcd, voxel_size=voxel_size) pc_xyzs = np.asarray(pcd.points) x_width_rob_mask = 1.7 y_width_rob_mask = 1 robot_mask = np.logical_and( np.abs(pc_xyzs[:, 0]) < x_width_rob_mask, np.abs(pc_xyzs[:, 1]) < y_width_rob_mask) pc_xyzs_norobot = pc_xyzs[np.logical_not(robot_mask)] no_robot_pcd = o3d.geometry.PointCloud() no_robot_pcd.points = o3d.utility.Vector3dVector(pc_xyzs_norobot) pcd, _ = o3d.geometry.statistical_outlier_removal(no_robot_pcd, nb_neighbors=15, std_ratio=1.5) T = self.get_transformation(pcd_fname) pcd.transform(T) pc_xyzs = np.asarray(pcd.points) floor_z = PCloudParser.get_ground_z(pc_xyzs) if debug: print(f"ground z: {floor_z}") x_min = np.min(pc_xyzs[:, 0]) y_min = np.min(pc_xyzs[:, 1]) pcd.translate([-x_min, -y_min, -floor_z]) pc_xyzs = np.asarray(pcd.points) if debug: print(f" pc pre cieling mask filter has {len(pc_xyzs)} points") # Filter all points above k meters cieling_mask = pc_xyzs[:, 2] < 2.5 pc_xyzs = pc_xyzs[cieling_mask] if debug: print(f" pc post cieling mask filter has {len(pc_xyzs)} points") print("new ground z: ", PCloudParser.get_ground_z(pc_xyzs)) floor_threshold = 0.04 floor_mask = np.abs(pc_xyzs[:, 2]) < floor_threshold floor_points_xyz = pc_xyzs[floor_mask] floor_pcd = o3d.geometry.PointCloud() floor_pcd.points = o3d.utility.Vector3dVector(floor_points_xyz) # non_floor_mask = np.logical_not(floor_mask) non_floor_mask = pc_xyzs[:, 2] > floor_threshold + 0.025 non_floor_points_xyz = pc_xyzs[non_floor_mask] non_floor_pcd = o3d.geometry.PointCloud() non_floor_pcd.points = o3d.utility.Vector3dVector(non_floor_points_xyz) a0, b0, c0, x0, y0, z0 = PCloudParser.best_fitting_plane( floor_points_xyz, debug=False) ang_to_zaxis_rad = MathUtils.angle_between_two_3d_vectors([a0, b0, c0], [0, 0, 1]) ang_to_zaxis_deg = np.rad2deg(ang_to_zaxis_rad) round_amt = 6 if debug: print( "a0, b0, c0 :", round(a0, round_amt), round(b0, round_amt), round(c0, round_amt), " angle to <0, 0, 1>:", round(ang_to_zaxis_deg, 3), " degrees", ) planar_pc = o3d.geometry.PointCloud() planar_pc.points = o3d.utility.Vector3dVector( self.planar_point_cloud(0, 0, 1, 5, 5, 0, 10, 25, 10, 25)) if visualize: planar_pc.paint_uniform_color([1, 0, 0]) floor_pcd.paint_uniform_color([1, 0.706, 0]) o3d.visualization.draw_geometries( [floor_pcd, non_floor_pcd, planar_pc, coordinate_frame_mesh]) return pc_xyzs, non_floor_points_xyz
def get_end_affector_xyz_coords_from_xy_yaw_deg_at_base_state( self, xy_yaw_deg, debug=False, visualize=False, with_x_margin=None, with_y_margin=None): yaw_rad = np.deg2rad(xy_yaw_deg[2]) x = xy_yaw_deg[0] y = xy_yaw_deg[1] base = (parameters.BASE_STATE_END_EFF_DX_FROM_TORSO, parameters.BASE_STATE_END_EFF_DY_FROM_TORSO) fl = MathUtils._2d_rotation_transformation(base[0], base[1], yaw_rad) fr = MathUtils._2d_rotation_transformation(base[0], -base[1], yaw_rad) bl = MathUtils._2d_rotation_transformation(-base[0], base[1], yaw_rad) br = MathUtils._2d_rotation_transformation(-base[0], -base[1], yaw_rad) fl_x = x + fl[0] fl_y = y + fl[1] fr_x = x + fr[0] fr_y = y + fr[1] bl_x = x + bl[0] bl_y = y + bl[1] br_x = x + br[0] br_y = y + br[1] if ((not self.xy_inbound(fl_x, fl_y, with_x_margin=with_x_margin, with_y_margin=with_y_margin)) or (not self.xy_inbound(fr_x, fr_y, with_x_margin=with_x_margin, with_y_margin=with_y_margin)) or (not self.xy_inbound(br_x, br_y, with_x_margin=with_x_margin, with_y_margin=with_y_margin)) or (not self.xy_inbound(bl_x, bl_y, with_x_margin=with_x_margin, with_y_margin=with_y_margin))): if debug: print( " value error in get end effector xyz coords at base state!" ) return False, False, False, False if visualize: self.visualize_cost(fl_x, fl_y, 1, "FRONT LEFT") self.visualize_cost(fr_x, fr_y, 1, "FRONT RIGHT") self.visualize_cost(bl_x, bl_y, 1, "BACK LEFT") self.visualize_cost(br_x, br_y, 1, "BACK RIGHT") if debug: print( "\nin get_end_affector_xy_coords_from_xy_yaw_at_base_state()") print("\n input x:", x) print(" y:", y) print(" yaw:", xy_yaw_deg[2]) print("\n fl:", Logger.pp_list([fl_x, fl_y])) print(" fr:", Logger.pp_list([fr_x, fr_y])) print(" br:", Logger.pp_list([br_x, br_y])) print(" bl:", Logger.pp_list([bl_x, bl_y])) fl_z = self.height_map.height_at_xy(fl_x, fl_y) fr_z = self.height_map.height_at_xy(fr_x, fr_y) bl_z = self.height_map.height_at_xy(bl_x, bl_y) br_z = self.height_map.height_at_xy(br_x, br_y) return (fl_x, fl_y, fl_z), (fr_x, fr_y, fr_z), (br_x, br_y, br_z), (bl_x, bl_y, bl_z)
def xy_centroid_from_o_3dgeoms(self, o_2DGeometry_objs, closest_to=None): """ @summary returns the centroid of the intersection area of numrerous _2D_xxx support ibjects @param o_2D_objects: @return: """ intersection_poly = self.get_shapely_intersection_from_o_3dgeoms( o_2DGeometry_objs) # VisUtils.visualize_shapely_poly(intersection_poly, add_z=.25, color=VisUtils.TURQ) intersection_centroid = intersection_poly.centroid.coords try: try: intersection_poly = intersection_poly.geoms[1] except IndexError: Logger.log("2d objects do not intersect", "FAIL") return None except AttributeError: pass if closest_to: d = 0.001 intersection_poly_centroid = intersection_poly.centroid.coords[0] v = MathUtils.normalize([ closest_to[0] - intersection_poly_centroid[0], closest_to[1] - intersection_poly_centroid[1] ]) xy = intersection_poly_centroid while intersection_poly.contains(Point(xy)): xy = MathUtils.add_scaled_vector_to_pt(xy, v, d) # print(f"xy: {xy}\t inside: {intersection_poly.contains(Point(xy))}") xy = MathUtils.add_scaled_vector_to_pt(xy, v, -d) # print(f"Done. xy: {xy}\t inside: {intersection_poly.contains(Point(xy))}") return xy # point = Point(closest_to[0], closest_to[1]) # pol_ext = LinearRing(intersection_poly.exterior.coords) # d = pol_ext.project(point) # p = pol_ext.interpolate(d) # closest_point_coords = list(p.coords)[0] # print(type(closest_point_coords)) # print(closest_point_coords) # return closest_point_coords # p1, p2 = nearest_points(intersection_poly, point) # print("intersection_poly.contains(p1): ",intersection_poly.contains(p1)) # print("intersection_poly.contains(p2): ",intersection_poly.contains(p2)) # return p1.x, p1.y # if type(intersection_poly) == type(GeometryCollection): # for i in intersection_poly.geoms: # if type(i) == type(Polygon): # intersection_poly = i # pol_ext = LinearRing(intersection_poly.exterior.coords) # closest_to_point = Point(closest_to[0], closest_to[1]) # d = pol_ext.project(closest_to_point) # p = pol_ext.interpolate(d) # closest_point_coords = list(p.coords)[0] # return closest_point_coords else: try: x = intersection_centroid.xy[0][0] y = intersection_centroid.xy[1][0] return [x, y] except AttributeError: Logger.log("2d objects do not intersect", "FAIL") return None
def enforce_safety_margin(self, scaling_factor): points = list(self.shapely_poly.exterior.coords) P1 = points[0] P2 = points[1] P3 = points[2] # 0, 10 V1 = MathUtils.get_vector_between_points(P1, P2) V1_neg = MathUtils.flip_vector(V1) V2 = MathUtils.get_vector_between_points(P2, P3) V2_neg = MathUtils.flip_vector(V2) V3 = MathUtils.get_vector_between_points(P3, P1) V3_neg = MathUtils.flip_vector(V3) v1_prime = MathUtils.scalar_multiply_vector(MathUtils.vector_adder(V1_neg, V3), scaling_factor) v2_prime = MathUtils.scalar_multiply_vector(MathUtils.vector_adder(V1, V2_neg), scaling_factor) v3_prime = MathUtils.scalar_multiply_vector(MathUtils.vector_adder(V2, V3_neg), scaling_factor) P_ret1 = MathUtils.add_scaled_vector_to_pt(P1, v1_prime, 1) P_ret2 = MathUtils.add_scaled_vector_to_pt(P2, v2_prime, 1) P_ret3 = MathUtils.add_scaled_vector_to_pt(P3, v3_prime, 1) self.points = [P_ret1, P_ret2, P_ret3] self.shapely_poly = Polygon(self.points) self.shapely_poly.exterior.coords = self.points self.update_diag_points() self.save_incenter_xyr()
def save_incenter_xyr(self): self.incenterx, self.incentery, self.incenterr = MathUtils.incenter_circle_xy_R( self.points[0], self.points[1], self.points[2] )
def build_costmap(self, debug=False, exlude_slope=False, exlude_roughness=False, exlude_step=False, normalize_cost_arr=True): """ returns runtime """ if debug: print("calculating footstep location costs") start_t = time.time() x_idxs_per_step = parameters.CMAP_STEP_SIZEX y_idxs_per_step = parameters.CMAP_STEP_SIZEY x_idxs_per_step_on2 = x_idxs_per_step / 2.0 y_idxs_per_step_on2 = y_idxs_per_step / 2.0 # x_idxs_per_step = parameters.CMAP_STEP_SIZEX / self.hm_obj.x_granularity # x_idxs_per_step_on2 = x_idxs_per_step / 2.0 # y_idxs_per_step = parameters.CMAP_STEP_SIZEY / self.hm_obj.y_granularity # y_idxs_per_step_on2 = y_idxs_per_step / 2.0 # print( # f"parameters.CMAP_STEP_SIZEX / hm_obj.x_granularity: {parameters.CMAP_STEP_SIZEX} / {self.hm_obj.x_granularity}" # ) # print(f"x_idxs_per_step: {x_idxs_per_step}") # print(f"x_idxs_per_step_on2: {x_idxs_per_step_on2}") # print( # f"\nparameters.CMAP_STEP_SIZEY / hm_obj.y_granularity: {parameters.CMAP_STEP_SIZEY} / {self.hm_obj.y_granularity}" # ) # print(f"y_idxs_per_step: {y_idxs_per_step}") # print(f"y_idxs_per_step_on2: {y_idxs_per_step_on2}") print("x_idxs_per_step:", x_idxs_per_step) print("y_idxs_per_step:", y_idxs_per_step) assert ( abs(int(x_idxs_per_step) - x_idxs_per_step) < 1e-5 ), "CMAP_STEP_SIZE_X doesnt resolve into discrete indexes. make sure the value is a multiple of x_granularity" assert ( abs(int(y_idxs_per_step) - y_idxs_per_step) < 1e-5 ), "CMAP_STEP_SIZE_Y doesnt resolve into discrete indexes. make sure the value is a multiple of y_granularity" assert ( abs(int(x_idxs_per_step_on2) - x_idxs_per_step_on2) < 1e-5 ), "CMAP_STEP_SIZE_X /2 doesnt resolve into discrete indexes. make sure the value is a multiple of 2*x_granularity" assert ( abs(int(y_idxs_per_step_on2) - y_idxs_per_step_on2) < 1e-5 ), "CMAP_STEP_SIZE_Y/2 doesnt resolve into discrete indexes. make sure the value is a multiple of 2*y_granularity" x_idxs_per_step = int(x_idxs_per_step) y_idxs_per_step = int(y_idxs_per_step) x_idxs_per_step_on2 = int(x_idxs_per_step_on2) y_idxs_per_step_on2 = int(y_idxs_per_step_on2) x_idx = int(x_idxs_per_step_on2) y_idx = int(y_idxs_per_step_on2) # discontinuity_degree_threshold - slopes greater than this will be considered discontinuities and ignored by this heuristic. x_idxs_in_step_fn_search_area = int( parameters.CMAP_STEP_COSTFN_SEARCH_SIZE / self.hm_obj.x_granularity) y_idxs_in_step_fn_search_area = int( parameters.CMAP_STEP_COSTFN_SEARCH_SIZE / self.hm_obj.y_granularity) hm_np_arr = self.hm_obj.get_np_hm_array() # grad_x_np_arr = self.gradient_map.get_np_x_gradient() # grad_y_np_arr = self.gradient_map.get_np_y_gradient() np.set_printoptions(precision=4) # this should cover whole search area while x_idx < self.x_indices: while y_idx < self.y_indices: # world_x = self.hm_obj.get_x_from_xindex(x_idx) # world_y = self.hm_obj.get_y_from_yindex(y_idx) x0 = x_idx - x_idxs_in_step_fn_search_area xf = x_idx + x_idxs_in_step_fn_search_area y0 = y_idx - y_idxs_in_step_fn_search_area yf = y_idx + y_idxs_in_step_fn_search_area if x0 < 0: x0 = 0 if xf >= self.x_indices: xf = self.x_indices - 1 if y0 < 0: y0 = 0 if yf >= self.y_indices: yf = self.y_indices - 1 hm_sub_arr = hm_np_arr[x0:xf, y0:yf] # grad_x_sub_arr = grad_x_np_arr[x0:xf, y0:yf] # grad_y_sub_arr = grad_x_np_arr[x0:xf, y0:yf] slope_cost = 0 step_cost = 0 roughness_cost = 0 # _____ Slope Cost if not exlude_slope: x_grad, y_grad = self.gradient_map.get_grad_at_xy_idx( x_idx, y_idx) abs_x_slope_deg, abs_y_slope_deg = np.abs( MathUtils.gradient_to_slope_deg(x_grad)), np.abs( MathUtils.gradient_to_slope_deg(y_grad)) x_cost = abs_x_slope_deg - parameters.CMAP_MAX_NONPENALIZED_SLOPE y_cost = abs_y_slope_deg - parameters.CMAP_MAX_NONPENALIZED_SLOPE # Filter height map discontinuities and non penalized slopes if (abs_x_slope_deg > parameters. CMAP_SLOPE_HEURISTIC_MAX_CONSIDERED_DEGREE or abs_x_slope_deg < parameters.CMAP_MAX_NONPENALIZED_SLOPE): x_cost = 0 if (abs_y_slope_deg > parameters. CMAP_SLOPE_HEURISTIC_MAX_CONSIDERED_DEGREE or abs_y_slope_deg < parameters.CMAP_MAX_NONPENALIZED_SLOPE): y_cost = 0 slope_cost = parameters.CMAP_SLOPE_HEURISTIC_COEFF * ( x_cost + y_cost) / 2 # if c > 0: # print(f" {round(world_x, 2)}, {round(world_y, 2)}, abs slope x: {round(abs_x_slope_deg,2)}\t y: {round(abs_y_slope_deg,2)}") # _____ Step Cost if not exlude_step: max_dif = 0 for x_i in range(0, xf - x0): y_col = hm_sub_arr[x_i, :] if np.amax(np.abs(np.diff(y_col))) > max_dif: max_dif = np.amax(np.abs(np.diff(y_col))) for y_i in range(0, yf - y0): x_row = hm_sub_arr[:, y_i] if np.amax(np.abs(np.diff(x_row))) > max_dif: max_dif = np.amax(np.abs(np.diff(x_row))) if max_dif > parameters.CMAP_STEP_COSTFN_MIN_HEIGHT_DIF: step_cost = ( parameters.CMAP_STEP_COSTFN_BASELINE_COST + parameters.CMAP_STEP_COSTFN_DIF_SLOPE_COEFF * max_dif) # d = .025 # if np.abs(world_x - 5) < d and np.abs(world_y - 1.4) < d: # print("\n\n______________") # print("world x:", world_x) # print("world y:", world_y) # print("hm array:") # print(hm_sub_arr) # print("max_dif:",max_dif) # print("step_cost:", step_cost) # print(".CMAP_STEP_COSTFN_DIF_SLOPE_COEFF * max_dif: ", parameters.CMAP_STEP_COSTFN_DIF_SLOPE_COEFF * max_dif) # print("CMAP_STEP_COSTFN_BASELINE_COST * max_dif: ", parameters.CMAP_STEP_COSTFN_BASELINE_COST) # _____ Roughness Cost # 'The “roughness” of the location. A measure of the deviation of the surface from the fitted plane. # Computed by averaging the difference in height of each cell to the plane’s height at the that cell' # Quoted from "Navigation Planning for Legged Robots", Joel Chestnutt if not exlude_roughness: if not np.max(hm_sub_arr) == np.min( hm_sub_arr) and step_cost == 0: a, b, c, z0 = MathUtils.best_fitting_plane(hm_sub_arr) specified_plane = MathUtils.hm_specified_by_abc_z0( a, b, c, z0, hm_sub_arr.shape) mse = np.sum(np.abs(specified_plane - hm_sub_arr)) # normalized_mse = mse / (hm_sub_arr.shape[0] * hm_sub_arr.shape[1]) # roughness_cost = parameters.CMAP_ROUGHNESS_HEURISTIC_COEFF * normalized_mse roughness_cost = parameters.CMAP_ROUGHNESS_HEURISTIC_COEFF * mse # d = .025 # if np.abs(world_x - 5) < d and np.abs(world_y - .9) < d: # print("\n\n______________") # print("world x:", world_x) # print("world y:", world_y) # print("a:", round(a, 4)) # print("b:", round(b, 4)) # print("c:", round(c, 4)) # print("z0:", round(z0, 4)) # print("hm array:") # print(hm_sub_arr) # print("mse:",mse) # # print("normalized mse:", normalized_mse) # print("cost: ", roughness_cost) # # if np.abs(world_x - 4.25) < 2*d and np.abs(world_y - 1) < 2*d: # print("\n\n______________") # print("world x:", world_x) # print("world y:", world_y) # print("a:", round(a,4)) # print("b:", round(b,4)) # print("v:", round(c,4)) # print("z0:", round(z0,4)) # print("hm array:") # print(hm_sub_arr) # print("mse:",mse) # # print("normalized mse:", normalized_mse) # print("cost: ", roughness_cost) cost = slope_cost + step_cost + roughness_cost self.fs_costmap.np_cmap_arr[x_idx - x_idxs_per_step_on2:x_idx + x_idxs_per_step_on2, y_idx - y_idxs_per_step_on2:y_idx + y_idxs_per_step_on2, ] = cost y_idx += y_idxs_per_step y_idx = y_idxs_per_step_on2 x_idx += x_idxs_per_step if normalize_cost_arr: self.fs_costmap.normalize_cost_arr( parameters.CMAP_NORMALIZED_MAX_VALUE, debug=debug) self.fs_costmap.runtime = time.time() - start_t self.fs_costmap.failed = False if debug: print("footstep cost map built in:", time.time() - start_t, "s") return self.fs_costmap