TrajectoryTask usage

TrajectoryTasks are fed HighLevelTask to track, typically one of the following:

• PositionTask
• OrientationTask
• CoMTask
• GazeTask
• PositionBasedVisServoTask
• VectorOrientationTask.

There are few more things that can be used, but those should be a good start.

NB: the PIDTask is deprecated.

You can thus use:

• SetPointTask to servo around a fixed reference
• TrajectoryTask to follow a task-space trajectory
• TrackingTask to follow an error-space trajectory. Basically, in TrajectoryTask, the error is automatically computed from the HighLevelTask, but this is not always possible, for example if the error depends on external sensors, e.g. force sensors.
• TargetObjectiveTask to reach a target in a certain time. Warning: this gets quite unstable when you approach the time limit.

Example Sketch

To follow a 6D trajectory, one would create one task per objective:

// Load all your data here
// Either something like 
// std::vector<Eigen::Vector3d> positions = loadFromFile()
// or
// Generator positions = trajectory.generate()...
// I'll assume the latter for this example.
tasks::qp::PositionTask posTask(mbs, robotIndex, "EF", Eigen::Vector3d(0, 0, 0));
tasks::qp::OrientationTask oriTask(mbs, robotIndex, "EF", Eigen::Matrix3d::Identity());
tasks::qp::TrajectoryTask posTrajTask(mbs, robotIndex, &posTask, 100, 20, 100);
tasks::qp::TrajectoryTask oriTrajTask(mbs, robotIndex, &oriTask, 100, 20, 100);

solver.addTask(posTrajTask);
solver.addTask(oriTrajTask);

for(std::size_t i = 0; i < positions.size(); ++i)
{
    posTask.position(positions.next());
    posTrajTask.refVel(linearSpeeds.next());
    oriTask.orientation(orientations.next());
    oriTrajTask.refVel(rotationSpeeds.next());
    solver.solve();
}