int main(int argc, char **argv) {
int num_segments = 3;
vector<double> segment_times = generateSegmentTimes(num_segments);
double x0 = uniform(generator);
double xd0 = uniform(generator);
double xf = uniform(generator);
double xdf = uniform(generator);
double x1 = uniform(generator);
double x2 = uniform(generator);
PiecewisePolynomial<double> result = twoWaypointCubicSpline(segment_times, x0, xd0, xf, xdf, x1, x2);
for (int i = 0; i < num_segments; i++) {
valuecheck(segment_times[i], result.getStartTime(i));
}
valuecheck(segment_times[num_segments], result.getEndTime(num_segments - 1));
// check value constraints
double tol = 1e-10;
PiecewisePolynomial<double> derivative = result.derivative();
PiecewisePolynomial<double> second_derivative = derivative.derivative();
valuecheck(result.value(result.getStartTime(0)), x0, tol);
valuecheck(derivative.value(result.getStartTime(0)), xd0, tol);
valuecheck(result.value(result.getEndTime(num_segments - 1)), xf, tol);
valuecheck(derivative.value(result.getEndTime(num_segments - 1)), xdf, tol);
valuecheck(result.value(result.getStartTime(1)), x1, tol);
valuecheck(result.value(result.getStartTime(2)), x2, tol);
// check continuity constraints
double eps = 1e-10;
int num_knots = num_segments - 1;
for (int i = 0; i < num_knots; i++) {
double t_knot = result.getEndTime(i);
valuecheck(result.value(t_knot - eps), result.value(t_knot + eps), 1e-8);
valuecheck(derivative.value(t_knot - eps), derivative.value(t_knot + eps), 1e-8);
valuecheck(second_derivative.value(t_knot - eps), second_derivative.value(t_knot + eps), 1e-8);
}
#if !defined(WIN32) && !defined(WIN64)
int ntests = 1000;
cout << "time: " << measure<chrono::microseconds>::execution(randomSpeedTest, ntests) / (double) ntests << " microseconds." << endl;
#endif
cout << "test passed" << endl;
return 0;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
string usage = "[coefs, objval] = nWaypointCubicSplinemex(ts, xs, xd0, xdf)";
if (nrhs != 4)
mexErrMsgIdAndTxt("Drake:nWaypointCubicSplinemex:WrongNumberOfInputs",
usage.c_str());
if (nlhs > 2)
mexErrMsgIdAndTxt("Drake:nWaypointCubicSplinemex:WrongNumberOfOutputs",
usage.c_str());
const std::vector<double> segment_times = matlabToStdVector<double>(prhs[0]);
MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[1]);
auto xd0 = matlabToEigen<Dynamic, 1>(prhs[2]);
auto xdf = matlabToEigen<Dynamic, 1>(prhs[3]);
mwSize ndof = static_cast<mwSize>(xs.rows());
mwSize num_segments = static_cast<mwSize>(xs.cols()) - 1;
mwSize num_knots = num_segments - 1;
mwSize num_coeffs_per_segment = 4;
mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
double objective_value = 0.0;
for (mwSize dof = 0; dof < ndof; dof++) {
VectorXd xi = xs.block(dof, 1, 1, num_knots).transpose();
PiecewisePolynomial<double> spline =
nWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof],
xs(dof, num_segments), xdf[dof], xi);
PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
acceleration_squared *= acceleration_squared;
PiecewisePolynomial<double> acceleration_squared_integral =
acceleration_squared.integral();
objective_value +=
acceleration_squared_integral.scalarValue(spline.getEndTime()) -
acceleration_squared_integral.scalarValue(spline.getStartTime());
for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments();
segment_index++) {
for (mwSize coefficient_index = 0;
coefficient_index < num_coeffs_per_segment; coefficient_index++) {
mwSize sub[] = {dof, segment_index,
num_coeffs_per_segment - coefficient_index -
1
}; // Matlab's reverse coefficient indexing...
*(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) =
spline.getPolynomial(static_cast<int>(segment_index))
.getCoefficients()[coefficient_index];
}
}
}
if (nlhs > 1) {
plhs[1] = mxCreateDoubleScalar(objective_value);
}
}
void evaluateXYZExpmapCubicSpline(double t, const PiecewisePolynomial<double> &spline, Isometry3d &body_pose_des, Vector6d &xyzdot_angular_vel, Vector6d &xyzddot_angular_accel) {
Vector6d xyzexp = spline.value(t);
auto derivative = spline.derivative();
Vector6d xyzexpdot = derivative.value(t);
Vector6d xyzexpddot = derivative.derivative().value(t);
xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>();
xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>();
Vector3d expmap = xyzexp.tail<3>();
auto quat_grad = expmap2quat(expmap,2);
Vector4d quat = quat_grad.value();
body_pose_des.linear() = quat2rotmat(quat);
body_pose_des.translation() = xyzexp.head<3>();
Vector4d quat_dot = quat_grad.gradient().value() * xyzexpdot.tail<3>();
quat_grad.gradient().gradient().value().resize(12,3);
Matrix<double,12,3> dE = quat_grad.gradient().gradient().value();
Vector3d expdot = xyzexpdot.tail<3>();
Matrix<double,4,3> Edot = matGradMult(dE,expdot);
Vector4d quat_ddot = quat_grad.gradient().value()*xyzexpddot.tail<3>() + Edot*expdot;
Matrix<double,3,4> M;
Matrix<double,12,4> dM;
quatdot2angularvelMatrix(quat,M,&dM);
xyzdot_angular_vel.tail<3>() = M*quat_dot;
xyzddot_angular_accel.tail<3>() = M*quat_ddot + matGradMult(dM,quat_dot)*quat_dot;
}
void evaluateXYZExpmapCubicSpline(double t,
const PiecewisePolynomial<double> &spline,
Isometry3d &body_pose_des,
Vector6d &xyzdot_angular_vel,
Vector6d &xyzddot_angular_accel) {
Vector6d xyzexp = spline.value(t);
auto derivative = spline.derivative();
Vector6d xyzexpdot = derivative.value(t);
Vector6d xyzexpddot = derivative.derivative().value(t);
// translational part
body_pose_des.translation() = xyzexp.head<3>();
xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>();
xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>();
// rotational part
auto expmap = xyzexp.tail<3>();
auto expmap_dot = xyzexpdot.tail<3>();
auto expmap_ddot = xyzexpddot.tail<3>();
// construct autodiff version of expmap
// autodiff derivatives represent first and second derivative w.r.t. time
// TODO(tkoolen): should use 1 instead of dynamic, but causes issues
// with eigen on MSVC 32 bit; should be fixed in 3.3
typedef AutoDiffScalar<Matrix<double, Dynamic, 1>> ADScalar;
// TODO(tkoolen): should use 1 instead of dynamic, but causes issues
// with eigen on MSVC 32 bit; should be fixed in 3.3
typedef AutoDiffScalar<Matrix<ADScalar, Dynamic, 1>> ADScalarSecondDeriv;
Matrix<ADScalarSecondDeriv, 3, 1> expmap_autodiff;
for (int i = 0; i < expmap_autodiff.size(); i++) {
expmap_autodiff(i).value() = expmap(i);
expmap_autodiff(i).derivatives().resize(1);
expmap_autodiff(i).derivatives()(0) = expmap_dot(i);
expmap_autodiff(i).derivatives()(0).derivatives().resize(1);
expmap_autodiff(i).derivatives()(0).derivatives()(0) = expmap_ddot(i);
}
auto quat_autodiff = expmap2quat(expmap_autodiff);
Vector4d quat = autoDiffToValueMatrix(autoDiffToValueMatrix(quat_autodiff));
body_pose_des.linear() = quat2rotmat(quat);
// angular velocity and acceleration are computed from quaternion derivative
// meaning of derivative vectors remains the same: first and second
// derivatives w.r.t. time
decltype(quat_autodiff) quat_dot_autodiff;
for (int i = 0; i < quat_dot_autodiff.size(); i++) {
quat_dot_autodiff(i).value() = quat_autodiff(i).derivatives()(0).value();
quat_dot_autodiff(i).derivatives().resize(1);
quat_dot_autodiff(i).derivatives()(0).value() =
quat_autodiff(i).derivatives()(0).derivatives()(0);
quat_dot_autodiff(i).derivatives()(0).derivatives().resize(1);
quat_dot_autodiff(i).derivatives()(0).derivatives()(0) =
std::numeric_limits<double>::quiet_NaN(); // we're not interested in
// second deriv of angular
// velocity
}
auto omega_autodiff =
(quatdot2angularvelMatrix(quat_autodiff) * quat_dot_autodiff).eval();
auto omega = xyzdot_angular_vel.tail<3>();
auto omega_dot = xyzddot_angular_accel.tail<3>();
for (int i = 0; i < omega_autodiff.size(); i++) {
omega(i) = omega_autodiff(i).value().value();
omega_dot(i) = omega_autodiff(i).derivatives()(0).value();
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
string usage =
"[coefs, ts, objective_value] = "
"nWaypointCubicSplineFreeKnotTimesmex.cpp(t0, tf, xs, xd0, xdf)";
if (nrhs != 5)
mexErrMsgIdAndTxt(
"Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfInputs",
usage.c_str());
if (nlhs < 2 || nlhs > 3)
mexErrMsgIdAndTxt(
"Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfOutputs",
usage.c_str());
double t0 = mxGetPrSafe(prhs[0])[0];
double tf = mxGetPrSafe(prhs[1])[0];
MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[2]);
auto xd0 = matlabToEigen<Dynamic, 1>(prhs[3]);
auto xdf = matlabToEigen<Dynamic, 1>(prhs[4]);
mwSize ndof = static_cast<mwSize>(xs.rows());
mwSize num_segments = static_cast<mwSize>(xs.cols()) - 1;
mwSize num_knots = num_segments - 1;
if (num_knots >= 3)
mexWarnMsgTxt(
"More knots than two is likely to be super slow in a grid search!\n");
if (num_knots <= 0)
mexErrMsgIdAndTxt(
"Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:"
"NotEnoughKnotsToJustifyThisFunction",
usage.c_str());
mwSize num_coeffs_per_segment = 4;
mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
std::vector<double> segment_times;
segment_times.resize(static_cast<size_t>(num_segments) + 1);
segment_times[0] = t0;
segment_times[static_cast<size_t>(num_segments)] = tf;
std::vector<double> best_segment_times = segment_times;
double t_step = (tf - t0) / GRID_STEPS;
double min_objective_value = numeric_limits<double>::infinity();
// assemble the knot point locations for input to nWaypointCubicSpline
MatrixXd xi = xs.block(0, 1, ndof, num_knots);
if (GRID_STEPS <= num_knots) {
// If we have have too few grid steps, then by pigeonhole it's
// impossible to give each a unique time in our grid search.
mexErrMsgIdAndTxt(
"Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:"
"TooManyKnotsForNumGridSteps",
usage.c_str());
}
std::vector<int> t_indices;
t_indices.reserve(num_knots);
for (mwSize i = 0; i < num_knots; i++) {
t_indices.push_back(i + 1); // assume knot point won't be the same time as
// the initial state, or previous knot point
}
while (t_indices[0] < (GRID_STEPS - static_cast<int>(num_knots) + 1)) {
for (mwSize i = 0; i < num_knots; i++)
segment_times[i + 1] = t0 + t_indices[i] * t_step;
bool valid_solution = true;
double objective_value = 0.0;
for (mwSize dof = 0; dof < ndof && valid_solution; dof++) {
try {
PiecewisePolynomial<double> spline = nWaypointCubicSpline(
segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments),
xdf[dof], xi.row(dof).transpose());
PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
acceleration_squared *= acceleration_squared;
PiecewisePolynomial<double> acceleration_squared_integral =
acceleration_squared.integral();
objective_value +=
acceleration_squared_integral.scalarValue(spline.getEndTime()) -
acceleration_squared_integral.scalarValue(spline.getStartTime());
} catch (ConstraintMatrixSingularError &) {
valid_solution = false;
}
}
if (valid_solution && objective_value < min_objective_value) {
best_segment_times = segment_times;
min_objective_value = objective_value;
}
// Advance grid search counter or terminate, counting from
// the latest t_index, and on overflow carrying to the
// next lowest t_index and resetting to the new value of that
// next lowest t_index. (since times must always be in order!)
t_indices[num_knots - 1]++;
// carry, except for the lowest place, which we
// use to detect doneness.
for (size_t i = num_knots - 1; i > 0; i--) {
if ((i == num_knots - 1 && t_indices[i] >= GRID_STEPS) ||
(i < num_knots - 1 && t_indices[i] >= t_indices[i + 1])) {
t_indices[i - 1]++;
t_indices[i] = t_indices[i - 1] + 1;
}
}
}
for (mwSize dof = 0; dof < ndof; dof++) {
PiecewisePolynomial<double> spline = nWaypointCubicSpline(
best_segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments),
xdf[dof], xi.row(dof).transpose());
for (mwSize segment_index = 0;
segment_index < static_cast<mwSize>(spline.getNumberOfSegments());
segment_index++) {
for (mwSize coefficient_index = 0;
coefficient_index < num_coeffs_per_segment; coefficient_index++) {
mwSize sub[] = {dof, segment_index,
num_coeffs_per_segment - coefficient_index -
1}; // Matlab's reverse coefficient indexing...
*(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) =
spline.getPolynomial(static_cast<int>(segment_index))
.GetCoefficients()[coefficient_index];
}
}
}
plhs[1] = stdVectorToMatlab(best_segment_times);
if (nlhs > 2) plhs[2] = mxCreateDoubleScalar(min_objective_value);
}
void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
string usage = "[coefs, ts, objective_value] = twoWaypointCubicSplineFreeKnotTimesmex.cpp(t0, tf, xs, xd0, xdf)";
if (nrhs != 5)
mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfInputs", usage.c_str());
if (nlhs < 2 || nlhs > 3)
mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfOutputs", usage.c_str());
double t0 = mxGetPrSafe(prhs[0])[0];
double tf = mxGetPrSafe(prhs[1])[0];
MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[2]);
auto xd0 = matlabToEigen<Dynamic, 1>(prhs[3]);
auto xdf = matlabToEigen<Dynamic, 1>(prhs[4]);
mwSize ndof = static_cast<mwSize>(xs.rows());
mwSize num_segments = 3;
mwSize num_coeffs_per_segment = 4;
mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
plhs[0] = mxCreateNumericArray(num_segments, dims, mxDOUBLE_CLASS, mxREAL);
std::vector<double> segment_times;
segment_times.resize(static_cast<size_t>(num_segments) + 1);
segment_times[0] = t0;
segment_times[static_cast<size_t>(num_segments)] = tf;
std::vector<double> best_segment_times = segment_times;
double t_step = (tf - t0) / GRID_STEPS;
double min_objective_value = numeric_limits<double>::infinity();
for (int t1_index = 0; t1_index < GRID_STEPS; t1_index++) {
segment_times[1] = t0 + t1_index * t_step;
for (int t2_index = t1_index; t2_index < GRID_STEPS; t2_index++) {
segment_times[2] = t0 + t2_index * t_step;
bool valid_solution = true;
double objective_value = 0.0;
for (int dof = 0; dof < ndof && valid_solution; dof++) {
try {
PiecewisePolynomial<double> spline = twoWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof], xs(dof, 3), xdf[dof], xs(dof, 1), xs(dof, 2));
PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
acceleration_squared *= acceleration_squared;
PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
objective_value += acceleration_squared_integral.value(spline.getEndTime()) - acceleration_squared_integral.value(spline.getStartTime());
}
catch (ConstraintMatrixSingularError&) {
valid_solution = false;
}
}
if (valid_solution && objective_value < min_objective_value) {
best_segment_times[1] = segment_times[1];
best_segment_times[2] = segment_times[2];
min_objective_value = objective_value;
}
}
}
for (mwSize dof = 0; dof < ndof; dof++) {
PiecewisePolynomial<double> spline = twoWaypointCubicSpline(best_segment_times, xs(dof, 0), xd0[dof], xs(dof, 3), xdf[dof], xs(dof, 1), xs(dof, 2));
for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) {
for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) {
mwSize sub[] = {dof, segment_index, num_coeffs_per_segment - coefficient_index - 1}; // Matlab's reverse coefficient indexing...
*(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(segment_index).getCoefficients()[coefficient_index];
}
}
}
plhs[1] = stdVectorToMatlab(best_segment_times);
if (nlhs > 2)
plhs[2] = mxCreateDoubleScalar(min_objective_value);
}