planners failing most of the time

[ahedfares]

Hello all,

I have been migrating to the new tesseract which uses the commandLanguage branch.
since i have done this migration, i am having many problems of understanding how the motion planners are supposed to work.
I am sorry for the lengthy post, but i really have many questions and would appreciate if you can assist me as i am currently using this framework in my thesis research.

In my case, the framework is not providing reliable solutions and is failing most of the time, if it succeeds, it takes many iterations for a very simple motion request although i am assigning no constraints, costs, or attaching collision objects. I am sure i am doing something wrong as the assosiated examples in the package are solving MUCH more complex problems in an incredibly shorter time thus, i would appreciate some help on guiding me towards the proper approach. I will divide this question into sub parts which we can discuss in a follow up post if needed.

1. how do i assign the number of steps required in the trajectory solution (in the previous framework, there was a parameter defining the number of steps).

2. What are the major difference between using the three planning profiles in the problem i define (TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile, TrajOptDefaultSolverProfile), which is the default one if none is assigned. If i need to assign coefficients, and collision options, do i have to use two planning profiles together?

3. in solution, there are still both constraints and costs terms for collisions, velocity, jerk, and acceleration even though i set both the
   smooth_velocity/smooth_acceleration parameters = false in "TrajOptDefaultCompositeProfile", and
   collision_constraint_config.enabled / collision_cost_config.enabled = false in "TrajOptDefaultCompositeProfile" for the same problem (assuming that i should combine multiple profiles if i need to access different settings).
   Why this is happening? isn't there any way to have control on what constraints/costs are activated similar to how it was being done in previous framework?

4. How can i assign coefficients to specific time steps like earlier in the previous framework, and how can i enable/disable collisions to specific time steps as well?

5. the solver fails many times even when there are no objects to avoid and no complex motion requested. It is just a simple plan to a specific target joint configuration or cartesian pose which is reachable. Could i be missing something in my problem, i have been trying to follow the examples provided but with no avail.

6. what is the difference in the code in freespace_hybrid_example.cpp and freespace_ompl_example, they are the exact same code.

7. demo_scene.cpp in tesseract_monitoring/demos is still using the modify_env_master and modify_env_rviz which i believe are from the previous framework (the demo is not updated to use the new functionalities)

Note/ i can provide parts of my code which are relative to these raised issues if needed.

@Levi-Armstrong i would appreciate if you can help regarding this.

Thanking you in advance.

[mpowelson]

I can answer some of these.

1. In whatever taskflow you are using there is probably a SimpleMotionPlanner task. Its job is to set that number of points. There are a couple of different implementations of SimplePlannerPlanProfile, but if you want a fixed size interpolated seed, I believe you want SimplePlannerFixedSizeAssignPlanProfile. It then has a steps member that you can set.
2. I'm told there are more docs in the works. Hopefully this will be more clear soon. Most of the Tasks have 2 kinds of profiles (sets of settings). CompositeProfiles operate on groups of instructions (CompositeInstructions). These are things like velocity smoothing. PlanProfiles operate on PlanInstructions. These are things like a goal point. TrajOpt is a bit special at the moment in that it also has a SolverProfile which defines trajopt specific parameters about the solver itself.
3. You need to make sure the profile where you are changing the settings matches the profile in the instructions. For example, if the profile is FREESPACE in your CompositeInstruction you'll want to set it like this. trajopt_planner->plan_profiles["FREESPACE"] = std::make_shared<TrajOptDefaultPlanProfile>(); If you aren't setting the profile, the default is tesseract_planning::DEFAULT_PROFILE_KEY = "DEFAULT
4. There are 2 ways. First you can assign different profiles to the instructions that different. These are meant to be different types of motions. Then you can set a profile to match each of the keys in the planner. The second option is that instructions have profile overrides where you can add a profile for a specific task directly to the instruction. This likely has some small performance penalties, but that may or may not be significant. I for example am doing this for time parameterization because I want to change the scaling factor for each point but don't want each point to have to have its own profile.
5. We definitely need better debugging tools. Those are a work in progress. I'd try turning on the info output for trajopt and see which constraints are not being satisfied. A common culprit is links that are always in collision.
6. These might just be legacy examples. I'm not sure.
7. Same as 6

Thanks for upgrading and trying it out. We are pretty excited about the new capabilities of the Taskflow and commmand language framework. There are some exciting things in the pipeline for this I think.