27. Implement Quintic Polynomial Solver C++
Implement a Quintic Polynomial Solver
In this exercise you will implement a quintic polynomial solver. This will let you take boundary conditions as input and generate a polynomial trajectory which matches those conditions with minimal jerk.
Inputs
Your solver will take three inputs.
-
start
- [s_i, \dot{s_i}, \ddot{s_i}] -
end
- [s_f, \dot{s_f}, \ddot{s_f}] -
T
- the duration of maneuver in seconds.
Instructions
-
Implement the
JMT(start, end, T)
function inmain.cpp
-
Hit
Test Run
and see if you're correct!
Tips
Remember, you are solving a system of equations: matrices will be helpful! The Eigen library used from Sensor Fusion is included.
The equations for position, velocity, and acceleration are given by:
and if you evaluate these at t=0 you find the first three coeffecients of your JMT are:
[\alpha_0, \alpha_1, \alpha_2] = [s_i, \dot{s_i}, \frac{1}{2}\ddot{s_i}]
and you can get the last three coefficients by evaluating these equations at t = T . When you carry out the math and write the problem in matrix form you get the following:
All these quantities are known except for \alpha_3, \alpha_4, \alpha_5
Start Quiz:
#include <cmath>
#include <iostream>
#include <vector>
#include "Dense"
#include "grader.h"
using std::vector;
using Eigen::MatrixXd;
using Eigen::VectorXd;
/**
* TODO: complete this function
*/
vector<double> JMT(vector<double> &start, vector<double> &end, double T) {
/**
* Calculate the Jerk Minimizing Trajectory that connects the initial state
* to the final state in time T.
*
* @param start - the vehicles start location given as a length three array
* corresponding to initial values of [s, s_dot, s_double_dot]
* @param end - the desired end state for vehicle. Like "start" this is a
* length three array.
* @param T - The duration, in seconds, over which this maneuver should occur.
*
* @output an array of length 6, each value corresponding to a coefficent in
* the polynomial:
* s(t) = a_0 + a_1 * t + a_2 * t**2 + a_3 * t**3 + a_4 * t**4 + a_5 * t**5
*
* EXAMPLE
* > JMT([0, 10, 0], [10, 10, 0], 1)
* [0.0, 10.0, 0.0, 0.0, 0.0, 0.0]
*/
return {1,2,3,4,5,6};
}
int main() {
// create test cases
vector< test_case > tc = create_tests();
bool total_correct = true;
for(int i = 0; i < tc.size(); ++i) {
vector<double> jmt = JMT(tc[i].start, tc[i].end, tc[i].T);
bool correct = close_enough(jmt, answers[i]);
total_correct &= correct;
}
if(!total_correct) {
std::cout << "Try again!" << std::endl;
} else {
std::cout << "Nice work!" << std::endl;
}
return 0;
}
#ifndef GRADER_H
#define GRADER_H
#include <cmath>
#include <vector>
using std::vector;
struct test_case {
vector<double> start;
vector<double> end;
double T;
};
bool close_enough(vector<double> &poly, vector<double> &target_poly,
double eps=0.01) {
if (poly.size() != target_poly.size()) {
std::cout << "Your solution didn't have the correct number of terms"
<< std::endl;
return false;
}
for (int i = 0; i < poly.size(); ++i) {
double diff = poly[i]-target_poly[i];
if (abs(diff) > eps) {
std::cout << "At least one of your terms differed from target by more than "
<< eps << std::endl;
return false;
}
}
return true;
}
vector<test_case> create_tests() {
// Create test case vector
vector<test_case> tc;
test_case tc1;
tc1.start = {0,10,0};
tc1.end = {10,10,0};
tc1.T = 1;
tc.push_back(tc1);
test_case tc2;
tc2.start = {0,10,0};
tc2.end = {20,15,20};
tc2.T = 2;
tc.push_back(tc2);
test_case tc3;
tc3.start = {5,10,2};
tc3.end = {-30,-20,-4};
tc3.T = 5;
tc.push_back(tc3);
return tc;
}
vector<vector<double>> answers = {{0.0, 10.0, 0.0, 0.0, 0.0, 0.0},
{0.0,10.0,0.0,0.0,-0.625,0.3125},
{5.0,10.0,1.0,-3.0,0.64,-0.0432}};
#endif // GRADER_H