How to generate trajectory with non-zero start and end velocity?

[zzzbbs]

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?

[mishrasubhransu]

     // 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);

[zzzbbs]

Thank you very much for your kind sharing! I'll do my best to understand. I have two more questions,

1. Does this approach work for multiple joints?
2. Do I need to generate my own geometric path geom_path with tangential initial and final velocities?

[mishrasubhransu]

1. I have a path in 3D, which is equivalent to 3 joint angles, so yes.
2. You need a geometric path, with non zero first derivative. I use a custom 5th order spline for acceleration continuity, but you should be able to use the cubic spline as shown in the toppra docs/examples.

BTW, can you please edit your comment above so that my logger-> line matches yours.

[zzzbbs]

Thank you for solving my problems! I have changed my comment to fit with your comment.

[zzzbbs]

@mishrasubhransu Sorry for the bother! I still have question on reproducing your code. The current version of toppra does not have fuction like getSdMax, what's your version on toppra?

[mishrasubhransu]

getSdMax method is not a part of toppra, it's something that you will have to write. You can compute it as follows:

// compute sd_max
// ||dq(s)/ds|| * ds/dt = cartesian_speed => ds/dt = cartesian_speed / ||dq(s)/ds||

Does this make sense?