Need help creating PCI that minimizes time and stays within velocity constraints

[samraise]

Hi,

I'm having some issues using the JointVelTermInfo and TotalTimeTermInfo to achieve the requirements for my trajectory. My desired goal is a trajectory that:

1. Stays below a maximum velocity of 0.5 rad/s
2. Reduces time where possible
3. Isn't dependent on n_steps

I've fiddled around with this configuration a bit and either the problem doesn't converge or the 1/dt column is filled with 1s. Any guidance would be much appreciated.

  pci.kin = kin;

  pci.basic_info.n_steps = 20;
  pci.basic_info.manip = req.group_name;
  pci.basic_info.use_time = true;
  pci.basic_info.fixed_timesteps = sco::IntVec{0};
  pci.basic_info.dt_lower_lim = 0;
  pci.basic_info.dt_lower_lim = 100;

  pci.cnt_infos.clear();

  auto start_joint_cnt = std::make_shared<trajopt::JointPosTermInfo>();
  start_joint_cnt->term_type = trajopt::TermType::TT_CNT;
  start_joint_cnt->name = "start";
  start_joint_cnt->targets = start_joint_values;
  start_joint_cnt->first_step = 0;
  start_joint_cnt->last_step = 0;
  pci.cnt_infos.push_back(start_joint_cnt);

  auto joint_vel_cost = std::make_shared<trajopt::JointVelTermInfo>();
  joint_vel_cost->term_type = trajopt::TermType::TT_COST + trajopt::TermType::TT_USE_TIME;
  joint_vel_cost->name = "vel";
  joint_vel_cost->first_step = 0;
  joint_vel_cost->last_step = pci.basic_info.n_steps - 1;
  joint_vel_cost->coeffs = std::vector<double>(pci.kin->numJoints(), 10);
  joint_vel_cost->targets = std::vector<double>(pci.kin->numJoints(), 0.5);
  joint_vel_cost->lower_tols = std::vector<double>(pci.kin->numJoints(), 0);
  pci.cost_infos.push_back(joint_vel_cost);

  auto time_cost = std::make_shared<trajopt::TotalTimeTermInfo>();
  time_cost->term_type = trajopt::TermType::TT_COST + trajopt::TermType::TT_USE_TIME;
  time_cost->name = "time";
  time_cost->coeff = 100;
  pci.cost_infos.push_back(time_cost);

  auto goal_joint_cnt = std::make_shared<trajopt::JointPosTermInfo>();
  goal_joint_cnt->term_type = trajopt::TermType::TT_CNT;
  goal_joint_cnt->name = name;
  goal_joint_cnt->first_step = pci.basic_info.n_steps - 1;
  goal_joint_cnt->last_step = pci.basic_info.n_steps - 1;
  goal_joint_cnt->targets = targets;
  pci.cnt_infos.push_back(goal_joint_cnt);

  pci.init_info.type = trajopt::InitInfo::JOINT_INTERPOLATED;
  pci.init_info.data = trajopt_common::toVectorXd(goal_joint_cnt->targets);

  trajopt::TrajOptProb::Ptr prob = trajopt::ConstructProblem(pci);

  sco::BasicTrustRegionSQP opt(prob);

  opt.initialize(trajopt::trajToDblVec(prob->GetInitTraj()));
  opt.optimize();