Skip to content

Commit ca7cd40

Browse files
jhavljhavl
committed
fix some lgtm issues
1 parent 3eafbe0 commit ca7cd40

File tree

13 files changed

+649
-633
lines changed

13 files changed

+649
-633
lines changed

‎roboticstoolbox/backends/PyPlot/EllipsePlot.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -197,7 +197,7 @@ def make_ellipsoid2(self):
197197

198198
# points on unit circle
199199
theta = np.linspace(0.0, 2.0 * np.pi, 50)
200-
y = np.array([np.cos(theta), np.sin(theta)])
200+
# y = np.array([np.cos(theta), np.sin(theta)])
201201
# RVC2 p 602
202202
# x = sp.linalg.sqrtm(A) @ y
203203

‎roboticstoolbox/examples/mobile.py

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -7,24 +7,24 @@
77

88
## simple automata
99

10-
from scipy.io import loadmat
11-
from roboticstoolbox import Bug2, DXform
10+
# from scipy.io import loadmat
11+
# from roboticstoolbox import Bug2, DXform
1212

13-
vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/house.mat", squeeze_me=True, struct_as_record=False)
14-
house = vars['house']
15-
place = vars['place']
13+
# vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/house.mat", squeeze_me=True, struct_as_record=False)
14+
# house = vars['house']
15+
# place = vars['place']
1616
# bug = Bug2(house)
1717
# p = bug.query(place.br3, place.kitchen, animate=True)
1818

19-
vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/map1.mat", squeeze_me=True, struct_as_record=False)
20-
map = vars['map']
21-
bug = Bug2(map)
19+
# vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/map1.mat", squeeze_me=True, struct_as_record=False)
20+
# map = vars['map']
21+
# bug = Bug2(map)
2222
# # bug.plot()
2323
# p = bug.query([20, 10], [50, 35], animate=True)
2424
# print(p)
2525
# p = bug.query(place.br3, place.kitchen)
2626
# about(p)
27-
# p = bug.query([], place.kitchen)
27+
# p = bug.query([], place.kitchen)
2828

2929
# bug = Bug2(house), inflate=7)
3030
# p = bug.query(place.br3, place.kitchen, animate=False)
@@ -38,15 +38,15 @@
3838

3939
## map based planning
4040

41-
dx = DXform(house)
42-
dx.plan(place.kitchen)
41+
# dx = DXform(house)
42+
# dx.plan(place.kitchen)
4343

44-
dx.plot()
44+
# dx.plot()
4545

46-
p = dx.query(place.br3) #, animate=True)
47-
print(p)
46+
# p = dx.query(place.br3) #, animate=True)
47+
# print(p)
4848

49-
dx.plot(path=p, block=True)
49+
# dx.plot(path=p, block=True)
5050

5151
# p = dx.query(place.br3)
5252

‎roboticstoolbox/examples/vehicle1.py

Lines changed: 19 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -7,28 +7,28 @@
77
# v = VehiclePolygon()
88
anim = VehicleIcon('greycar', scale=2)
99

10-
veh = Bicycle(
11-
animation=anim,
12-
control=RandomPath(
13-
dim=dim),
14-
dim=dim,
15-
verbose=False)
16-
print(veh)
10+
# veh = Bicycle(
11+
# animation=anim,
12+
# control=RandomPath(
13+
# dim=dim),
14+
# dim=dim,
15+
# verbose=False)
16+
# print(veh)
1717

18-
# odo = veh.step(1, 0.3)
19-
# print(odo)
18+
# # odo = veh.step(1, 0.3)
19+
# # print(odo)
2020

21-
# print(veh.x)
21+
# # print(veh.x)
2222

23-
# print(veh.f([0, 0, 0], odo))
23+
# # print(veh.f([0, 0, 0], odo))
2424

25-
# def control(v, t, x):
26-
# goal = (6,6)
27-
# goal_heading = atan2(goal[1]-x[1], goal[0]-x[0])
28-
# d_heading = base.angdiff(goal_heading, x[2])
29-
# v.stopif(base.norm(x[0:2] - goal) < 0.1)
25+
# # def control(v, t, x):
26+
# # goal = (6,6)
27+
# # goal_heading = atan2(goal[1]-x[1], goal[0]-x[0])
28+
# # d_heading = base.angdiff(goal_heading, x[2])
29+
# # v.stopif(base.norm(x[0:2] - goal) < 0.1)
3030

31-
# return (1, d_heading)
31+
# # return (1, d_heading)
3232

33-
p = veh.run(1000, plot=True)
34-
print(p)
33+
# p = veh.run(1000, plot=True)
34+
# print(p)

‎roboticstoolbox/mobile/CurvaturePolyPlanner.py

Lines changed: 30 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,3 @@
1-
21
import math
32
import scipy.integrate
43
import scipy.optimize
@@ -7,6 +6,7 @@
76
from collections import namedtuple
87
from roboticstoolbox.mobile import *
98

