Pitch/AOA based on dynamics calculation

[woensug-choi]

Ported dynamics calculation from the previous branch on top of current working kinematics plugin to calculate PITCH.

> Status of the previous branch : Unstable, only work for 3-DOF (pitch-X-Z)

Last time, the 6-DOF dynamics calculation was coded based on Isa, 2014 Paper modified to EPIC-DAUG Glider dynamics Overleaf. HOWEVER, it only was stable for 3-DOF (only moving forward; only pitch-X-Z calculation). The background equations are verified at MATLAB, and 3-DOF hard input comparisons look promising. But not in Gazebo (in Gazebo, the force turms are applied as force input to the model with this->link->AddRelativeForce(hydForce))

> Porting dynamics calculation for the pitch to Kinematics to replace flight model

This PR is an attempt to use dynamics based pitch calculation. Yaw controls are done kinematically. Something like a hybrid dynamics-kinematics method.

• For dynamics calculation,
  • Pitch command type of battery position frl_vehicle_msgs::UwGliderCommand::PITCH_CMD_BATT_POS should be used
  • Dynamics flag should be on https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/blob/3bd43f898435947930e04a4b54a7cc23ada91950/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro#L162
  • Dynamics properties should be defined at .xacro file https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/blob/824e8e0abaff1ac76fa144bf66ad01be39036f56/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro#L164-L186

[woensug-choi]

@greg-burgess This PR could be a solution for AOA/Pitch calculations. Could you try giving it a spin? My testings with hard-code input worked alright. To run this, we need the commanding side to give battery position frl_vehicle_msgs::UwGliderCommand::PITCH_CMD_BATT_POS for pitch command.