OMPL to OpenRAVE Trajectory with custom delta times.

trajectory_with_deltatime.cpp

// Input:
// path: ompl::control::PathControl
// penv: OpenRAVE::EnvironmentBasePtr
// robot: OpenRAVE::RobotBasePtr
//
// Output: Will create an OpenRAVE trajectory from the OMPL PathControl and will also parse deltatimes. 

TrajectoryBasePtr or_traj = RaveCreateTrajectory(penv, "");
auto configuration_specification = robot->GetActiveConfigurationSpecification();
configuration_specification.AddDeltaTimeGroup();
or_traj->Init(configuration_specification);

double deltaTime = 0.0;
for (size_t i = 0; i < path.getStateCount(); ++i) {
    std::vector<double> values;
    stateSpace->copyToReals(values, path.getState(i));

    if( i > 0 ) {
        double controlDuration = path.getControlDuration(i);
        deltaTime = deltaTime + controlDuration;
    }

    values.push_back(deltaTime);
    or_traj->Insert(i, values, true);
}