Add python wrapper for trajectory.
Reformatted several files and added some useful comments on how to use
the python wrappers.
Change-Id: I9fd89977e0d4f16fa24d0e3b3b7c6657df865b32
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 117290d..0d14f66 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -1,62 +1,66 @@
#include <vector>
+#include <string>
#include "Eigen/Dense"
+#include "aos/logging/implementations.h"
+#include "aos/network/team_number.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
extern "C" {
- NSpline<6>* NewSpline(double x[6], double y[6]) {
+ // Based on spline.h
+ 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 deleteSpline(NSpline<6> *spline) { delete spline; }
- void SplinePoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->Point(alpha).data();
+ void SplinePoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->Point(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void SplineDPoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->DPoint(alpha).data();
+ void SplineDPoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->DPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void SplineDDPoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->DDPoint(alpha).data();
+ void SplineDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->DDPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- void SplineDDDPoint(NSpline<6>* spline, double alpha, double* res) {
- double* val = spline->DDDPoint(alpha).data();
+ void SplineDDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ double *val = spline->DDDPoint(alpha).data();
res[0] = val[0];
res[1] = val[1];
}
- double SplineTheta(NSpline<6>* spline, double alpha) {
+ double SplineTheta(NSpline<6> *spline, double alpha) {
return spline->Theta(alpha);
}
- double SplineDTheta(NSpline<6>* spline, double alpha) {
+ double SplineDTheta(NSpline<6> *spline, double alpha) {
return spline->DTheta(alpha);
}
- double SplineDDTheta(NSpline<6>* spline, double alpha) {
+ double SplineDDTheta(NSpline<6> *spline, double alpha) {
return spline->DDTheta(alpha);
}
- void SplineControlPoints(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) {
@@ -65,7 +69,8 @@
}
}
- DistanceSpline* NewDistanceSpline(Spline** splines, int count) {
+ // Based on distance_spline.h
+ DistanceSpline *NewDistanceSpline(Spline **splines, int count) {
::std::vector<Spline> splines_;
for (int i = 0; i < count; ++i) {
splines_.push_back(*splines[i]);
@@ -73,9 +78,7 @@
return new DistanceSpline(::std::vector<Spline>(splines_));
}
- void deleteDistanceSpline(DistanceSpline* spline) {
- delete spline;
- }
+ void deleteDistanceSpline(DistanceSpline *spline) { delete spline; }
void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
double *val = spline->XY(distance).data();
@@ -116,6 +119,89 @@
double DistanceSplineLength(DistanceSpline *spline) {
return spline->length();
}
+
+ // Based on trajectory.h
+ Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
+ int num_distance) {
+ return new Trajectory(
+ spline, ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
+ num_distance);
+ }
+
+ void deleteTrajectory(Trajectory *t) { delete t; }
+
+ void TrajectorySetLongitudalAcceleration(Trajectory *t, double accel) {
+ t->set_longitudal_acceleration(accel);
+ }
+
+ void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
+ t->set_lateral_acceleration(accel);
+ }
+
+ void TrajectorySetVoltageLimit(Trajectory *t, double limit) {
+ t->set_voltage_limit(limit);
+ }
+
+ void TrajectoryLimitVelocity(Trajectory *t, double start, double end,
+ double max) {
+ t->LimitVelocity(start, end, max);
+ }
+
+ void TrajectoryPlan(Trajectory *t) { t->Plan(); }
+
+ double TrajectoryLength(Trajectory *t) { return t->length(); }
+
+ int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
+
+ // This assumes that res is created in python to be getPathLength() long.
+ // Likely to SEGFAULT otherwise.
+ void Distances(Trajectory *t, double *res) {
+ const ::std::vector<double> &distances = t->Distances();
+ ::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
+ }
+
+ double TrajectoryDistance(Trajectory *t, int index) {
+ return t->Distance(index);
+ }
+
+ // This assumes that res is created in python to be getPathLength() long.
+ // Likely to SEGFAULT otherwise.
+ void TrajectoryGetPlan(Trajectory *t, double *res) {
+ const ::std::vector<double> &velocities = t->plan();
+ ::std::memcpy(res, velocities.data(), sizeof(double) * velocities.size());
+ }
+
+ // Time in in nanoseconds.
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *TrajectoryGetPlanXVAPtr(
+ Trajectory *t, int dt) {
+ return new ::std::vector<::Eigen::Matrix<double, 3, 1>>(
+ t->PlanXVA(::std::chrono::nanoseconds(dt)));
+ }
+
+ void TrajectoryDeleteVector(
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ delete vec;
+ }
+
+ int TrajectoryGetVectorLength(
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ return vec->size();
+ }
+
+ void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
+ double *X, double *V, double *A) {
+ for (size_t i = 0; i < vec->size(); ++i) {
+ X[i] = (*vec)[0][0];
+ V[i] = (*vec)[0][1];
+ A[i] = (*vec)[0][2];
+ }
+ }
+
+ // Util
+ void SetUpLogging() {
+ ::aos::logging::Init();
+ ::aos::network::OverrideTeamNumber(971);
+ }
}
} // namespace drivetrain