Trajopt: Setting an Initial Trajectory as a "Warm Start"?

[PikaTheLeg]

Currently been using code based around the basic_cartesian_examples, which seem to use a combination of profiles and taskflow to setup the problem and solve. I am trying to feed TrajOpt a trajectory which I have saved from previous calls of TrajOpt and saved using the format of trajectory_msgs/JointTrajectory, but I only want this trajectory to be a "guiding" hand only - which means the trajectory I feed is similar to the start and end goal, but not necessarily correct.

I had a look through a few potential functions I might need, such as ProblemConstructionInfo's init_info.data (TrajArray), but I am currently struggling figuring out how to access those with just profiles. Perhaps it is something that I need to define in the program (CompositeInstruction) itself?

Here is what I currently got to get TrajOpt running with the "default" version used in basic_cartesian_examples:

  // Create Program
  CompositeInstruction program(
      "cartesian_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("arm", "base_link", "gripper_link"));

  // Start Joint Position for the program
  StateWaypointPoly wp0{ StateWaypoint(arm_joint_names, other_pose) };
  MoveInstruction start_instruction(wp0, MoveInstructionType::START);
  program.setStartInstruction(start_instruction);

  // Some cartesian waypoint that I am setting
  CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * trans * rot * input_quat) };
  MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE");
  plan_f0.setDescription("from_start_plan");
  program.appendMoveInstruction(plan_f0);

  // Create Executor
  auto executor = std::make_unique<TaskflowTaskComposerExecutor>(5);

  // Create TrajOpt Profile
  auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();

  auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
  trajopt_composite_profile->longest_valid_segment_length = 0.05;
  trajopt_composite_profile->collision_constraint_config.enabled = true;
  trajopt_composite_profile->collision_constraint_config.safety_margin = 0.0;
  trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005;
  trajopt_composite_profile->collision_constraint_config.coeff = 10;
  trajopt_composite_profile->collision_cost_config.safety_margin = 0.005;
  trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
  trajopt_composite_profile->collision_cost_config.coeff = 50;

  auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
  trajopt_solver_profile->opt_info.max_iter = 100;
  trajopt_solver_profile->opt_info.merit_coeff_increase_ratio = 5;

  auto post_check_profile = std::make_shared<ContactCheckProfile>();

  // Create profile dictionary
  auto profiles = std::make_shared<ProfileDictionary>();
  profiles->addProfile<TrajOptPlanProfile>(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
  profiles->addProfile<TrajOptCompositeProfile>(
      profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
  profiles->addProfile<TrajOptSolverProfile>(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile);

  profiles->addProfile<ContactCheckProfile>(
      node_names::DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile);
  profiles->addProfile<ContactCheckProfile>(
      node_names::DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile);

  // Create Task Input Data
  TaskComposerDataStorage input_data;
  input_data.setData("input_program", program);

  // Create Task Composer Problem
  TaskComposerProblem problem(env_, input_data);

  // Solve task
  TaskComposerInput pick_input(problem, profiles);
  TrajOptMotionPipelineTask pick_task("input_program", "output_program");
  TaskComposerFuture::UPtr pick_future = executor->run(pick_task, pick_input);
  pick_future->wait(); 

It's somewhat similar to defining a seed trajectory in Issue #199, except that I already have a warm trajectory without needing to use another planner, I am simply feeding it in as an input.

[Levi-Armstrong]

In your case you have a few options. Write a simple planner which does what you want with the provided trajectory. The other option is to manual create the full composite program based on your joint trajectory and assign the state as a seed to the cartesian waypoint.