Initial trajectory

[allicen]

Hello! How can initialization of an initial trajectory that needs optimization be added to this code https://github.com/tesseract-robotics/tesseract_planning/blob/master/tesseract_examples/src/glass_upright_example.cpp?

Perhaps there are some examples.... I've tried this, but it doesn't work:

  std::shared_ptr<trajopt::TrajArray> init_traj_tmp = std::make_shared<trajopt::TrajArray>(toTrajArray(init_traj_));
  TrajOptMotionPlanner trajopt_planner(TRAJOPT_DEFAULT_NAMESPACE);
  std::shared_ptr<trajopt::ProblemConstructionInfo> pci = trajopt_planner.createProblem(request);
  pci->basic_info.use_time = false;
  pci->init_info.type = InitInfo::GIVEN_TRAJ;
  pci->init_info.data = *init_traj_tmp;

this code doesn't work:

  trajopt::ProblemConstructionInfo pci(env_);
  trajopt::TrajOptProb::Ptr prob = trajopt::ConstructProblem(pci); 
  prob->SetInitTraj(*init_traj_tmp);

Thank you for the answer!

[allicen]

I tried to initialize the trajectory as in the example from the freespace_example.cpp. But the output ompl_response.results is equal to the output trajopt_response.results. I think it's wrong.

I also checked how the plugin OMPL (RRT, RRTConnect, etc.) works together with the TrajOpt, ЕкфоЩзе optimizes the trajectory built by OMPL. However, if I want to separately get the trajectory from OMPL and optimize it with TrajOpt, I can't do that. How to correctly send the trajectory so that TrajOpt optimizes it? I didn't find a single working example. TrajOpt shows the same result as OMPL.

Do you have any ideas? Best regards.

[Levi-Armstrong]

You could use the task composer planning pipelines which handles this for you.

[allicen]

I have solved the problem