Initial commit - test serial
This commit is contained in:
Cole A. Deck
310
SerialTest/include/okapi/squiggles/spline.hpp
Normal file
310
SerialTest/include/okapi/squiggles/spline.hpp
Normal file
@@ -0,0 +1,310 @@
|
||||
/**
|
||||
* Copyright 2020 Jonathan Bayless
|
||||
*
|
||||
* Use of this source code is governed by an MIT-style license that can be found
|
||||
* in the LICENSE file or at https://opensource.org/licenses/MIT.
|
||||
*/
|
||||
#ifndef _SQUIGGLES_SPLINE_HPP_
|
||||
#define _SQUIGGLES_SPLINE_HPP_
|
||||
|
||||
#include <initializer_list>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "constraints.hpp"
|
||||
#include "geometry/controlvector.hpp"
|
||||
#include "geometry/profilepoint.hpp"
|
||||
#include "math/quinticpolynomial.hpp"
|
||||
#include "physicalmodel/passthroughmodel.hpp"
|
||||
#include "physicalmodel/physicalmodel.hpp"
|
||||
|
||||
namespace squiggles {
|
||||
class SplineGenerator {
|
||||
public:
|
||||
/**
|
||||
* Generates curves that match the given motion constraints.
|
||||
*
|
||||
* @param iconstraints The maximum allowable values for the robot's motion.
|
||||
* @param imodel The robot's physical characteristics and constraints
|
||||
* @param idt The difference in time in seconds between each state for the
|
||||
* generated paths.
|
||||
*/
|
||||
SplineGenerator(Constraints iconstraints,
|
||||
std::shared_ptr<PhysicalModel> imodel =
|
||||
std::make_shared<PassthroughModel>(),
|
||||
double idt = 0.1);
|
||||
|
||||
/**
|
||||
* Creates a motion profiled path between the given waypoints.
|
||||
*
|
||||
* @param iwaypoints The list of poses that the robot should reach along the
|
||||
* path.
|
||||
* @param fast If true, the path optimization process will stop as soon as the
|
||||
* constraints are met. If false, the optimizer will find the
|
||||
* smoothest possible path between the points.
|
||||
*
|
||||
* @return A series of robot states defining a path between the poses.
|
||||
*/
|
||||
std::vector<ProfilePoint> generate(std::vector<Pose> iwaypoints,
|
||||
bool fast = false);
|
||||
std::vector<ProfilePoint> generate(std::initializer_list<Pose> iwaypoints,
|
||||
bool fast = false);
|
||||
|
||||
/**
|
||||
* Creates a motion profiled path between the given waypoints.
|
||||
*
|
||||
* @param iwaypoints The list of vectors that the robot should reach along the
|
||||
* path.
|
||||
*
|
||||
* @return A series of robot states defining a path between the vectors.
|
||||
*/
|
||||
std::vector<ProfilePoint> generate(std::vector<ControlVector> iwaypoints);
|
||||
std::vector<ProfilePoint>
|
||||
generate(std::initializer_list<ControlVector> iwaypoints);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* The maximum allowable values for the robot's motion.
|
||||
*/
|
||||
Constraints constraints;
|
||||
|
||||
/**
|
||||
* Defines the physical structure of the robot and translates the linear
|
||||
* kinematics to wheel velocities.
|
||||
*/
|
||||
std::shared_ptr<PhysicalModel> model;
|
||||
|
||||
/**
|
||||
* The time difference between each value in the generated path.
|
||||
*/
|
||||
double dt;
|
||||
|
||||
/**
|
||||
* The minimum and maximum durations for a path to take. A larger range allows
|
||||
* for longer possible paths at the expense of a longer path generation time.
|
||||
*/
|
||||
const int T_MIN = 2;
|
||||
const int T_MAX = 15;
|
||||
const int MAX_GRAD_DESCENT_ITERATIONS = 10;
|
||||
|
||||
/**
|
||||
* This is factor is used to create a "dummy velocity" in the initial path
|
||||
* generation step one or both of the preferred start or end velocities is
|
||||
* zero. The velocity will be replaced with the preferred start/end velocity
|
||||
* in parameterization but a nonzero velocity is needed for the spline
|
||||
* calculation.
|
||||
*
|
||||
* This was 1.2 in the WPILib example but that large of a value seems to
|
||||
* create wild paths, 0.12 worked better in testing with VEX-sized paths.
|
||||
*/
|
||||
public:
|
||||
const double K_DEFAULT_VEL = 1.0;
|
||||
|
||||
/**
|
||||
* The output of the initial, "naive" generation step. We discard the
|
||||
* derivative values to replace them with values from a motion profile.
|
||||
*/
|
||||
|
||||
struct GeneratedPoint {
|
||||
GeneratedPoint(Pose ipose, double icurvature = 0.0)
|
||||
: pose(ipose), curvature(icurvature) {}
|
||||
|
||||
std::string to_string() const {
|
||||
return "GeneratedPoint: {" + pose.to_string() +
|
||||
", curvature: " + std::to_string(curvature) + "}";
|
||||
}
|
||||
|
||||
Pose pose;
|
||||
double curvature;
|
||||
};
|
||||
|
||||
/**
|
||||
* An intermediate value used in the "naive" generation step. Contains the
|
||||
* final GeneratedPoint value that will be returned as well as the spline's
|
||||
* derivative values to perform the initial check against the constraints.
|
||||
*/
|
||||
struct GeneratedVector {
|
||||
GeneratedVector(GeneratedPoint ipoint,
|
||||
double ivel,
|
||||
double iaccel,
|
||||
double ijerk)
|
||||
: point(ipoint), vel(ivel), accel(iaccel), jerk(ijerk) {}
|
||||
|
||||
GeneratedPoint point;
|
||||
double vel;
|
||||
double accel;
|
||||
double jerk;
|
||||
|
||||
std::string to_string() const {
|
||||
return "GeneratedVector: {" + point.to_string() +
|
||||
", vel: " + std::to_string(vel) +
|
||||
", accel: " + std::to_string(accel) +
|
||||
", jerk: " + std::to_string(jerk) + "}";
|
||||
}
|
||||
};
|
||||
|
||||
std::vector<GeneratedVector> gen_single_raw_path(ControlVector start,
|
||||
ControlVector end,
|
||||
int duration,
|
||||
double start_vel,
|
||||
double end_vel);
|
||||
/**
|
||||
* Runs a Gradient Descent algorithm to minimize the linear acceleration,
|
||||
* linear jerk, and curvature for the generated path.
|
||||
*
|
||||
* This is used when there is not a start/end velocity specified for a given
|
||||
* path.
|
||||
*/
|
||||
std::vector<GeneratedPoint>
|
||||
gradient_descent(ControlVector& start, ControlVector& end, bool fast);
|
||||
|
||||
/**
|
||||
* An intermediate value used in the parameterization step. Adds the
|
||||
* constrained values from the motion profile to the output from the "naive"
|
||||
* generation step.
|
||||
*/
|
||||
struct ConstrainedState {
|
||||
ConstrainedState(Pose ipose,
|
||||
double icurvature,
|
||||
double idistance,
|
||||
double imax_vel,
|
||||
double imin_accel,
|
||||
double imax_accel)
|
||||
: pose(ipose),
|
||||
curvature(icurvature),
|
||||
distance(idistance),
|
||||
max_vel(imax_vel),
|
||||
min_accel(imin_accel),
|
||||
max_accel(imax_accel) {}
|
||||
|
||||
ConstrainedState() = default;
|
||||
|
||||
Pose pose = Pose();
|
||||
double curvature = 0;
|
||||
double distance = 0;
|
||||
double max_vel = 0;
|
||||
double min_accel = 0;
|
||||
double max_accel = 0;
|
||||
|
||||
std::string to_string() const {
|
||||
return "ConstrainedState: {x: " + std::to_string(pose.x) +
|
||||
", y: " + std::to_string(pose.y) +
|
||||
", yaw: " + std::to_string(pose.yaw) +
|
||||
", k: " + std::to_string(curvature) +
|
||||
", dist: " + std::to_string(distance) +
|
||||
", v: " + std::to_string(max_vel) +
|
||||
", min_a: " + std::to_string(min_accel) +
|
||||
", max_a: " + std::to_string(max_accel) + "}";
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* The actual function called by the "generate" functions.
|
||||
*
|
||||
* @param start An iterator pointing to the first ControlVector in the path
|
||||
* @param end An iterator pointting to the last ControlVector in the path
|
||||
*
|
||||
* @return The points from each path concatenated together
|
||||
*/
|
||||
template <class Iter>
|
||||
std::vector<ProfilePoint> _generate(Iter start, Iter end, bool fast);
|
||||
|
||||
public:
|
||||
/**
|
||||
* Performs the "naive" generation step.
|
||||
*
|
||||
* This step calculates the spline polynomials that fit within the
|
||||
* SplineGenerator's acceleration and jerk constraints and returns the points
|
||||
* that form the curve.
|
||||
*/
|
||||
std::vector<GeneratedPoint>
|
||||
gen_raw_path(ControlVector& start, ControlVector& end, bool fast);
|
||||
|
||||
/**
|
||||
* Imposes a linear motion profile on the raw path.
|
||||
*
|
||||
* This step creates the velocity and acceleration values to command the robot
|
||||
* at each point along the curve.
|
||||
*/
|
||||
std::vector<ProfilePoint>
|
||||
parameterize(const ControlVector start,
|
||||
const ControlVector end,
|
||||
const std::vector<GeneratedPoint>& raw_path,
|
||||
const double preferred_start_vel,
|
||||
const double preferred_end_vel,
|
||||
const double start_time);
|
||||
|
||||
/**
|
||||
* Finds the new timestamps for each point along the curve based on the motion
|
||||
* profile.
|
||||
*/
|
||||
std::vector<ProfilePoint>
|
||||
integrate_constrained_states(std::vector<ConstrainedState> constrainedStates);
|
||||
|
||||
/**
|
||||
* Finds the ProfilePoint on the profiled curve for the given timestamp.
|
||||
*
|
||||
* This with interpolate between points on the curve if a point with an exact
|
||||
* matching timestamp is not found.
|
||||
*/
|
||||
ProfilePoint get_point_at_time(const ControlVector start,
|
||||
const ControlVector end,
|
||||
std::vector<ProfilePoint> points,
|
||||
double t);
|
||||
|
||||
/**
|
||||
* Linearly interpolates between points along the profiled curve.
|
||||
*/
|
||||
ProfilePoint lerp_point(QuinticPolynomial x_qp,
|
||||
QuinticPolynomial y_qp,
|
||||
ProfilePoint start,
|
||||
ProfilePoint end,
|
||||
double i);
|
||||
|
||||
/**
|
||||
* Returns the spline curve for the given control vectors and path duration.
|
||||
*/
|
||||
QuinticPolynomial get_x_spline(const ControlVector start,
|
||||
const ControlVector end,
|
||||
const double duration);
|
||||
QuinticPolynomial get_y_spline(const ControlVector start,
|
||||
const ControlVector end,
|
||||
const double duration);
|
||||
|
||||
/**
|
||||
* Applies the general constraints and model constraints to the given state.
|
||||
*/
|
||||
void enforce_accel_lims(ConstrainedState* state);
|
||||
|
||||
/**
|
||||
* Imposes the motion profile constraints on a segment of the path from the
|
||||
* perspective of iterating forwards through the path.
|
||||
*/
|
||||
void forward_pass(ConstrainedState* predecessor, ConstrainedState* successor);
|
||||
|
||||
/**
|
||||
* Imposes the motion profile constraints on a segment of the path from the
|
||||
* perspective of iterating backwards through the path.
|
||||
*/
|
||||
void backward_pass(ConstrainedState* predecessor,
|
||||
ConstrainedState* successor);
|
||||
|
||||
/**
|
||||
* Calculates the final velocity for a path segment.
|
||||
*/
|
||||
double vf(double vi, double a, double ds);
|
||||
|
||||
/**
|
||||
* Calculates the initial acceleration needed to match the segments'
|
||||
* velocities.
|
||||
*/
|
||||
double ai(double vf, double vi, double s);
|
||||
|
||||
/**
|
||||
* Values that are closer to each other than this value are considered equal.
|
||||
*/
|
||||
static constexpr double K_EPSILON = 1e-5;
|
||||
};
|
||||
} // namespace squiggles
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user