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 }