expose diff-flatness FF enabler as launch arg

*use launch param to enable/disable FF in controller

Author: ajshank

freyja_controller.launch

@@ -20,8 +20,9 @@
   <arg name="use_waypoint_handler"      default="false" />
 
   <!-- advanced settings -->
-  <arg name="use_velctrl_only"          default="false"/>
+  <arg name="use_velctrl_only"          default="false" />
   <arg name="bias_compensation"         default="false" />
+  <arg name="enable_flatness_ff"        default="false" />
 
   <!-- Used for examples -->
   <arg name="use_examples"              default="false"/>
@@ -49,6 +50,7 @@
   	<param name="bias_compensation"   type="string" value="$(arg bias_compensation)" />
   	<param name="mass_estimation"     type="bool"   value="true" />
   	<param name="mass_correction"     type="bool"   value="false" />
+  	<param name="enable_flatness_ff"  type="bool"   value="$(arg enable_flatness_ff)" />
   </node>
 
   <node name="lqr_vel_controller" pkg="lqr_control" type="lqr_vel_ctrl_node"
@@ -77,8 +79,12 @@
 
 
   <!-- handler for discrete waypoints -->
-  <node name="waypoint_manager" pkg="waypoint_manager" type="waypoint_manager_node" output="screen"
-				if="$(eval use_waypoint_handler)"> 
+  <node name="waypoint_manager" pkg="waypoint_manager" type="waypoint_manager_node"
+        output="screen"  if="$(eval use_waypoint_handler and not use_examples)"> 
+    <param name="init_pn"       type="double"     value="0.0" />
+    <param name="init_pe"       type="double"     value="0.0" />
+    <param name="init_pd"       type="double"     value="-0.5" />
+    <param name="init_yaw"      type="double"     value="0.0" />
 	</node>
 
 

lqg_controller/include/lqr_control_bias.h

@@ -41,6 +41,8 @@ class LQRController
 
   /* Reference state vector */
   Eigen::Matrix<double, 7, 1> reference_state_;
+  Eigen::Matrix<double, 4, 1> reference_ff_;
+  bool enable_flatness_ff_;
 
   /* Rate of execution for LQR's feedback */
   int controller_rate_;

lqg_controller/src/lqr_control_bias.cpp

@@ -68,6 +68,10 @@ LQRController::LQRController(BiasEstimator &b) : nh_(),
 
   f_biases_ << 0.0, 0.0, 0.0;
 
+  /* Differential flatness feed-forward accelerations */
+  priv_nh_.param( "enable_flatness_ff", enable_flatness_ff_, bool(false) );
+  reference_ff_ << 0.0, 0.0, 0.0, 0.0;
+  
 
   /* Mass estimation */
   enable_dyn_mass_correction_ = false;
@@ -187,6 +191,7 @@ void LQRController::trajectoryReferenceCallback( const TrajRef::ConstPtr &msg )
   reference_state_ << msg->pn, msg->pe, msg->pd, msg->vn, msg->ve, msg->vd, msg->yaw;
   reference_state_mutex_.unlock();
 
+  reference_ff_ << msg->an, msg->ae, msg->ad, 0.0;    // only NED accelerations
   have_reference_update_ = true;
 }
 
@@ -216,7 +221,8 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   else
   {
     /* Compute control inputs (accelerations, in this case) */
-    control_input = -1 * lqr_K_ * reduced_state_;
+    control_input = -1 * lqr_K_ * reduced_state_ 
+                    + static_cast<double>(enable_flatness_ff_) * reference_ff_;
 
     /* Force saturation on downward acceleration */
     control_input(2) = std::min( control_input(2), 8.0 );