fix some lgtm issues

Author: jhavl

roboticstoolbox/backends/PyPlot/EllipsePlot.py

@@ -197,7 +197,7 @@ def make_ellipsoid2(self):
 
         # points on unit circle
         theta = np.linspace(0.0, 2.0 * np.pi, 50)
-        y = np.array([np.cos(theta), np.sin(theta)])
+        # y = np.array([np.cos(theta), np.sin(theta)])
         # RVC2 p 602
         # x = sp.linalg.sqrtm(A) @ y
 

roboticstoolbox/examples/mobile.py

@@ -7,24 +7,24 @@
 
 ## simple automata
 
-from scipy.io import loadmat
-from roboticstoolbox import Bug2, DXform
+# from scipy.io import loadmat
+# from roboticstoolbox import Bug2, DXform
 
-vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/house.mat", squeeze_me=True, struct_as_record=False)
-house = vars['house']
-place = vars['place']
+# vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/house.mat", squeeze_me=True, struct_as_record=False)
+# house = vars['house']
+# place = vars['place']
 # bug = Bug2(house)
 # p = bug.query(place.br3, place.kitchen, animate=True)
 
-vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/map1.mat", squeeze_me=True, struct_as_record=False)
-map = vars['map']
-bug = Bug2(map)
+# vars = loadmat("/Users/corkep/code/robotics-toolbox-python/data/map1.mat", squeeze_me=True, struct_as_record=False)
+# map = vars['map']
+# bug = Bug2(map)
 # # bug.plot()
 # p = bug.query([20, 10], [50, 35], animate=True)
 # print(p)
 # p = bug.query(place.br3, place.kitchen)
 # about(p)
-# p = bug.query([], place.kitchen) 
+# p = bug.query([], place.kitchen)
 
 # bug = Bug2(house), inflate=7)
 # p = bug.query(place.br3, place.kitchen, animate=False)
@@ -38,15 +38,15 @@
 
 ## map based planning
 
-dx = DXform(house)
-dx.plan(place.kitchen)
+# dx = DXform(house)
+# dx.plan(place.kitchen)
 
-dx.plot()
+# dx.plot()
 
-p = dx.query(place.br3) #, animate=True)
-print(p)
+# p = dx.query(place.br3) #, animate=True)
+# print(p)
 
-dx.plot(path=p, block=True)
+# dx.plot(path=p, block=True)
 
 # p = dx.query(place.br3)
 

roboticstoolbox/examples/vehicle1.py

@@ -7,28 +7,28 @@
 # v = VehiclePolygon()
 anim = VehicleIcon('greycar', scale=2)
 
-veh = Bicycle(
-    animation=anim,
-    control=RandomPath(
-        dim=dim),
-    dim=dim,
-    verbose=False)
-print(veh)
+# veh = Bicycle(
+#     animation=anim,
+#     control=RandomPath(
+#         dim=dim),
+#     dim=dim,
+#     verbose=False)
+# print(veh)
 
-# odo = veh.step(1, 0.3)
-# print(odo)
+# # odo = veh.step(1, 0.3)
+# # print(odo)
 
-# print(veh.x)
+# # print(veh.x)
 
-# print(veh.f([0, 0, 0], odo))
+# # print(veh.f([0, 0, 0], odo))
 
-# def control(v, t, x):
-#     goal = (6,6)
-#     goal_heading = atan2(goal[1]-x[1], goal[0]-x[0])
-#     d_heading = base.angdiff(goal_heading, x[2])
-#     v.stopif(base.norm(x[0:2] - goal) < 0.1)
+# # def control(v, t, x):
+# #     goal = (6,6)
+# #     goal_heading = atan2(goal[1]-x[1], goal[0]-x[0])
+# #     d_heading = base.angdiff(goal_heading, x[2])
+# #     v.stopif(base.norm(x[0:2] - goal) < 0.1)
 
-#     return (1, d_heading)
+# #     return (1, d_heading)
 
-p = veh.run(1000, plot=True)
-print(p)
+# p = veh.run(1000, plot=True)
+# print(p)

roboticstoolbox/mobile/CurvaturePolyPlanner.py

@@ -1,4 +1,3 @@
-
 import math
 import scipy.integrate
 import scipy.optimize
@@ -7,6 +6,7 @@
 from collections import namedtuple
 from roboticstoolbox.mobile import *
 
+
 def solvepath(x, q0=[0, 0, 0], **kwargs):
     # x[:4] is 4 coeffs of curvature polynomial
     # x[4] is total path length
