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; }