9+
1010
def solvepath(x, q0=[0, 0, 0], **kwargs):
1111
# x[:4] is 4 coeffs of curvature polynomial
1212
# x[4] is total path length
@@ -16,7 +16,7 @@ def solvepath(x, q0=[0, 0, 0], **kwargs):
1616
def dotfunc(s, q, poly):
1717
# q = (x, y, θ)
1818
# qdot = (cosθ, sinθ, ϰ)
19-
k = poly[0] * s ** 3 + poly[1] * s ** 2 + poly[2] * s + poly[3]
19+
k = poly[0] * s**3 + poly[1] * s**2 + poly[2] * s + poly[3]
2020
# k = ((poly[0] * s + poly[1]) * s + poly[2]) * s + poly[3]
2121

2222
# save maximum curvature for this path solution
@@ -31,11 +31,13 @@ def dotfunc(s, q, poly):
3131
sol = scipy.integrate.solve_ivp(dotfunc, [0, s_f], q0, args=(cpoly,), **kwargs)
3232
return sol.y, maxcurvature
3333

34+
3435
def xcurvature(x):
3536
# inequality constraint function, must be non-negative
3637
_, maxcurvature = solvepath(x, q0=(0, 0, 0))
3738
return maxcurvature
3839

40+
3941
def costfunc(x, start, goal):
4042
# final cost of path from start with params
4143
# p[0:4] is polynomial: k0, a, b, c
@@ -49,8 +51,9 @@ def costfunc(x, start, goal):
4951
e = np.linalg.norm(path[:, -1] - np.r_[goal])
5052

5153
return e
52-
class CurvaturePolyPlanner(PlannerBase):
5354

55+
56+
class CurvaturePolyPlanner(PlannerBase):
5457
def __init__(self, curvature=None):
5558
"""
5659
Continuous curvature path planner
@@ -81,7 +84,6 @@ def __init__(self, curvature=None):
8184
super().__init__(ndims=3)
8285
self.curvature = curvature
8386

84-
8587
def query(self, start, goal):
8688
r"""
8789
Find a path betwee two configurations
@@ -113,41 +115,51 @@ def query(self, start, goal):
113115
self._start = start
114116
self._goal = goal
115117

116-
# initial estimate of path length is Euclidean distance
118+
# initial estimate of path length is Euclidean distance
117119
d = np.linalg.norm(goal[:2] - start[:2])
118120
# state vector is kappa_0, a, b, c, s_f
119121

120122
if self.curvature is not None:
121-
nlcontraints = (scipy.optimize.NonlinearConstraint(xcurvature, 0, self.curvature),)
123+
nlcontraints = (
124+
scipy.optimize.NonlinearConstraint(xcurvature, 0, self.curvature),
125+
)
122126
else:
123127
nlcontraints = ()
124128

125-
sol = scipy.optimize.minimize(costfunc, [0, 0, 0, 0, d],
129+
sol = scipy.optimize.minimize(
130+
costfunc,
131+
[0, 0, 0, 0, d],
126132
constraints=nlcontraints,
127133
bounds=[(None, None), (None, None), (None, None), (None, None), (d, None)],
128-
args=(start, goal))
134+
args=(start, goal),
135+
)
129136
print(sol)
130-
path, maxcurvature = solvepath(sol.x, q0=start, dense_output=True, max_step = 1e-2)
137+
path, maxcurvature = solvepath(
138+
sol.x, q0=start, dense_output=True, max_step=1e-2
139+
)
131140

132-
status = namedtuple('CurvaturePolyStatus', ['length', 'maxcurvature', 'poly'])(sol.x[4], maxcurvature, sol.x[:4])
141+
status = namedtuple("CurvaturePolyStatus", ["length", "maxcurvature", "poly"])(
142+
sol.x[4], maxcurvature, sol.x[:4]
143+
)
133144

134145
return path.T, status
135146

136-
if __name__ == '__main__':
147+
148+
if __name__ == "__main__":
137149
from math import pi
138150

139151
# start = (1, 1, pi/4)
140152
# goal = (-3, -3, -pi/4)
141-
start = (0, 0, -pi/4)
142-
goal = (1, 2, pi/4)
153+
# start = (0, 0, -pi/4)
154+
# goal = (1, 2, pi/4)
143155

144-
start = (0, 0, pi/2)
145-
goal = (1, 0, pi/2)
156+
start = (0, 0, pi / 2)
157+
goal = (1, 0, pi / 2)
146158

147159
planner = CurvaturePolyPlanner()
148160
path, status = planner.query(start, goal)
149-
print('start', path[0,:])
150-
print('goal', path[-1, :])
161+
print("start", path[0, :])
162+
print("goal", path[-1, :])
151163

152164
print(status)
153165
planner.plot(path, block=True)
@@ -163,4 +175,4 @@ def query(self, start, goal):
163175
# c[i] /= sf ** (3-i)
164176

165177
# print(solvepath(np.r_[c, 1], start))
166-
# print(c)
178+
# print(c)

0 commit comments

Comments
 (0)