void testPlotting(std::vector< Matrix<P_DIM> >& waypoints) {
std::vector<Matrix<B_DIM> > B(T*NUM_WAYPOINTS, zeros<B_DIM,1>());
Matrix<P_DIM> start = zeros<P_DIM,1>();
int index = 0;
for(int i = 0; i < NUM_WAYPOINTS; ++i) {
std::vector<Matrix<P_DIM> > interp = linearlyInterpolate(start, waypoints[i], T);
for(int t = 0; t < T; ++t) {
B[index++].insert<P_DIM>(0, 0, interp[t]);
}
start = waypoints[i];
}
pythonDisplayTrajectory(B, waypoints, T*NUM_WAYPOINTS);
}
bool MissionControlSpeedFilter::setToInterpolated(const MissionControlBase& missionController1, const MissionControlBase& missionController2, double t) {
const MissionControlSpeedFilter& controller1 = static_cast<const MissionControlSpeedFilter& >(missionController1);
const MissionControlSpeedFilter& controller2 = static_cast<const MissionControlSpeedFilter& >(missionController2);
maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().x() = linearlyInterpolate(controller1.getMaximumBaseTwistInHeadingFrame().getTranslationalVelocity().x(),
controller2.getMaximumBaseTwistInHeadingFrame().getTranslationalVelocity().x(), 0.0, 1.0, t);
maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().y() = linearlyInterpolate(controller1.getMaximumBaseTwistInHeadingFrame().getTranslationalVelocity().y(),
controller2.getMaximumBaseTwistInHeadingFrame().getTranslationalVelocity().y(), 0.0, 1.0, t);
maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().z() = linearlyInterpolate(controller1.getMaximumBaseTwistInHeadingFrame().getTranslationalVelocity().z(),
controller2.getMaximumBaseTwistInHeadingFrame().getTranslationalVelocity().z(), 0.0, 1.0, t);
maximumBaseTwistInHeadingFrame_.getRotationalVelocity().z() = linearlyInterpolate(controller1.getMaximumBaseTwistInHeadingFrame().getRotationalVelocity().z(),
controller2.getMaximumBaseTwistInHeadingFrame().getRotationalVelocity().z(), 0.0, 1.0, t);
minimalDesiredPositionOffsetInWorldFrame_ = linearlyInterpolate(controller1.getMinimalPositionOffsetInWorldFrame(),
controller2.getMinimalPositionOffsetInWorldFrame(), 0.0, 1.0, t);
maximalDesiredPositionOffsetInWorldFrame_ = linearlyInterpolate(controller1.getMaximalPositionOffsetInWorldFrame(),
controller2.getMaximalPositionOffsetInWorldFrame(), 0.0, 1.0, t);
return true;
}
lwr_impedance_controller::CartImpTrajectoryPoint
CartImpTrajectory::sampleInterpolation() {
lwr_impedance_controller::CartImpTrajectoryPoint next_point;
double timeFromStart = (double)time_ * dt_;
double segStartTime = last_point_.time_from_start.toSec();
double segEndTime = trajectory_.trajectory[trajectory_index_].time_from_start.toSec();
//std::cout << "initial pose : [ " << trajectory_.trajectory[trajectory_index_].pose.position.x << " " << trajectory_.trajectory[trajectory_index_].pose.position.y << " " << trajectory_.trajectory[trajectory_index_].pose.position.z << " ] [ " << trajectory_.trajectory[trajectory_index_].pose.orientation.x << " " << trajectory_.trajectory[trajectory_index_].pose.orientation.y << " " << trajectory_.trajectory[trajectory_index_].pose.orientation.z << " " << trajectory_.trajectory[trajectory_index_].pose.orientation.w << " ]" << std::endl;
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
// 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;
}
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;
}
