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/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