OMPL planner doesn't provide a trajectory about tesseract

[mpowelson]

The PlannerResponse has a joint_trajectory that should be filled with the solution. The trajectory is an Eigen matrix timesteps x num_dof.

However, if you are saying that that matrix only has 2 rows then that is another issue. You could make sure that you set simply=false in the OMPLFreespacePlannerConfig. However we have experienced very short trajectories even for quite long paths. It is something we have meant to look into. I believe that theoretically you should be able to joint interpolate between them and it should still satisfy the validators. But again, the OMPL planner is pretty new and there are still some kinks to work out.

Here's some code that we have added after OMPL recently before passing the ompl results (ompl_seed) to trajopt.

  int min_samples = 30;
  if (ompl_seed.rows() < min_samples)
  {
    int spacing = static_cast<int>(ceil(static_cast<double>(min_samples)) / static_cast<double>(ompl_seed.rows()));
    ROS_INFO("Interpolating with spacing %d", spacing);
    trajopt::TrajArray interpolated_seed(spacing * (ompl_seed.rows() - 1), ompl_seed.cols());
    for (int row = 0; row < ompl_seed.rows() - 1; row++)
    {
      for (int idof = 0; idof < ompl_seed.cols(); ++idof)
        interpolated_seed.block(spacing * row, idof, spacing, 1) =
            Eigen::VectorXd::LinSpaced(spacing, ompl_seed.row(row)(idof), ompl_seed.row(row + 1)(idof));
    }

[marip8]

@nicolacastaman @mpowelson #160 adds a variable to the OMPL configuration structure that allows you to specify the number of output states you want in your trajectory

[marip8]

Closing this issue because it seems to be addressed in #160. Feel free to re-open if any further discussion is required