Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to) { int n_dims = y_from.size(); assert(n_dims==y_to.size()); assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3 int n_time_steps = ts.size(); int viapoint_time_step = 0; while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time) viapoint_time_step++; if (viapoint_time_step>=n_time_steps) { cerr << __FILE__ << ":" << __LINE__ << ":"; cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." << endl; return Trajectory(); } VectorXd yd_from = VectorXd::Zero(n_dims); VectorXd ydd_from = VectorXd::Zero(n_dims); VectorXd y_viapoint = y_yd_ydd_viapoint.segment(0*n_dims,n_dims); VectorXd yd_viapoint = y_yd_ydd_viapoint.segment(1*n_dims,n_dims); VectorXd ydd_viapoint = y_yd_ydd_viapoint.segment(2*n_dims,n_dims); VectorXd yd_to = VectorXd::Zero(n_dims); VectorXd ydd_to = VectorXd::Zero(n_dims); Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint); traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to)); return traj; }