void QPLocomotionPlan::updateSwingTrajectory(double t_plan, BodyMotionData& body_motion_data, int body_motion_segment_index, const KinematicsCache<double>& cache) {
int takeoff_segment_index = body_motion_segment_index + 1; // this function is called before takeoff
int num_swing_segments = 3;
int landing_segment_index = takeoff_segment_index + num_swing_segments - 1;
// last three knot points from spline
PiecewisePolynomial<double>& trajectory = body_motion_data.getTrajectory();
VectorXd x1 = trajectory.value(trajectory.getEndTime(takeoff_segment_index));
VectorXd x2 = trajectory.value(trajectory.getEndTime(takeoff_segment_index + 1));
VectorXd xf = trajectory.value(trajectory.getEndTime(takeoff_segment_index + 2));
// first knot point from current position
auto x0_pose = robot.relativeTransform(cache, 0, body_motion_data.getBodyOrFrameId());
auto x0_twist = robot.relativeTwist(cache, 0, body_motion_data.getBodyOrFrameId(), 0);
Vector4d x0_quat = rotmat2quat(x0_pose.linear());
Matrix<double, QUAT_SIZE, SPACE_DIMENSION> Phi;
angularvel2quatdotMatrix(x0_quat, Phi, static_cast<Gradient<decltype(Phi), Dynamic>::type*>(nullptr));
auto quatdot = (Phi * x0_twist.topRows<3>()).eval();
auto x0_expmap_autodiff = quat2expmap(Drake::initializeAutoDiffGivenGradientMatrix(x0_quat, quatdot));
auto x0_expmap = autoDiffToValueMatrix(x0_expmap_autodiff);
auto xd0_expmap = autoDiffToGradientMatrix(x0_expmap_autodiff);
// std::cout << "x0: " << x0_expmap.value()(2) << " x1: " << x1(5) << " x2: " << x2(5) << " xf: " << xf(5) << std::endl;
typedef Matrix<double, 6, 1> Vector6d;
Vector6d x0;
x0.head<3>() = x0_pose.translation();
if (settings.use_plan_shift) {
for (auto it = settings.plan_shift_body_motion_indices.begin(); it != settings.plan_shift_body_motion_indices.end(); ++it) {
x0(*it) += plan_shift(*it);
}
}
x0.tail<3>() = x0_expmap;
Vector6d xd0;
// We set the xyz components of xd0 to zero intentionally because, if it
// happens that the foot is moving while in support, we don't want to
// continue that motion through the swing trajectory.
xd0.head<3>().setZero();
xd0.tail<3>() = xd0_expmap;
// If the current pose is pitched down more than the first aerial knot
// point, adjust the knot point to match the current pose
Vector3d unit_x = Vector3d::UnitX();
Vector3d unit_x_rotated_0 = quatRotateVec(x0_quat, unit_x);
Vector3d unit_x_rotated_1 = quatRotateVec(expmap2quat(x1.tail<3>()), unit_x);
if (unit_x_rotated_0(2) < unit_x_rotated_1(2)) {
x1.tail<3>() = quat2expmap(slerp(x0_quat, expmap2quat(x2.tail<3>()), 0.1));
}
// TODO: find a less expensive way of doing this
VectorXd xdf = trajectory.derivative().value(trajectory.getEndTime(landing_segment_index));
// Unwrap all of the knots in the trajectory to ensure we don't create a wraparound
x1.tail<3>() = closestExpmap(x0.tail<3>(), x1.tail<3>());
x2.tail<3>() = closestExpmap(x1.tail<3>(), x2.tail<3>());
auto xf_unwrap_autodiff = closestExpmap(initializeAutoDiffGivenGradientMatrix(x2.tail<3>(), xdf.tail<3>()), initializeAutoDiffGivenGradientMatrix(xf.tail<3>(), xdf.tail<3>()));
xf.tail<3>() = autoDiffToValueMatrix(xf_unwrap_autodiff);
xdf.tail<3>() = autoDiffToGradientMatrix(xf_unwrap_autodiff);
// std::cout << "after all modifications:" << std::endl;
// std::cout << "x0: " << x0(5) << " x1: " << x1(5) << " x2: " << x2(5) << " xf: " << xf(5) << std::endl;
// std::cout << "xd0: " << xd0.transpose() << " xdf: " << xdf.transpose();
auto start_it = trajectory.getSegmentTimes().begin() + takeoff_segment_index;
int num_breaks = num_swing_segments + 1;
std::vector<double> breaks(start_it, start_it + num_breaks);
for (int dof_num = 0; dof_num < x0.rows(); ++dof_num) {
PiecewisePolynomial<double> updated_spline_for_dof = nWaypointCubicSpline(breaks, x0(dof_num), xd0(dof_num), xf(dof_num), xdf(dof_num), Vector2d(x1(dof_num), x2(dof_num)));
for (int j = 0; j < updated_spline_for_dof.getNumberOfSegments(); j++) {
body_motion_data.getTrajectory().setPolynomialMatrixBlock(updated_spline_for_dof.getPolynomialMatrix(j), takeoff_segment_index + j, dof_num, 0);
}
}
// NOTE: not updating times here. If we use a spline generation method that also determines the times, remember to update those as well
}
void QPLocomotionPlan::updateSwingTrajectory(double t_plan, BodyMotionData& body_motion_data, int body_motion_segment_index, const VectorXd& qd) {
int takeoff_segment_index = body_motion_segment_index + 1; // this function is called before takeoff
int num_swing_segments = 3;
int landing_segment_index = takeoff_segment_index + num_swing_segments - 1;
// last three knot points from spline
PiecewisePolynomial<double>& trajectory = body_motion_data.getTrajectory();
VectorXd x1 = trajectory.value(trajectory.getEndTime(takeoff_segment_index));
VectorXd x2 = trajectory.value(trajectory.getEndTime(takeoff_segment_index + 1));
VectorXd xf = trajectory.value(trajectory.getEndTime(takeoff_segment_index + 2));
// first knot point from current position
auto x0_xyzquat = robot.forwardKinNew((Vector3d::Zero()).eval(), body_motion_data.getBodyOrFrameId(), 0, 2, 1);
auto& J = x0_xyzquat.gradient().value();
auto xd0_xyzquat = (J * qd).eval();
Vector4d x0_quat = x0_xyzquat.value().tail<4>(); // copying to Vector4d for quatRotateVec later on.
auto x0_expmap = quat2expmap(x0_quat, 1);
Vector3d xd0_expmap = x0_expmap.gradient().value() * xd0_xyzquat.tail<4>();
// std::cout << "x0: " << x0_expmap.value()(2) << " x1: " << x1(5) << " x2: " << x2(5) << " xf: " << xf(5) << std::endl;
typedef Matrix<double, 6, 1> Vector6d;
Vector6d x0;
x0.head<3>() = x0_xyzquat.value().head<3>();
if (settings.use_plan_shift) {
for (auto it = settings.plan_shift_body_motion_indices.begin(); it != settings.plan_shift_body_motion_indices.end(); ++it) {
x0(*it) += plan_shift(*it);
}
}
x0.tail<3>() = x0_expmap.value().tail<3>();
Vector6d xd0;
// We set the xyz components of xd0 to zero intentionally because, if it
// happens that the foot is moving while in support, we don't want to
// continue that motion through the swing trajectory.
xd0.head<3>().setZero();
xd0.tail<3>() = xd0_expmap;
// If the current pose is pitched down more than the first aerial knot
// point, adjust the knot point to match the current pose
Vector3d unit_x = Vector3d::UnitX();
auto quat1_gradientvar = expmap2quat(x1.tail<3>(), 0);
Vector3d unit_x_rotated_0 = quatRotateVec(x0_quat, unit_x);
Vector3d unit_x_rotated_1 = quatRotateVec(quat1_gradientvar.value(), unit_x);
if (unit_x_rotated_0(2) < unit_x_rotated_1(2)) {
auto quat2_gradientvar = expmap2quat(x2.tail<3>(), 0);
x1.tail<3>() = quat2expmap(slerp(x0_quat, quat2_gradientvar.value(), 0.1), 0).value();
}
// TODO: find a less expensive way of doing this
VectorXd xdf = trajectory.derivative().value(trajectory.getEndTime(landing_segment_index));
// Unwrap all of the knots in the trajectory to ensure we don't create a wraparound
x1.tail<3>() = closestExpmap(x0.tail<3>(), x1.tail<3>(), 0).value();
x2.tail<3>() = closestExpmap(x1.tail<3>(), x2.tail<3>(), 0).value();
auto xf_unwrap_gradvar = closestExpmap(x2.tail<3>(), xf.tail<3>(), 1);
xf.tail<3>() = xf_unwrap_gradvar.value();
xdf.tail<3>() = xf_unwrap_gradvar.gradient().value() * xdf.tail<3>();
// std::cout << "after all modifications:" << std::endl;
// std::cout << "x0: " << x0(5) << " x1: " << x1(5) << " x2: " << x2(5) << " xf: " << xf(5) << std::endl;
// std::cout << "xd0: " << xd0.transpose() << " xdf: " << xdf.transpose();
auto start_it = trajectory.getSegmentTimes().begin() + takeoff_segment_index;
int num_breaks = num_swing_segments + 1;
std::vector<double> breaks(start_it, start_it + num_breaks);
for (int dof_num = 0; dof_num < x0.rows(); ++dof_num) {
PiecewisePolynomial<double> updated_spline_for_dof = nWaypointCubicSpline(breaks, x0(dof_num), xd0(dof_num), xf(dof_num), xdf(dof_num), Vector2d(x1(dof_num), x2(dof_num)));
for (int j = 0; j < updated_spline_for_dof.getNumberOfSegments(); j++) {
body_motion_data.getTrajectory().setPolynomialMatrixBlock(updated_spline_for_dof.getPolynomialMatrix(j), takeoff_segment_index + j, dof_num, 0);
}
}
// NOTE: not updating times here. If we use a spline generation method that also determines the times, remember to update those as well
}
