Create Python wrapper for distance_spline.
Also reformatted libspline.py
Also made the constructor for Spline more useful.
Change-Id: I0e54a54df7c872ba0c06f9d6ad414d2880c157d7
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 62daca1..ca20f61 100644
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -1,5 +1,4 @@
#!/usr/bin/python
-
"""Wrapper around spline.h/cc through spline_array.cc."""
__author__ = 'Alex Perry (alex.perry96@gmail.com)'
@@ -11,22 +10,31 @@
libSpline = None
for path in os.environ.get('PYTHONPATH').split(':'):
try:
- libSpline = ct.cdll.LoadLibrary(os.path.join(path, 'frc971/control_loops/drivetrain/spline.so'))
+ libSpline = ct.cdll.LoadLibrary(
+ os.path.join(path, 'frc971/control_loops/drivetrain/spline.so'))
except OSError, e:
pass
# Define required output types.
-libSpline.Theta.restype = ct.c_double
-libSpline.DTheta.restype = ct.c_double
-libSpline.DDTheta.restype = ct.c_double
+libSpline.SplineTheta.restype = ct.c_double
+libSpline.SplineDTheta.restype = ct.c_double
+libSpline.SplineDDTheta.restype = ct.c_double
+libSpline.DistanceSplineTheta.restype = ct.c_double
+libSpline.DistanceSplineDTheta.restype = ct.c_double
+libSpline.DistanceSplineDThetaDt.restype = ct.c_double
+libSpline.DistanceSplineDDTheta.restype = ct.c_double
+libSpline.DistanceSplineLength.restype = ct.c_double
+
class Spline:
- """A wrapper around spline.h/cc through spline_array.cc."""
+ """A wrapper around spline.h/cc through libspline.cc."""
- def __init__(self):
- self.__points = np.zeros((2,6))
- self.__spline = libSpline.newSpline(np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1]))
+ def __init__(self, points):
+ assert points.shape == (2, 6)
+ self.__points = points
+ self.__spline = libSpline.NewSpline(
+ np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1]))
def __del__(self):
libSpline.deleteSpline(self.__spline)
@@ -35,37 +43,101 @@
self.__points[0, index] = x
self.__points[1, index] = y
libSpline.deleteSpline(self.__spline)
- self.__spline = libSpline.newSpline(np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1]))
+ self.__spline = libSpline.newSpline(
+ np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1]))
def Point(self, alpha):
result = np.zeros(2)
- libSpline.Point(self.__spline, ct.c_double(alpha), np.ctypeslib.as_ctypes(result))
+ libSpline.SplinePoint(self.__spline, ct.c_double(alpha),
+ np.ctypeslib.as_ctypes(result))
return result
def DPoint(self, alpha):
result = np.zeros(2)
- libSpline.DPoint(self.__spline, ct.c_double(alpha), np.ctypeslib.as_ctypes(result))
+ libSpline.SplineDPoint(self.__spline, ct.c_double(alpha),
+ np.ctypeslib.as_ctypes(result))
return result
def DDPoint(self, alpha):
result = np.zeros(2)
- libSpline.DDPoint(self.__spline, ct.c_double(alpha), np.ctypeslib.as_ctypes(result))
+ libSpline.SplineDDPoint(self.__spline, ct.c_double(alpha),
+ np.ctypeslib.as_ctypes(result))
return result
def DDDPoint(self, alpha):
result = np.zeros(2)
- libSpline.DDDPoint(self.__spline, ct.c_double(alpha), np.ctypeslib.as_ctypes(result))
+ libSpline.SplineDDDPoint(self.__spline, ct.c_double(alpha),
+ np.ctypeslib.as_ctypes(result))
return result
def Theta(self, alpha):
- return libSpline.Theta(self.__spline, ct.c_double(alpha))
+ return libSpline.SplineTheta(self.__spline, ct.c_double(alpha))
def DTheta(self, alpha):
- return libSpline.DTheta(self.__spline, ct.c_double(alpha))
+ return libSpline.SplineDTheta(self.__spline, ct.c_double(alpha))
def DDTheta(self, alpha):
- return libSpline.DDTheta(self.__spline, ct.c_double(alpha))
+ return libSpline.SplineDDTheta(self.__spline, ct.c_double(alpha))
def ControlPoints(self):
- return self.__points;
+ return self.__points
+
+ def Spline(self):
+ return self.__spline
+
+
+class DistanceSpline:
+ """A wrapper around distance_spline.h/cc through libdistancespline.cc."""
+
+ def __init__(self, splines):
+ self.__spline = None
+ spline_ptrs = []
+ for spline in splines:
+ spline_ptrs.append(spline.Spline())
+ spline_ptrs = np.array(spline_ptrs)
+
+ spline_array = np.ctypeslib.as_ctypes(spline_ptrs)
+ self.__spline = libSpline.NewDistanceSpline(
+ ct.byref(spline_array), len(splines))
+
+ def __del__(self):
+ libSpline.deleteDistanceSpline(self.__spline)
+
+ def XY(self, distance):
+ result = np.zeros(2)
+ libSpline.DistanceSplineXY(self.__spline, ct.c_double(distance),
+ np.ctypeslib.as_ctypes(result))
+ return result
+
+ def DXY(self, distance):
+ result = np.zeros(2)
+ libSpline.DistanceSplineDXY(self.__spline, ct.c_double(distance),
+ np.ctypeslib.as_ctypes(result))
+ return result
+
+ def DDXY(self, distance):
+ result = np.zeros(2)
+ libSpline.DistanceSplineDDXY(self.__spline, ct.c_double(distance),
+ np.ctypeslib.as_ctypes(result))
+ return result
+
+ def Theta(self, distance):
+ return libSpline.DistanceSplineTheta(self.__spline,
+ ct.c_double(distance))
+
+ def DTheta(self, distance):
+ return libSpline.DistanceSplineDTheta(self.__spline,
+ ct.c_double(distance))
+
+ def DThetaDt(self, distance, velocity):
+ return libSpline.DistanceSplineDThetaDt(self.__spline,
+ ct.c_double(distance),
+ ct.c_double(velocity))
+
+ def DDTheta(self, distance):
+ return libSpline.DistanceSplineDDTheta(self.__spline,
+ ct.c_double(distance))
+
+ def Length(self):
+ return libSpline.DistanceSplineLength(self.__spline)