void OmniDrivePositionController::setTargetTrajectory(const TrajectoryWithId& trajectory) {
    resetFlags();

    targetTrajectory.setTrajectory(trajectory.getTrajectory());
    targetTrajectory.id = trajectory.id;

}
TrajectoryWithId::TrajectoryWithId(const TrajectoryWithId& orig) {
    id = orig.id;
    trajectory = orig.getTrajectory().Clone();
}