本文整理汇总了C++中eigen::Quaternion::slerp方法的典型用法代码示例。如果您正苦于以下问题：C++ Quaternion::slerp方法的具体用法？C++ Quaternion::slerp怎么用？C++ Quaternion::slerp使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Quaternion的用法示例。
在下文中一共展示了Quaternion::slerp方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: sampleInterpolation
lwr_impedance_controller::CartImpTrajectoryPoint sampleInterpolation() {
lwr_impedance_controller::CartImpTrajectoryPoint next_point;
double timeFromStart =
(double) (now() - trajectory_.header.stamp).toSec();
double segStartTime = last_point_.time_from_start.toSec();
double segEndTime =
trajectory_.trajectory[trajectory_index_].time_from_start.toSec();
next_point = setpoint_;
// interpolate position
// x
next_point.pose.position.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.position.x,
trajectory_.trajectory[trajectory_index_].pose.position.x);
// y
next_point.pose.position.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.position.y,
trajectory_.trajectory[trajectory_index_].pose.position.y);
// z
next_point.pose.position.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.position.z,
trajectory_.trajectory[trajectory_index_].pose.position.z);
// interpolate orientation
Eigen::Quaternion<double> start = Eigen::Quaternion<double>(last_point_.pose.orientation.w,
last_point_.pose.orientation.x, last_point_.pose.orientation.y,
last_point_.pose.orientation.z);
Eigen::Quaternion<double> end = Eigen::Quaternion<double>(
trajectory_.trajectory[trajectory_index_].pose.orientation.w,
trajectory_.trajectory[trajectory_index_].pose.orientation.x,
trajectory_.trajectory[trajectory_index_].pose.orientation.y,
trajectory_.trajectory[trajectory_index_].pose.orientation.z);
double t = linearlyInterpolate(timeFromStart, segStartTime, segEndTime, 0,
1);
Eigen::Quaternion<double> rot = start.slerp(t, end);
next_point.pose.orientation.x = rot.x();
next_point.pose.orientation.y = rot.y();
next_point.pose.orientation.z = rot.z();
next_point.pose.orientation.w = rot.w();
/*
// x
next_point.pose.orientation.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.x,
trajectory_.trajectory[trajectory_index_].pose.orientation.x);
// y
next_point.pose.orientation.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.y,
trajectory_.trajectory[trajectory_index_].pose.orientation.y);
// z
next_point.pose.orientation.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.z,
trajectory_.trajectory[trajectory_index_].pose.orientation.z);
// w
next_point.pose.orientation.w = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.w,
trajectory_.trajectory[trajectory_index_].pose.orientation.w);
*/
//interpolate stiffness
// x
next_point.impedance.stiffness.force.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.force.x,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.x);
next_point.impedance.stiffness.force.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.force.y,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.y);
next_point.impedance.stiffness.force.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.force.z,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.z);
next_point.impedance.stiffness.torque.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.torque.x,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.x);
next_point.impedance.stiffness.torque.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.torque.y,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.y);
next_point.impedance.stiffness.torque.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.torque.z,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.z);
next_point.impedance.damping =
trajectory_.trajectory[trajectory_index_].impedance.damping;
next_point.wrench = trajectory_.trajectory[trajectory_index_].wrench;
return next_point;
}