Skip to content

Commit 5e19e80

Browse files
cognitiveRobotcognitiveRobot
committed
Merge remote-tracking branch 'upstream/master'
2 parents 2bf72d4 + 135e33a commit 5e19e80

File tree

98 files changed

+2025
-1370
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

98 files changed

+2025
-1370
lines changed

‎.lgtm.yml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
extraction:
2+
python:
3+
python_setup:
4+
version: 3

‎.travis.yml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ before_install:
2626

2727

2828
install:
29-
- conda install numpy
29+
- conda install numpy==1.15
3030
- conda install scipy
3131
- conda install matplotlib
3232
- conda install pandas

‎AerialNavigation/drone_3d_trajectory_following/Quadrotor.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,12 +51,12 @@ def transformation_matrix(self):
5151
yaw = self.yaw
5252
return np.array(
5353
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
54-
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
55-
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
54+
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch)
55+
* sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
5656
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
5757
])
5858

59-
def plot(self):
59+
def plot(self): # pragma: no cover
6060
T = self.transformation_matrix()
6161

6262
p1_t = np.matmul(T, self.p1)

‎AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -73,8 +73,8 @@ def quad_sim(x_c, y_c, z_c):
7373
# des_x_pos = calculate_position(x_c[i], t)
7474
# des_y_pos = calculate_position(y_c[i], t)
7575
des_z_pos = calculate_position(z_c[i], t)
76-
des_x_vel = calculate_velocity(x_c[i], t)
77-
des_y_vel = calculate_velocity(y_c[i], t)
76+
# des_x_vel = calculate_velocity(x_c[i], t)
77+
# des_y_vel = calculate_velocity(y_c[i], t)
7878
des_z_vel = calculate_velocity(z_c[i], t)
7979
des_x_acc = calculate_acceleration(x_c[i], t)
8080
des_y_acc = calculate_acceleration(y_c[i], t)

‎AerialNavigation/rocket_powered_landing/rocket_powered_landing.py‎

