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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy