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/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index c3ea79e..8f98953 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -268,6 +268,17 @@
],
)
+cc_binary(
+ name = "spline.so",
+ srcs = ["libspline.cc"],
+ deps = [
+ ":distance_spline",
+ ":spline",
+ "//third_party/eigen",
+ ],
+ linkshared=True,
+)
+
cc_test(
name = "spline_test",
srcs = [
@@ -294,16 +305,6 @@
],
)
-cc_binary(
- name = "spline.so",
- srcs = ["libspline.cc"],
- deps = [
- ":spline",
- "//third_party/eigen",
- ],
- linkshared=1,
-)
-
cc_test(
name = "distance_spline_test",
srcs = [
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index f53dd23..117290d 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -1,7 +1,8 @@
-#include <string.h>
+#include <vector>
#include "Eigen/Dense"
+#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/spline.h"
namespace frc971 {
@@ -9,7 +10,7 @@
namespace drivetrain {
extern "C" {
- NSpline<6>* newSpline(double x[6], double y[6]) {
+ NSpline<6>* NewSpline(double x[6], double y[6]) {
return new NSpline<6>((::Eigen::Matrix<double, 2, 6>() << x[0], x[1], x[2],
x[3], x[4], x[5], y[0], y[1], y[2], y[3], y[4],
y[5]).finished());
@@ -19,43 +20,43 @@
delete spline;
}
- void Point(NSpline<6>* spline, double alpha, double* res) {
+ void SplinePoint(NSpline<6>* spline, double alpha, double* res) {
double* val = spline->Point(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void DPoint(NSpline<6>* spline, double alpha, double* res) {
+ void SplineDPoint(NSpline<6>* spline, double alpha, double* res) {
double* val = spline->DPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void DDPoint(NSpline<6>* spline, double alpha, double* res) {
+ void SplineDDPoint(NSpline<6>* spline, double alpha, double* res) {
double* val = spline->DDPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void DDDPoint(NSpline<6>* spline, double alpha, double* res) {
+ void SplineDDDPoint(NSpline<6>* spline, double alpha, double* res) {
double* val = spline->DDDPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- double Theta(NSpline<6>* spline, double alpha) {
+ double SplineTheta(NSpline<6>* spline, double alpha) {
return spline->Theta(alpha);
}
- double DTheta(NSpline<6>* spline, double alpha) {
+ double SplineDTheta(NSpline<6>* spline, double alpha) {
return spline->DTheta(alpha);
}
- double DDTheta(NSpline<6>* spline, double alpha) {
+ double SplineDDTheta(NSpline<6>* spline, double alpha) {
return spline->DDTheta(alpha);
}
- void control_points(NSpline<6>* spline, double* x, double* y) {
+ void SplineControlPoints(NSpline<6>* spline, double* x, double* y) {
auto points = spline->control_points();
// Deal with incorrectly strided matrix.
for (int i = 0; i < 6; ++i) {
@@ -63,6 +64,58 @@
y[i] = points(1, i);
}
}
+
+ DistanceSpline* NewDistanceSpline(Spline** splines, int count) {
+ ::std::vector<Spline> splines_;
+ for (int i = 0; i < count; ++i) {
+ splines_.push_back(*splines[i]);
+ }
+ return new DistanceSpline(::std::vector<Spline>(splines_));
+ }
+
+ void deleteDistanceSpline(DistanceSpline* spline) {
+ delete spline;
+ }
+
+ void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
+ double *val = spline->XY(distance).data();
+ res[0] = val[0];
+ res[1] = val[1];
+ }
+
+ void DistanceSplineDXY(DistanceSpline *spline, double distance, double *res) {
+ double *val = spline->DXY(distance).data();
+ res[0] = val[0];
+ res[1] = val[1];
+ }
+
+ void DistanceSplineDDXY(DistanceSpline *spline, double distance,
+ double *res) {
+ double *val = spline->DDXY(distance).data();
+ res[0] = val[0];
+ res[1] = val[1];
+ }
+
+ double DistanceSplineTheta(DistanceSpline *spline, double distance) {
+ return spline->Theta(distance);
+ }
+
+ double DistanceSplineDTheta(DistanceSpline *spline, double distance) {
+ return spline->DTheta(distance);
+ }
+
+ double DistanceSplineDThetaDt(DistanceSpline *spline, double distance,
+ double velocity) {
+ return spline->DThetaDt(distance, velocity);
+ }
+
+ double DistanceSplineDDTheta(DistanceSpline *spline, double distance) {
+ return spline->DDTheta(distance);
+ }
+
+ double DistanceSplineLength(DistanceSpline *spline) {
+ return spline->length();
+ }
}
} // namespace drivetrain
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index bedd763..8d467e1 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -82,10 +82,20 @@
"libspline.py",
],
data = [
- "//frc971/control_loops/drivetrain:spline.so"
+ "//frc971/control_loops/drivetrain:spline.so",
],
)
+py_test(
+ name = "lib_spline_test",
+ srcs = [
+ "lib_spline_test.py",
+ ],
+ deps = [
+ ":libspline",
+ ]
+)
+
py_library(
name = "polydrivetrain",
srcs = [
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
new file mode 100644
index 0000000..2c77d62
--- /dev/null
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -0,0 +1,27 @@
+#!/usr/bin/python
+
+import math
+import numpy as np
+from numpy.testing import *
+import unittest
+
+from libspline import Spline, DistanceSpline
+
+class TestSpline(unittest.TestCase):
+ def testSimpleSpline(self):
+ points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
+ [2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
+ spline = Spline(points)
+ assert_allclose(spline.Point(.5), [3.5, 4.5])
+ assert_almost_equal(spline.Theta(.5), math.atan2(1, 1))
+
+class TestDistanceSpline(unittest.TestCase):
+ def testDistanceSpline(self):
+ points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
+ [2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
+ spline = Spline(points)
+ dSpline = DistanceSpline([spline])
+ assert_almost_equal(dSpline.Length(), 5 * math.sqrt(2))
+
+if __name__ == '__main__':
+ unittest.main()
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)