Lines changed: 58 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
88
Ref:
99
- Python implementation of 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper
10-
by Michael Szmuk and Behçet Açıkmeşe.
10+
by Michael Szmuk and Behcet Acıkmese.
1111
1212
- EmbersArc/SuccessiveConvexificationFreeFinalTime: Implementation of "Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time" https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime
1313
@@ -129,12 +129,12 @@ def f_func(self, x, u):
129129
[vx],
130130
[vy],
131131
[vz],
132-
[(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy *
133-
(q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m],
134-
[(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2 +
135-
2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m],
136-
[(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy *
137-
(q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m],
132+
[(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy
133+
* (q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m],
134+
[(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2
135+
+ 2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m],
136+
[(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy
137+
* (q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m],
138138
[-0.5 * q1 * wx - 0.5 * q2 * wy - 0.5 * q3 * wz],
139139
[0.5 * q0 * wx + 0.5 * q2 * wz - 0.5 * q3 * wy],
140140
[0.5 * q0 * wy - 0.5 * q1 * wz + 0.5 * q3 * wx],
@@ -154,20 +154,20 @@ def A_func(self, x, u):
154154
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
155155
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
156156
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
157-
[(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz -
158-
q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0],
159-
[(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz +
160-
q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0],
161-
[(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy -
162-
q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0],
163-
[0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy, -
164-
0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3],
165-
[0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz, -
166-
0.5 * wy, 0.5 * q0, -0.5 * q3, 0.5 * q2],
157+
[(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz
158+
- q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0],
159+
[(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz
160+
+ q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0],
161+
[(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy
162+
- q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0],
163+
[0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy,
164+
- 0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3],
165+
[0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz,
166+
- 0.5 * wy, 0.5 * q0, -0.5 * q3, 0.5 * q2],
167167
[0, 0, 0, 0, 0, 0, 0, 0.5 * wy, -0.5 * wz, 0,
168168
0.5 * wx, 0.5 * q3, 0.5 * q0, -0.5 * q1],
169-
[0, 0, 0, 0, 0, 0, 0, 0.5 * wz, 0.5 * wy, -
170-
0.5 * wx, 0, -0.5 * q2, 0.5 * q1, 0.5 * q0],
169+
[0, 0, 0, 0, 0, 0, 0, 0.5 * wz, 0.5 * wy,
170+
- 0.5 * wx, 0, -0.5 * q2, 0.5 * q1, 0.5 * q0],
171171
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
172172
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
173173
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
@@ -184,12 +184,12 @@ def B_func(self, x, u):
184184
[0, 0, 0],
185185
[0, 0, 0],
186186
[0, 0, 0],
187-
[(-2 * q2**2 - 2 * q3**2 + 1) / m, 2 *
188-
(-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m],
189-
[2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2 *
190-
q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m],
191-
[2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3) /
192-
m, (-2 * q1**2 - 2 * q2**2 + 1) / m],
187+
[(-2 * q2**2 - 2 * q3**2 + 1) / m, 2
188+
* (-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m],
189+
[2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2
190+
* q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m],
191+
[2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3)
192+
/ m, (-2 * q1**2 - 2 * q2**2 + 1) / m],
193193
[0, 0, 0],
194194
[0, 0, 0],
195195
[0, 0, 0],
@@ -227,12 +227,12 @@ def skew(self, v):
227227

228228
def dir_cosine(self, q):
229229
return np.matrix([
230-
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] +
231-
q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
232-
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 *
233-
(q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
234-
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] -
235-
q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
230+
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2]
231+
+ q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
232+
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2
233+
* (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
234+
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3]
235+
- q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
236236
])
237237

238238
def omega(self, w):
@@ -460,15 +460,15 @@ def __init__(self, m, K):
460460
# x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu
461461
constraints += [
462462
self.var['X'][:, k + 1] ==
463-
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x))
464-
* self.var['X'][:, k]
465-
+ cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u))
466-
* self.var['U'][:, k]
467-
+ cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u))
468-
* self.var['U'][:, k + 1]
469-
+ self.par['S_bar'][:, k] * self.var['sigma']
470-
+ self.par['z_bar'][:, k]
471-
+ self.var['nu'][:, k]
463+
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) *
464+
self.var['X'][:, k] +
465+
cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) *
466+
self.var['U'][:, k] +
467+
cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) *
468+
self.var['U'][:, k + 1] +
469+
self.par['S_bar'][:, k] * self.var['sigma'] +
470+
self.par['z_bar'][:, k] +
471+
self.var['nu'][:, k]
472472
for k in range(K - 1)
473473
]
474474

@@ -486,10 +486,10 @@ def __init__(self, m, K):
486486

487487
# Objective:
488488
sc_objective = cvxpy.Minimize(
489-
self.par['weight_sigma'] * self.var['sigma']
490-
+ self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf')
491-
+ self.par['weight_delta'] * self.var['delta_norm']
492-
+ self.par['weight_delta_sigma'] * self.var['sigma_norm']
489+
self.par['weight_sigma'] * self.var['sigma'] +
490+
self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf') +
491+
self.par['weight_delta'] * self.var['delta_norm'] +
492+
self.par['weight_delta_sigma'] * self.var['sigma_norm']
493493
)
494494

495495
objective = sc_objective
@@ -550,20 +550,20 @@ def solve(self, **kwargs):
550550

551551
def axis3d_equal(X, Y, Z, ax):
552552

553-
max_range = np.array([X.max() - X.min(), Y.max() -
554-
Y.min(), Z.max() - Z.min()]).max()
555-
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
556-
1:2:2][0].flatten() + 0.5 * (X.max() + X.min())
557-
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
558-
1:2:2][1].flatten() + 0.5 * (Y.max() + Y.min())
559-
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
560-
1:2:2][2].flatten() + 0.5 * (Z.max() + Z.min())
553+
max_range = np.array([X.max() - X.min(), Y.max()
554+
- Y.min(), Z.max() - Z.min()]).max()
555+
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
556+
- 1:2:2][0].flatten() + 0.5 * (X.max() + X.min())
557+
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
558+
- 1:2:2][1].flatten() + 0.5 * (Y.max() + Y.min())
559+
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
560+
- 1:2:2][2].flatten() + 0.5 * (Z.max() + Z.min())
561561
# Comment or uncomment following both lines to test the fake bounding box:
562562
for xb, yb, zb in zip(Xb, Yb, Zb):
563563
ax.plot([xb], [yb], [zb], 'w')
564564

