How to add time parameterization to my optimization?

[schornakj]

I would like Trajopt to assign timesteps for each waypoint, so I can have my trajectory conform to joint velocity and acceleration goals without requiring a time parameterization postprocessing step.

So far, I've set pci.basic_info.use_time = true and I have a joint velocity cost with the TT_USE_TIME term type, which looks like this:

    std::shared_ptr<JointVelTermInfo> jv = std::shared_ptr<JointVelTermInfo>(new JointVelTermInfo);
    jv->coeffs = std::vector<double>(pci.kin->numJoints(), 0.5);
    jv->targets = std::vector<double>(pci.kin->numJoints(), 0.3);  // target joint velocity per joint in rad/s
    jv->first_step = 0;
    jv->last_step = pci.basic_info.n_steps - 1;
    jv->name = "joint_velocity_cost_with_time";
    jv->term_type = TT_COST + TT_USE_TIME;
    pci.cost_infos.push_back(jv);

The optimization converges to a reasonable freespace trajectory. The resulting trajectory is an N x 7 matrix, which is correct since I have a 6-DOF robot.

My problem is that the last column (which I've assumed is the duration elapsed since the first waypoint) is exclusively ones, which indicates that the optimization isn't working in the way I'd expect. What am I missing?

[mpowelson]

A few things.

1. The last column is 1/dt. This is because we can't have division in the SQP problem.
2. Time parameterization has not been extensively used - especially in the last 6 months (so good for you!). That said, TrajOpt has not really changed that much in that time, so it will probably work. But I do know some things like ROS 1 plotting assume it is turned off and will crash if you use it.
3. I don't know that I ever tried a equality cost with time param. It should work. With a hinge cost we always had to have a total time cost in it to keep it from being underconstrained. But an equality cost I would think would work without it. I assume you have some constraint at the beginning and end to fix the start and end position?

You might put a breakpoint or printout in the JointVelErrCalculator operator (in kinematic_terms.cpp) and just make sure it is being called. It could be that we broke something on the frontend.

[mpowelson]

@schornakj Any update on this? Did you get it to work or do we think it is broken? Or do we not know?

[schornakj]

I wasn't able to get it working in the way I expected (1/dt was always equal to 1 for every trajectory point thoughout all iterations of the optimization) so I ended up implementing velocity/acceleration limits in a postprocessing time parameterization step. I didn't get deep enough into it to determine if it's broken or if I was merely doing something wrong.