Dynamics vehicle control

[woensug-choi]

Rewrote dynamics code and it's working in 6-DOF

> Status of the previous branch and PR : 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))

How it's been resolved

• Dynamics code is rewritten strictly in parallel to the validated MATLAB code
  • The MATLAB code is validated using Brian Claus's input and compared with his results
  • The code is included in this branch at directory matlab_dynamics. Performance of the MATLAB code is shown at Comparisons.png
• The model position is updated instead of model velocity or force
  • Vehicle position is calculated independently with the initial condition of vehicle spawned
  • At every update of the vehicle state, ResetPhysicsStates() function is called to strip off Gazebo's generic physics calculations.

Quickstart UwGliderCommand

- In this branch, `<use_dynamics>` handle is on by default.
- Default transient current CSV file linked with relative path is at the dave repo. (newly updated)

roslaunch glider_hybrid_whoi_gazebo start_demo_kinematics_stratified_current.launch
# At another terminal window
rostopic pub /glider_hybrid_whoi/kinematics/UwGliderCommand frl_vehicle_msgs/UwGliderCommand "pitch_cmd_type: 1
target_pitch_value: -0.0001
motor_cmd_type: 2
target_motor_cmd: 0.1
rudder_control_mode: 2
rudder_angle: 4
target_rudder_angle: 0.5
target_pumped_volume: -220.0"

Here, you see the glider ascending and yawing at a constant angle. As it rotates, the roll is also affected as seen in the bottom left camera feed. You can also see that the rotation of the propeller graphically according to the thruster power command input.

• 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/6d3a6cf6998e5cad4a4460e5b58c564262e5fbd2/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/6d3a6cf6998e5cad4a4460e5b58c564262e5fbd2/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro#L164-L186

Limitations and To-dos

• Rudder hydrodynamics coefficients
  • Currently, Rudder hydrodynamics force is modeled with a simple copy&paste of the Hull hydrodynamics (Graver's theory). Area 1/10, Lift and Drag 1/10, Moment *10. Just to check the system stability. There is much to improve how to model this including how to include the effect of ocean current.
• Heading Control
  • Currently, controlling with target heading is not included since it requires a control algorithm for target yaw in the vehicle frame.
• Elevation wing : Not included yet. It could be modeled similar to that of the rudder.

[ashishraste]

Hi @woensug-choi,

Thanks to you and this repo's authors for opensourcing this work on underwater glider dynamics and simulation.

Following our e-mail conversation, I wanted to ask the below questions. I'm commenting here since I believe this PR is yet to be merged to the main branch.

• What coordinate frame of reference is used in the Matlab script GliderDynamics? I see that mostly Fossen's equations of motion is followed, with some hull-related dynamics from Graver and Isa et.al. So, shouldn't the state variables of pitch and depth (and corresponding velocity components) follow NED convention?
• When running the Matlab script with measurements from the field trials, plots of state-variables are displayed in NED (correctly). However, for simulation only plots, the sign is reversed for e.g., for depth at line #427 and pitch at #413.

Do you know which dynamics equations might be introducing/using ENU convention in the script?

[woensug-choi]

Before all, what would be the problem of calculating in ENU frame? The PR calculates vehicle motion and injects the position of the vehicle with setModelPose. Without gazebo's internal physics engine.

[woensug-choi]

I believe I tried to follow conventions in the development note at https://www.overleaf.com/read/pqstymzccttj. @ashishraste Not sure whether you have access to the document.

[ashishraste]

Before all, what would be the problem of calculating in ENU frame? The PR calculates vehicle motion and injects the position of the vehicle with setModelPose. Without gazebo's internal physics engine.

There wouldn't be any problem if all equations of motion were in ENU frame. But this is not the case with the dynamics simulation script I believe - this is so because (kinematics, dynamics) terms like \eta, \G(\eta), etc. are in NED frame, as described in the Latex document, and the translation and rotation state variables of pitch in \eta are output in ENU frame. Would you be able to confirm/validate this if you have time?

The immediate effect I'd think would be on designing a controller which will have to keep in mind (and hence some hardcoding of signs) the negated-sign of variables like body-velocities in heave-axis v_z or w. So variables like angle-of-attack which would in theory be positive when a glider is diving would be negative if ENU values are produced.

Side notes/thoughts -

• I believe this script works well with a Gazebo model (setModelPose(...) function call) because the (kinematics) pose arguments provided by the script are in ENU frame.
• It would be good to have the dynamics update steps producing kinematics output in NED, which can then be transformed to ENU if they are to be supplied to a Gazebo model via setModelPose.

What do you think?

[woensug-choi]

@ashishraste I did find some mismatch with the latex on the Matlab script. And made an update and clean-up. Checking the original 3-DOF Matlab code (private) by Brian Claus who provided the CBSoct.mat also plotted the m_pitch with a negative sign. I am guessing that the recordings were made so. Also, the depth recording is opposite to NED since it may be recorded with the altimeter sensor. It made sense to me comparing the value of m_ballast_pumped and m_battpos. What do you think?

[woensug-choi]

• [ ] update signs in the source code to match with Latex and the Matlab script