565565

566-
def plot_animation(X, U):
566+
def plot_animation(X, U): # pragma: no cover
567567

568568
fig = plt.figure()
569569
ax = fig.gca(projection='3d')
@@ -576,14 +576,14 @@ def plot_animation(X, U):
576576
axis3d_equal(X[2, :], X[3, :], X[1, :], ax)
577577

578578
rx, ry, rz = X[1:4, k]
579-
vx, vy, vz = X[4:7, k]
579+
# vx, vy, vz = X[4:7, k]
580580
qw, qx, qy, qz = X[7:11, k]
581581

582582
CBI = np.array([
583583
[1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz),
584584
2 * (qx * qz - qw * qy)],
585-
[2 * (qx * qy - qw * qz), 1 - 2 *
586-
(qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
585+
[2 * (qx * qy - qw * qz), 1 - 2
586+
* (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
587587
[2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx),
588588
1 - 2 * (qx ** 2 + qy ** 2)]
589589
])
@@ -618,8 +618,6 @@ def main():
618618
integrator = Integrator(m, K)
619619
problem = SCProblem(m, K)
620620

621-
last_linear_cost = None
622-
623621
converged = False
624622
w_delta = W_DELTA
625623
for it in range(iterations):
@@ -633,7 +631,7 @@ def main():
633631
X_last=X, U_last=U, sigma_last=sigma,
634632
weight_sigma=W_SIGMA, weight_nu=W_NU,
635633
weight_delta=w_delta, weight_delta_sigma=W_DELTA_SIGMA)
636-
info = problem.solve()
634+
problem.solve()
637635

638636
X = problem.get_variable('X')
639637
U = problem.get_variable('U')
@@ -658,7 +656,7 @@ def main():
658656
print(f'Converged after {it + 1} iterations.')
659657
break
660658

661-
if show_animation:
659+
if show_animation: # pragma: no cover
662660
plot_animation(X, U)
663661

664662
print("done!!")

‎ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py‎

Lines changed: 47 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -60,8 +60,8 @@ def detect_collision(line_seg, circle):
6060
closest_point = a_vec + line_vec * proj / line_mag
6161
if np.linalg.norm(closest_point - c_vec) > radius:
6262
return False
63-
else:
64-
return True
63+
64+
return True
6565

6666

6767
def get_occupancy_grid(arm, obstacles):
@@ -74,7 +74,7 @@ def get_occupancy_grid(arm, obstacles):
7474
Args:
7575
arm: An instance of NLinkArm
7676
obstacles: A list of obstacles, with each obstacle defined as a list
77-
of xy coordinates and a radius.
77+
of xy coordinates and a radius.
7878
7979
Returns:
8080
Occupancy grid in joint space
@@ -120,16 +120,7 @@ def astar_torus(grid, start_node, goal_node):
120120

121121
parent_map = [[() for _ in range(M)] for _ in range(M)]
122122

123-
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
124-
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
125-
for i in range(heuristic_map.shape[0]):
126-
for j in range(heuristic_map.shape[1]):
127-
heuristic_map[i, j] = min(heuristic_map[i, j],
128-
i + 1 + heuristic_map[M - 1, j],
129-
M - i + heuristic_map[0, j],
130-
j + 1 + heuristic_map[i, M - 1],
131-
M - j + heuristic_map[i, 0]
132-
)
123+
heuristic_map = calc_heuristic_map(M, goal_node)
133124

134125
explored_heuristic_map = np.full((M, M), np.inf)
135126
distance_map = np.full((M, M), np.inf)
@@ -150,46 +141,21 @@ def astar_torus(grid, start_node, goal_node):
150141

151142
i, j = current_node[0], current_node[1]
152143

153-
neighbors = []
154-
if i - 1 >= 0:
155-
neighbors.append((i - 1, j))
156-
else:
157-
neighbors.append((M - 1, j))
158-
159-
if i + 1 < M:
160-
neighbors.append((i + 1, j))
161-
else:
162-
neighbors.append((0, j))
163-
164-
if j - 1 >= 0:
165-
neighbors.append((i, j - 1))
166-
else:
167-
neighbors.append((i, M - 1))
168-
169-
if j + 1 < M:
170-
neighbors.append((i, j + 1))
171-
else:
172-
neighbors.append((i, 0))
144+
neighbors = find_neighbors(i, j)
173145

174146
for neighbor in neighbors:
175147
if grid[neighbor] == 0 or grid[neighbor] == 5:
176148
distance_map[neighbor] = distance_map[current_node] + 1
177149
explored_heuristic_map[neighbor] = heuristic_map[neighbor]
178150
parent_map[neighbor[0]][neighbor[1]] = current_node
179151
grid[neighbor] = 3
180-
'''
181-
plt.cla()
182-
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
183-
plt.show()
184-
plt.pause(1e-5)
185-
'''
186152

187153
if np.isinf(explored_heuristic_map[goal_node]):
188154
route = []
189155
print("No route found.")
190156
else:
191157
route = [goal_node]
192-
while parent_map[route[0][0]][route[0][1]] is not ():
158+
while parent_map[route[0][0]][route[0][1]] != ():
193159
route.insert(0, parent_map[route[0][0]][route[0][1]])
194160

195161
print("The route found covers %d grid cells." % len(route))
@@ -203,6 +169,46 @@ def astar_torus(grid, start_node, goal_node):
203169
return route
204170

205171

172+
def find_neighbors(i, j):
173+
neighbors = []
174+
if i - 1 >= 0:
175+
neighbors.append((i - 1, j))
176+
else:
177+
neighbors.append((M - 1, j))
178+
179+
if i + 1 < M:
180+
neighbors.append((i + 1, j))
181+
else:
182+
neighbors.append((0, j))
183+
184+
if j - 1 >= 0:
185+
neighbors.append((i, j - 1))
186+
else:
187+
neighbors.append((i, M - 1))
188+
189+
if j + 1 < M:
190+
neighbors.append((i, j + 1))
191+
else:
192+
neighbors.append((i, 0))
193+
194+
return neighbors
195+
196+
197+
def calc_heuristic_map(M, goal_node):
198+
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
199+
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
200+
for i in range(heuristic_map.shape[0]):
201+
for j in range(heuristic_map.shape[1]):
202+
heuristic_map[i, j] = min(heuristic_map[i, j],
203+
i + 1 + heuristic_map[M - 1, j],
204+
M - i + heuristic_map[0, j],
205+
j + 1 + heuristic_map[i, M - 1],
206+
M - j + heuristic_map[i, 0]
207+
)
208+
209+
return heuristic_map
210+
211+
206212
class NLinkArm(object):
207213
"""
208214
Class for controlling and plotting a planar arm with an arbitrary number of links.
@@ -235,7 +241,7 @@ def update_points(self):
235241

236242
self.end_effector = np.array(self.points[self.n_links]).T
237243

238-
def plot(self, obstacles=[]):
244+
def plot(self, obstacles=[]): # pragma: no cover
239245
plt.cla()
240246

241247
for obstacle in obstacles:

0 commit comments

Comments
 (0)