22 std::vector<double_t> ini_pos, std::vector<double_t> ini_vel, std::vector<double_t> ini_acc,
23 std::vector<double_t> fin_pos, std::vector<double_t> fin_vel, std::vector<double_t> fin_acc,
24 std::vector<double_t> via_pos, std::vector<double_t> via_vel, std::vector<double_t> via_acc)
32 double time_size = fin_time - ini_time;
33 double time_ratio = 0.5*
ratio_*time_size;
62 Eigen::MatrixXd time_mat;
63 Eigen::MatrixXd conditions_mat;
66 time_mat <<
powDI(
ini_time_,5),
powDI(
ini_time_,4),
powDI(
ini_time_,3),
powDI(
ini_time_,2),
ini_time_, 1.0, 0.0,
67 5.0*
powDI(ini_time_,4), 4.0*
powDI(ini_time_,3), 3.0*
powDI(ini_time_,2), 2.0*
ini_time_, 1.0, 0.0, 0.0,
69 powDI(
fin_time_,5),
powDI(
fin_time_,4),
powDI(
fin_time_,3),
powDI(
fin_time_,2),
fin_time_, 1.0,
powDI(
fin_time_ -
via_time_,5)/120.0,
70 5.0*
powDI(
fin_time_,4), 4.0*
powDI(
fin_time_,3), 3.0*
powDI(
fin_time_,2), 2.0*
fin_time_, 1.0, 0.0,
powDI(
fin_time_ -
via_time_,4)/24.0,
71 20.0*
powDI(
fin_time_,3), 12.0*
powDI(
fin_time_,2), 6.0*
fin_time_, 2.0, 0.0, 0.0,
powDI(
fin_time_ -
via_time_,3)/6.0,
72 powDI(
via_time_,5),
powDI(
via_time_,4),
powDI(
via_time_,3),
powDI(
via_time_,2),
via_time_, 1.0, 0.0;
74 conditions_mat.resize(7,1);
75 conditions_mat.fill(0.0);
79 conditions_mat.coeffRef(0,0) = ini_pos[i];
80 conditions_mat.coeffRef(1,0) = ini_vel[i];
81 conditions_mat.coeffRef(2,0) = ini_acc[i];
82 conditions_mat.coeffRef(3,0) = fin_pos[i];
83 conditions_mat.coeffRef(4,0) = fin_vel[i];
84 conditions_mat.coeffRef(5,0) = fin_acc[i];
85 conditions_mat.coeffRef(6,0) = via_pos[i];
87 Eigen::MatrixXd position_coeff = time_mat.inverse() * conditions_mat;
129 time_variables_ <<
powDI(time, 5),
powDI(time, 4),
powDI(time, 3),
powDI(time, 2), time, 1.0, 0.0;
137 else if (time < fin_time_ && time >
via_time_)
140 time_variables_ <<
powDI(time, 5),
powDI(time, 4),
powDI(time, 3),
powDI(time, 2), time, 1.0,
powDI(time - via_time_,5)/120.0;
160 time_variables_ <<
powDI(time, 5),
powDI(time, 4),
powDI(time, 3),
powDI(time, 2), time, 1.0, 0.0;
168 else if (time < fin_time_ && time >
via_time_)
171 time_variables_ <<
powDI(time, 5),
powDI(time, 4),
powDI(time, 3),
powDI(time, 2), time, 1.0,
powDI(time - via_time_,5)/120.0;
191 time_variables_ <<
powDI(time, 5),
powDI(time, 4),
powDI(time, 3),
powDI(time, 2), time, 1.0, 0.0;
199 else if (time < fin_time_ && time >
via_time_)
202 time_variables_ <<
powDI(time, 5),
powDI(time, 4),
powDI(time, 3),
powDI(time, 2), time, 1.0,
powDI(time - via_time_,5)/120.0;
std::vector< double_t > cur_acc_
std::vector< double_t > getVelocity(double time)
MinimumJerkViaPoint(double ini_time, double fin_time, double via_time, double ratio, std::vector< double_t > ini_pos, std::vector< double_t > ini_vel, std::vector< double_t > ini_acc, std::vector< double_t > fin_pos, std::vector< double_t > fin_vel, std::vector< double_t > fin_acc, std::vector< double_t > via_pos, std::vector< double_t > via_vel, std::vector< double_t > via_acc)
Eigen::MatrixXd position_coeff_
virtual ~MinimumJerkViaPoint()
std::vector< double_t > fin_vel_
std::vector< double_t > via_pos_
std::vector< double_t > cur_vel_
std::vector< double_t > getAcceleration(double time)
std::vector< double_t > fin_pos_
std::vector< double_t > ini_vel_
Eigen::MatrixXd time_variables_
std::vector< double_t > ini_pos_
std::vector< double_t > getPosition(double time)
std::vector< double_t > cur_pos_
Eigen::MatrixXd velocity_coeff_
double powDI(double a, int b)
Eigen::MatrixXd acceleration_coeff_
std::vector< double_t > ini_acc_
std::vector< double_t > fin_acc_