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 testIntegralAndDerivative() {
vector<Polynomial<CoefficientType>> polynomials;
int num_coefficients = 5;
int num_segments = 3;
typedef typename Polynomial<CoefficientType>::CoefficientsType CoefficientsType;
for (int i = 0; i < num_segments; ++i) {
CoefficientsType coefficients = CoefficientsType::Random(num_coefficients);
polynomials.push_back(Polynomial<CoefficientType>(coefficients));
}
// differentiate integral, get original back
PiecewisePolynomial<CoefficientType> piecewise(polynomials, generateSegmentTimes(num_segments));
PiecewisePolynomial<CoefficientType> piecewise_back = piecewise.integral().derivative();
if (!piecewise.isApprox(piecewise_back, 1e-10))
throw runtime_error("wrong");
// check value at start time
double value_at_t0 = uniform(generator);
PiecewisePolynomial<CoefficientType> integral = piecewise.integral(value_at_t0);
valuecheck(value_at_t0, integral.value(piecewise.getStartTime()), 1e-10);
// check continuity at knot points
for (int i = 0; i < piecewise.getNumberOfSegments() - 1; ++i) {
valuecheck(integral.getPolynomial(i).value(integral.getDuration(i)), integral.getPolynomial(i + 1).value(0.0));
}
}
double randomSpeedTest(int ntests) {
double ret = 0.0;
int num_segments = 3;
vector<double> segment_times = generateSegmentTimes(num_segments);
for (int i = 0; i < ntests; i++) {
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);
ret += result.value(0.0);
}
return ret;
}
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();
}
}
ExponentialPlusPiecewisePolynomial<double> s1Trajectory(const TVLQRData &sys, const PiecewisePolynomial<double> &zmp_trajectory,const Ref<const MatrixXd> &S) {
size_t n = static_cast<size_t>(zmp_trajectory.getNumberOfSegments());
int d = zmp_trajectory.getSegmentPolynomialDegree(0);
int k = d + 1;
for (size_t i = 1; i < n; i++) {
assert(zmp_trajectory.getSegmentPolynomialDegree(i) == d);
}
VectorXd dt(n);
std::vector<double> breaks = zmp_trajectory.getSegmentTimes();
for (size_t i = 0; i < n; i++) {
dt(i) = breaks[i + 1] - breaks[i];
}
MatrixXd zmp_tf = zmp_trajectory.value(zmp_trajectory.getEndTime());
PiecewisePolynomial<double> zbar_pp = zmp_trajectory - zmp_tf;
Matrix2d R1i = sys.R1.inverse();
MatrixXd NB = sys.N.transpose() + sys.B.transpose() * S; //2 x 4
Matrix4d A2 = NB.transpose() * R1i * sys.B.transpose() - sys.A.transpose();
MatrixXd B2 = 2 * (sys.C.transpose() - NB.transpose() * R1i * sys.D) * sys.Qy; //4 x 2
Matrix4d A2i = A2.inverse();
MatrixXd alpha = MatrixXd::Zero(4, n);
vector<MatrixXd> beta;
VectorXd s1dt;
for (size_t i = 0; i < n ; i++) {
beta.push_back(MatrixXd::Zero(4, k));
}
for (int j = static_cast<int>(n) - 1; j >= 0; j--) {
auto poly_mat = zbar_pp.getPolynomialMatrix(j);
size_t nq = poly_mat.rows();
MatrixXd poly_coeffs = MatrixXd::Zero(nq, k);
for (size_t x = 0; x < nq; x++) {
poly_coeffs.row(x) = poly_mat(x).getCoefficients().transpose();
}
beta[j].col(k - 1) = -A2i * B2 * poly_coeffs.col(k - 1);
for (int i = k - 2; i >= 0; i--) {
beta[j].col(i) = A2i * ((i+1) * beta[j].col(i + 1) - B2 * poly_coeffs.col(i));
}
if (j == n - 1) {
s1dt = VectorXd::Zero(4);
} else {
s1dt = alpha.col(j+1) + beta[j + 1].col(0);
}
VectorXd dtpow(k);
for (size_t p = 0; p < k; p++) {
dtpow(p) = pow(dt(j), static_cast<int>(p));
}
alpha.col(j) = (A2*dt(j)).eval().exp().inverse() * (s1dt - beta[j]*dtpow);
}
vector<PiecewisePolynomial<double>::PolynomialMatrix> polynomial_matrices;
for (int segment = 0; segment < n ; segment++) {
PiecewisePolynomial<double>::PolynomialMatrix polynomial_matrix(4, 1);
for(int row = 0; row < 4; row++) {
polynomial_matrix(row) = Polynomial<double>(beta[segment].row(row));
}
polynomial_matrices.push_back(polynomial_matrix);
}
PiecewisePolynomial<double> pp_part = PiecewisePolynomial<double>(polynomial_matrices, breaks);
auto s1traj = ExponentialPlusPiecewisePolynomial<double>(Matrix4d::Identity(), A2, alpha, pp_part);
return s1traj;
}
