How to solve trajopt problem using PCI(Problem Construction Info) and trajopt::ConstructProblem

[decdox]

In an earlier version (maybe a 2020 release) of tesseract and trajopt, a trajectory optimization problem can be constructed by such a workflow (part of my code):

    // create the problem construction info
    trajopt::ProblemConstructionInfo pci(env);
    // populate basic info
    pci.basic_info.n_steps = steps_per_phase * 2; 
    pci.basic_info.manip = "Manipulator";  
    pci.basic_info.dt_lower_lim = dt;  
    pci.basic_info.dt_upper_lim = dt;
    pci.basic_info.use_time = false;
    // add a joint velocity (jv) cost 
    if (true)
    {
      std::shared_ptr<trajopt::JointVelTermInfo> jv(new trajopt::JointVelTermInfo);

      jv->name = "joint_velocity_cost";
      jv->coeffs = std::vector<double>(7, 5.0);  // scale the cost
      jv->targets = std::vector<double>(7, 0.0);  // velocity targets why 0 ?
      jv->first_step = 0;
      jv->last_step = pci.basic_info.n_steps - 1;
      jv->term_type = trajopt::TT_COST;

      pci.cost_infos.push_back(jv);
    }
    // add an acceleration cnt
    if (true)
    {
      std::shared_ptr<trajopt::JointAccTermInfo> ja(new trajopt::JointAccTermInfo);

      // taken from kinova gen3 tutorial (rad/s) and scaled by 0.8
      std::vector<double> acc_upper_lim {4.16, 4.16, 4.16, 4.16, 8.0, 8.0, 8.0};
      std::vector<double> acc_lower_lim {-4.16, -4.16, -4.16, -4.16, -8.0, -8.0, -8.0};

      ja->name = "joint_acceleration_cnt";
      ja->coeffs = std::vector<double>(7, 50.0);
      ja->targets = std::vector<double>(7, 0.0);
      ja->upper_tols = acc_upper_lim;
      ja->lower_tols = acc_lower_lim;
      ja->first_step = 0;
      ja->last_step = pci.basic_info.n_steps - 1;
      ja->term_type = (trajopt::TT_CNT);

      pci.cnt_infos.push_back(ja);
    }

    // formulate the optimization problem
    TrajOptProbPtr prob = trajopt::ConstructProblem(pci);

    return prob;

and then solve the problem with:

      // Create the planner and the responses that will store the results
      tesseract::tesseract_planning::TrajOptPlanner planner;
      tesseract::tesseract_planning::PlannerResponse planning_response;
      tesseract::tesseract_planning::PlannerResponse planning_response_place;
      // Set the optimization parameters (Most are being left as defaults)
      tesseract::tesseract_planning::TrajOptPlannerConfig config(prob);
      config.params.max_iter = 500;
      planner.solve(planning_response, config);

But the current version seems not have the solve function for a PCI-constructed trajopt problem, because the relevant funtion such as TrajOptMotionPlanner in trajopt_motion_planner.h.

So could you give me some advice how to solve this type of trajectory optimization problem? The trajopt problem is just given a target pose of the end-effector of the manipulator and i would like to add some cost and constraints (such as collision, box for collision-free, joint position limit, joint acceleration limit, time cost, invariance of the orientation of the end-effector, etc,.)

[Levi-Armstrong]

I recommend taking a look at the examples, but the configuration of the problem is now controlled by profiles. In the case of trajopt it has Solver, Plan and Composite profiles. Take a look at these in the trajopt directory and everything should be configurable using the profile. If the default profiles do not have or do what you need you can create your own profile to use.