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