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)
49 Eigen::MatrixXd time_mat;
50 Eigen::MatrixXd conditions_mat;
53 time_mat <<
powDI(
ini_time_,5),
powDI(
ini_time_,4),
powDI(
ini_time_,3),
powDI(
ini_time_,2),
ini_time_, 1.0,
56 powDI(
fin_time_,5),
powDI(
fin_time_,4),
powDI(
fin_time_,3),
powDI(
fin_time_,2),
fin_time_, 1.0,
60 conditions_mat.resize(6,1);
61 conditions_mat.fill(0.0);
65 conditions_mat.coeffRef(0,0) = ini_pos[i];
66 conditions_mat.coeffRef(1,0) = ini_vel[i];
67 conditions_mat.coeffRef(2,0) = ini_acc[i];
68 conditions_mat.coeffRef(3,0) = fin_pos[i];
69 conditions_mat.coeffRef(4,0) = fin_vel[i];
70 conditions_mat.coeffRef(5,0) = fin_acc[i];
72 Eigen::MatrixXd position_coeff = time_mat.inverse() * conditions_mat;
MinimumJerk(double ini_time, double fin_time, 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 > ini_pos_
std::vector< double_t > getAcceleration(double time)
std::vector< double_t > cur_pos_
std::vector< double_t > cur_vel_
Eigen::MatrixXd position_coeff_
std::vector< double_t > getPosition(double time)
std::vector< double_t > fin_vel_
Eigen::MatrixXd time_variables_
Eigen::MatrixXd acceleration_coeff_
std::vector< double_t > cur_acc_
std::vector< double_t > fin_pos_
double powDI(double a, int b)
std::vector< double_t > fin_acc_
Eigen::MatrixXd velocity_coeff_
std::vector< double_t > getVelocity(double time)
std::vector< double_t > ini_acc_
std::vector< double_t > ini_vel_