@@ -16,7 +16,7 @@ def solvepath(x, q0=[0, 0, 0], **kwargs):
     def dotfunc(s, q, poly):
         # q = (x, y, θ)
         # qdot = (cosθ, sinθ, ϰ)
-        k = poly[0] * s ** 3 + poly[1] * s ** 2 + poly[2] * s + poly[3]
+        k = poly[0] * s**3 + poly[1] * s**2 + poly[2] * s + poly[3]
         # k = ((poly[0] * s + poly[1]) * s  + poly[2]) * s + poly[3]
 
         # save maximum curvature for this path solution
@@ -31,11 +31,13 @@ def dotfunc(s, q, poly):
     sol = scipy.integrate.solve_ivp(dotfunc, [0, s_f], q0, args=(cpoly,), **kwargs)
     return sol.y, maxcurvature
 
+
 def xcurvature(x):
     # inequality constraint function, must be non-negative
     _, maxcurvature = solvepath(x, q0=(0, 0, 0))
     return maxcurvature
 
+
 def costfunc(x, start, goal):
     # final cost of path from start with params
     # p[0:4] is polynomial: k0, a, b, c
@@ -49,8 +51,9 @@ def costfunc(x, start, goal):
     e = np.linalg.norm(path[:, -1] - np.r_[goal])
 
     return e
-class CurvaturePolyPlanner(PlannerBase):
 
+
+class CurvaturePolyPlanner(PlannerBase):
     def __init__(self, curvature=None):
         """
         Continuous curvature path planner
@@ -81,7 +84,6 @@ def __init__(self, curvature=None):
         super().__init__(ndims=3)
         self.curvature = curvature
 
-
     def query(self, start, goal):
         r"""
         Find a path betwee two configurations
@@ -113,41 +115,51 @@ def query(self, start, goal):
         self._start = start
         self._goal = goal
 
-        # initial estimate of path length is Euclidean distance 
+        # initial estimate of path length is Euclidean distance
         d = np.linalg.norm(goal[:2] - start[:2])
         # state vector is kappa_0, a, b, c, s_f
 
         if self.curvature is not None:
-            nlcontraints = (scipy.optimize.NonlinearConstraint(xcurvature, 0, self.curvature),)
+            nlcontraints = (
+                scipy.optimize.NonlinearConstraint(xcurvature, 0, self.curvature),
+            )
         else:
             nlcontraints = ()
 
-        sol = scipy.optimize.minimize(costfunc, [0, 0, 0, 0, d],
+        sol = scipy.optimize.minimize(
+            costfunc,
+            [0, 0, 0, 0, d],
             constraints=nlcontraints,
             bounds=[(None, None), (None, None), (None, None), (None, None), (d, None)],
-            args=(start, goal))
+            args=(start, goal),
+        )
         print(sol)
-        path, maxcurvature = solvepath(sol.x, q0=start, dense_output=True, max_step = 1e-2)
+        path, maxcurvature = solvepath(
+            sol.x, q0=start, dense_output=True, max_step=1e-2
+        )
 
-        status = namedtuple('CurvaturePolyStatus', ['length', 'maxcurvature', 'poly'])(sol.x[4], maxcurvature, sol.x[:4])
+        status = namedtuple("CurvaturePolyStatus", ["length", "maxcurvature", "poly"])(
+            sol.x[4], maxcurvature, sol.x[:4]
+        )
 
         return path.T, status
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     from math import pi
 
     # start = (1, 1, pi/4)
     # goal = (-3, -3, -pi/4)
-    start = (0, 0, -pi/4)
-    goal = (1, 2, pi/4)
+    # start = (0, 0, -pi/4)
+    # goal = (1, 2, pi/4)
 
-    start = (0, 0, pi/2)
-    goal = (1, 0, pi/2)
+    start = (0, 0, pi / 2)
+    goal = (1, 0, pi / 2)
 
     planner = CurvaturePolyPlanner()
     path, status = planner.query(start, goal)
-    print('start', path[0,:])
-    print('goal', path[-1, :])
+    print("start", path[0, :])
+    print("goal", path[-1, :])
 
     print(status)
     planner.plot(path, block=True)
@@ -163,4 +175,4 @@ def query(self, start, goal):
     #     c[i] /= sf ** (3-i)
 
     # print(solvepath(np.r_[c, 1], start))
-    # print(c)
+    # print(c)