Writing Trajectories

Quick Reference

Topic Name (publish to): /reference_state
Message Type: freyja_msgs::ReferenceState
Consumer Node(s): lqg_controller
Recommended Rate: 0Hz (last reference remains latched) -- lc Hz (lc is the controller rate, usually 40-50Hz).

Basic structure

Freyja requires continuous trajectory references, x_r, for the controller. This trajectory reference is, at minimum, a 7-vector of the form: [pn, pe, pd, vn, ve, vd, yaw]. This is a continuous "state" that the controller will attempt to maintain. Therefore, vn must be the first time-derivative of pn, and so on. This is often best written in a "time-parameterised" form. For instance, to move in a straight line northwards at a velocity of 0.5 (m/s), it must be that:

pn = 0.5 * t + c;
vn = 0.5;

with c as some constant offset, and t as the time.
On a similar theme, note that:

pn = 0.5 * t * t + c;
vn = 0.5 * t;

may seem to be a parabolic curve, but it is parabolic only in time. The path-point still moves in the same straight-line northwards as before (but now with an ever-increasing velocity). It also follows that if all the position references are constants (are not functions of time), then the corresponding velocity elements must be zero.

Feed-forward

Freyja also supports acceleration feed-forward: the full (actual) trajectory reference vector looks like so:
[pn, pe, pd, vn, ve, vd, yaw, an, ae, ad]
The last 3 elements are used internally as "feed-forward" components for the controller, and are not technically a "reference" that it chases. Note that if you only use the first form (7 elements), ROS automatically fills the acceleration terms with zeros (which is good). Enabling feed-forward is masked behind another launch argument, enable_flatness_ff, which must be set to true for these terms to take effect.

Example

Here's a ROS C++ snippet for flying in a unit circle at a constant height:

freyja_msgs::ReferenceState refstate;
ros::Time init_time = ros::Time::now();
while( ros::ok() )
{
  float t = (ros::Time::now() - init_time).toSec();
  /* position references */
  refstate.pn = std::sin(t);
  refstate.pe = std::cos(t);
  refstate.pd = -1.0;
  /* velocity references */
  refstate.vn = std::cos(t);
  refstate.ve = -std::sin(t);
  refstate.vd = 0.0;
  /* fill acceleration refs if needed.. */

  /* publish */
  refpublisher.publish( refstate );
  
  /* sleep for 0.05s and spin once to "refresh" ROS queues. */
  ros::Duration(0.05).sleep();
  ros::spinOnce();
}

More examples and complete code can be found under /freyja_examples directory.