Open zzzbbs opened 1 month ago
// Get path interval
const toppra::Bound path_interval = geom_path->pathInterval();
// Get path derivatives at start and end of path
const double T = path_interval[1];
const toppra::Vector deriv0 = geom_path->eval_single(0, 1); // \frac{dg(s)}{ds}(0)
const double deriv0_norm = deriv0.norm();
const toppra::Vector derivT = geom_path->eval_single(T, 1); // \frac{dg(s)}{ds}(T)
const double derivT_norm = derivT.norm();
// Create TOPPRA problem
auto topp_prob = std::make_shared<toppra::algorithm::TOPPRA>(constraints, geom_path);
topp_prob->setN(250);
topp_prob->solver(std::make_shared<toppra::solver::Seidel>());
topp_prob->initialize(); // sets up matrices in required size
// compute sdT
double sdT = vel_acc_constraint->getSdMax(*geom_path, T, true);
// compute sd0 based on controllable sets at s=0
toppra::Bound sdT_lb_ub{0, sdT};
auto ret_code_ctlb = topp_prob->computeControllableSets(sdT_lb_ub);
if (ret_code_ctlb != toppra::ReturnCode::OK) {
logger->error("TOPPRA Unable to compute controllable sets. Return code: {}. sdT: {}",
int(ret_code_ctlb), sdT);
assert(false);
return false;
}
const auto &par_data = topp_prob->getParameterizationData(); // only for controllable sets
const double sd0_lb = std::sqrt(par_data.controllable_sets(0, 0));
const double sd0_ub = std::sqrt(par_data.controllable_sets(0, 1)) * 0.999;
double sd0 = 0; // \frac{ds}{dt}(0)
if (deriv0_norm > 1e-3) {
const double vel0_lb = deriv0_norm * sd0_lb;
const double vel0_ub = deriv0_norm * sd0_ub;
const double vel0_proj = deriv0.dot(start_state_.vel.cast<double>()) / deriv0_norm;
sd0 = std::max(vel0_lb, std::min(vel0_ub, vel0_proj)) / deriv0_norm;
}
// Compute parameterization
// Note: par_data_final.gridpoints is the grid points used for solving the discretized problem.
// par_data_final.parametrization is the output parametrization (squared path velocity)
auto ret_code = topp_prob->computePathParametrization(sd0, sdT);
Thank you very much for your kind sharing! I'll do my best to understand. I have two more questions,
geom_path
with tangential initial and final velocities?BTW, can you please edit your comment above so that my logger-> line matches yours.
Thank you for solving my problems! I have changed my comment to fit with your comment.
Hi, thank you for sharing such wonderful work! I want to generate 7-dof joint trajectory using non-zero start and end velocity. I have checked out the previous issues about this problem, but I can't understand what to do. Do I need to make the position trajectory to have initial tangent velocity? After that, do I need to change s_dot?