What is the planned TrajOpt integration to MoveIt release date?

edit

As @Omid said, we are continuing to push on this as a side project but do not have an client projects or grants funding this effort. If you wish to get involved, your participation is very welcomed!

Thank you for your answers!

I have some progress on building TrajOpt. Although I could not build master branch of moveit (https://github.com/ros-planning/moveit) , because of some trajopt dependencies (tried cloning tesseract and trajopt_ros, but didnt solve the issue), I managed to build your private repo (https://github.com/ommmid/moveit/tree...). However, executing with panda demo.launch pipeline:=trajopt, motion planning crashes. Do you have any tips? The error message is related to the convex solver:

bpmpd_caller: /home/.../catkin_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp:35: void bpmpd_io::ser(int, T&, bpmpd_io::SerMode) [with T = char]: Assertion `n == sizeof(T)' failed. [move_group-6] process has died [pid 20480, exit code -11, cmd /home/.../catkin_ws/devel/lib/moveit_ros_move_group/move_group --debug /joint_states:=/joint_states_desired __name:=move_group __log:=/home/.../.ros/log/4ee11648-0111-11ea-8baa-2016b9417490/move_group-6.log]. log file: /home/.../.ros/log/4ee11648-0111-11ea-8baa-2016b9417490/move_group-6*.log Aborted (core dumped)

All dependencies to tesseract or any other repo are removed in this trajopt PR. you should be fine working with my private repo

I think the reason that you get the error is that the moveit_tutorial package does not have this PR merged in. The last commit in trajopt PR works with seed trajectories but the demo.launch that you run does not have that implemented yet. So please go back to one commit before the last one, this commit. This works with moveit_tutorial [melodic-devel] branch that you are running.

Thank you! I managed to build and execute the launch file. I was able to plan in Rviz, too, though the motion planning fails in many cases, because the goal position is not matching with the planned goal. Do you know the reason for that?

An other topic. If I understand correctly, the collision cost/contraint is not implemented yet, so basically we need to "merge" collision_terms into MoveIt, to successfully avoid obstacles. Am I correct?

Your initial guess plays an important role in the optimization methods. So I would suggest to provide that yourself for problem_info.init_info.data which varies based on problem_info.init_info.type. As my last PR for moveit_tutorial is merged, you can see an example of creating initial guess or seed/reference trajectory. However, in motion planning display in rviz, you do not have control on the initial guess, then it is harder to find a solution for the motion planning problem. This initial guess was hard-coded before but now that it can be set by the user look here, you need to set the initial_info.type to STATIONARY (in the yaml file) if you want to use the motion planning display in rviz

For the second question: correct, collision cost is not implemented. we need to add "CollisionTermInfo" to the codebse in MoveIt

Thank you for your help and sorry for the late reply. I can plan with interpolated reference trajectory, and now I am planning to implement collision cost and constraint first for discrete time. My only concern is if there is a simple way to get the signed distance for the collisions in MoveIt. Is the function distanceRobot of CollisionWorldFCL what I need? I would be grateful for any help, because I am not up to date around the collision checking in MoveIt...