How to use TOTG for time parameterization ?

[vegfood]

I follow the basic_cartesian_example in tesseract_ros_examples and i notice that it uses spline for time parameterization. In moveit, i can choose TOTG for time paramererization, so i wanna know how can this be done in tesseract ?

[Levi-Armstrong]

Yes, take a look at this planning_server.loadDefaultProcessPlanners(); in the example. This is where all of the motion pipelines get added. You would need to copy the function for the pipelinei you are interested in and switch it out ISP with TOTG.

That example is using the trajopt pipeline so you would copy this function and replace it with TOTG where ISP is.

tesseract_planning/tesseract_process_managers/src/core/default_process_planners.cpp

Lines 55 to 120 in f966455

	TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_collision_check, bool post_smoothing)
	{
	auto tf = std::make_unique<GraphTaskflow>("TrajOptTaskflow");
	int check_input_task{ std::numeric_limits<int>::min() };
	if (check_input)
	check_input_task = tf->addNode(std::make_unique<CheckInputTaskGenerator>(), true);
	// Check if seed was provided
	int has_seed_task = tf->addNode(std::make_unique<HasSeedTaskGenerator>(), true);
	// Simple planner as interpolator
	auto interpolator = std::make_shared<SimpleMotionPlanner>();
	int interpolator_task = tf->addNode(std::make_unique<MotionPlannerTaskGenerator>(interpolator), true);
	// Setup Seed Min Length Process Generator
	// This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is
	// to short.
	int seed_min_length_task = tf->addNode(std::make_unique<SeedMinLengthTaskGenerator>());
	// Setup TrajOpt
	auto motion_planner = std::make_shared<TrajOptMotionPlanner>();
	int motion_planner_task = tf->addNode(std::make_unique<MotionPlannerTaskGenerator>(motion_planner), true);
	// Setup post collision check
	int contact_check_task{ std::numeric_limits<int>::min() };
	if (post_collision_check)
	contact_check_task = tf->addNode(std::make_unique<DiscreteContactCheckTaskGenerator>(), true);
	// Setup time parameterization
	int time_parameterization_task = tf->addNode(std::make_unique<IterativeSplineParameterizationTaskGenerator>(), true);
	// Setup trajectory smoothing
	int smoothing_task{ std::numeric_limits<int>::min() };
	if (post_smoothing)
	smoothing_task = tf->addNode(std::make_unique<RuckigTrajectorySmoothingTaskGenerator>(), true);
	if (check_input)
	tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task });
	tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task });
	tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task });
	tf->addEdges(seed_min_length_task, { motion_planner_task });
	if (post_collision_check)
	{
	tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, contact_check_task });
	tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task });
	}
	else
	{
	tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task });
	}
	if (post_smoothing)
	{
	tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task });
	tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE });
	}
	else
	{
	tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
	}
	return tf;
	}

Then register this function with the planning server under some name.

planning_server.registerProcessPlanner(process_planner_names::TRAJOPT_PLANNER_NAME + "_TOTG", createTrajOptTOTGGenerator());

Then update this line request.name = process_planner_names::TRAJOPT_PLANNER_NAME; with request.name = process_planner_names::TRAJOPT_PLANNER_NAME + "_TOTG";

[vegfood]

Thanks, it works