Create python wrapper for spline.
Change-Id: Ia19bb1500542dd886c78cd201b251e6dff2b1d1b
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 2bad4d9..c3ea79e 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -294,6 +294,16 @@
],
)
+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
new file mode 100644
index 0000000..f53dd23
--- /dev/null
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -0,0 +1,70 @@
+#include <string.h>
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/drivetrain/spline.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+extern "C" {
+ 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());
+ }
+
+ void deleteSpline(NSpline<6>* spline) {
+ delete spline;
+ }
+
+ void Point(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) {
+ double* val = spline->DPoint(alpha).data();
+ res[0] = val[0];
+ res[1] = val[1];
+ }
+
+ void DDPoint(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) {
+ double* val = spline->DDDPoint(alpha).data();
+ res[0] = val[0];
+ res[1] = val[1];
+ }
+
+ double Theta(NSpline<6>* spline, double alpha) {
+ return spline->Theta(alpha);
+ }
+
+ double DTheta(NSpline<6>* spline, double alpha) {
+ return spline->DTheta(alpha);
+ }
+
+ double DDTheta(NSpline<6>* spline, double alpha) {
+ return spline->DDTheta(alpha);
+ }
+
+ void control_points(NSpline<6>* spline, double* x, double* y) {
+ auto points = spline->control_points();
+ // Deal with incorrectly strided matrix.
+ for (int i = 0; i < 6; ++i) {
+ x[i] = points(0, i);
+ y[i] = points(1, i);
+ }
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index c0b71f7..bedd763 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -77,6 +77,16 @@
)
py_library(
+ name = "libspline",
+ srcs = [
+ "libspline.py",
+ ],
+ data = [
+ "//frc971/control_loops/drivetrain:spline.so"
+ ],
+)
+
+py_library(
name = "polydrivetrain",
srcs = [
"polydrivetrain.py",
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
new file mode 100644
index 0000000..62daca1
--- /dev/null
+++ b/frc971/control_loops/python/libspline.py
@@ -0,0 +1,71 @@
+#!/usr/bin/python
+
+"""Wrapper around spline.h/cc through spline_array.cc."""
+
+__author__ = 'Alex Perry (alex.perry96@gmail.com)'
+
+import ctypes as ct
+import numpy as np
+import os
+
+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'))
+ 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
+
+class Spline:
+ """A wrapper around spline.h/cc through spline_array.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 __del__(self):
+ libSpline.deleteSpline(self.__spline)
+
+ def setPoint(self, index, x, y):
+ 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]))
+
+ def Point(self, alpha):
+ result = np.zeros(2)
+ libSpline.Point(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))
+ return result
+
+ def DDPoint(self, alpha):
+ result = np.zeros(2)
+ libSpline.DDPoint(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))
+ return result
+
+ def Theta(self, alpha):
+ return libSpline.Theta(self.__spline, ct.c_double(alpha))
+
+ def DTheta(self, alpha):
+ return libSpline.DTheta(self.__spline, ct.c_double(alpha))
+
+ def DDTheta(self, alpha):
+ return libSpline.DDTheta(self.__spline, ct.c_double(alpha))
+
+ def ControlPoints(self):
+ return self.__points;