Closed Rezenders closed 1 year ago
@Rezenders I can replicate this with a minimal setup:
System: macOS Monterey 12.6.2
Gazebo
gz sim -v4 -s -r underwater.world
SITL
sim_vehicle.py -v ArduSub --model=JSON -L RATBeach --console --map
The sub disappears as the FCU initialises. I suspect what is happening is an out of bounds PWM value is posted to the ardupilot_gazebo plugin during initialisation. This causes the thruster plugin to apply a very large force which results in a model position overflow as the physics engines numerics fail. I'll run a debug trace on the passed in values next.
If this is the case then the next step is to decide how we want to treat PWM values that are outside the expected range, and is this a plugin issue (handling the input PWMs) or a FCU / SITL initialisation problem (where we don't expect any PWMs to be passed to the motor controllers until initialisation is complete)?
I'm not sure why we are seeing this now, and not in previous versions. I have a fork of the bluerov-ignition project using the ignition branches which I'll bring up to date to see whether there has been a change there that is causing the issue. May also need to look at the thruster plugin to see if there have been changes there.
Update 1:
Ok - it's not the PWM values during initialisation:
[Dbg] [ArduPilotPlugin.cc:1476] apply input chan[0] to control chan[0] with joint name [thruster1_joint] pwm [1000] raw cmd [0] adjusted cmd [-50].
[Dbg] [ArduPilotPlugin.cc:1476] apply input chan[1] to control chan[1] with joint name [thruster2_joint] pwm [1000] raw cmd [0] adjusted cmd [-50].
[Dbg] [ArduPilotPlugin.cc:1476] apply input chan[2] to control chan[2] with joint name [thruster3_joint] pwm [1000] raw cmd [0] adjusted cmd [-50].
[Dbg] [ArduPilotPlugin.cc:1476] apply input chan[3] to control chan[3] with joint name [thruster4_joint] pwm [1000] raw cmd [0] adjusted cmd [-50].
[Dbg] [ArduPilotPlugin.cc:1476] apply input chan[4] to control chan[4] with joint name [thruster5_joint] pwm [1000] raw cmd [0] adjusted cmd [-50].
[Dbg] [ArduPilotPlugin.cc:1476] apply input chan[5] to control chan[5] with joint name [thruster6_joint] pwm [1000] raw cmd [0] adjusted cmd [-50].
[Msg] COMMAND[0] -50
[Msg] COMMAND[1] -50
[Msg] COMMAND[2] -50
[Msg] COMMAND[3] -50
[Msg] COMMAND[4] -50
[Msg] COMMAND[5] -50
The Blue ROV has the ArduPilotPlugin
configured to convert PWM from SITL to commands in the range -50N to 50N, which it is doing. It's probably not great that the thrust is set to -50N during initialisation but that should not cause the physics engine to overflow. I don't think the main issue is in SITL or this plugin (which appears to be doing what is expected of it).
The next place to look is changes to the thruster or hydrodynamics plugins (and physics). I know the added mass treatment has changed and wonder if that is affecting this vehicle?
Update 2:
I now suspect the issue may be related to the added mass changes in gz-physics6
and an unexpected impact on the hydrodynamics plugin. If the hydrodynamics plugin is disabled in the rover then there is no physics overflow. The sub can be commanded in GUIDED mode (it does not track well because of no drag, too small effective mass, poor tune etc. but it all runs).
The sub tumbles when initialising because all the thrusters are placed in full reverse - but this does not cause the sub to disappear out of the simulation bounds. What would do this is a force applied to a vehicle with an invalid (near zero) mass or invalid inertial.
Update 3:
Wrong there too... it's the quadratic damping terms causing the issue not added mass. When all the parameters are set to zero there is no overflow. The added mass and linear damping parameters are zero in the example. When the quadratic damping parameters are non-zero we see an issue (<xUU> != 0, <yVV> != 0, etc
).
Update 4:
The choice of quadratic damping parameters given to the BlueROV appear to result in an unstable simulation. I replicated the hydrodynamics code that deals with the params
<xUU>-33.800000000000004</xUU>
<yVV>-54.26875</yVV>
<zWW>-73.37135</zWW>
<kPP>-4.0</kPP>
<mQQ>-4.0</mQQ>
<nRR>-4.0</nRR>
in a Python notebook, capturing the simulation state for the first 50 steps during initialisation. I have not checked the calculations against the Fossen text. Assuming they are correct you can see that given an initial rotation (from the startup) the parameters positively feedback on the force. The main contribution is the angular velocity about x which quickly overflows.
import numpy as np
snameM = "xyzkmn"
snameV = "UVWPQR"
param = {}
param["xUU"] = -33.800000000000004
param["yVV"] = -54.26875
param["zWW"] = -73.37135
param["kPP"] = -4.0
param["mQQ"] = -4.0
param["nRR"] = -4.0
# consecutive states captured from a Gazebo run of the bluerov2
states = np.array([
[-1.37688e-17, -0.139383, 0.0652084, -12.4412, 1.10673e-16, 7.79847e-16],
[-1.31109e-17, -0.205924, 0.0731437, -18.3433, 1.7855e-16, 7.7466e-16],
[-1.07943e-17, -0.350138, 0.0759616, -31.1753, 2.23983e-16, 7.513e-16],
[-1.02972e-17, -0.764226, 0.0446482, -68.2437, 2.2405e-16, 6.70518e-16],
[-1.23131e-17, -2.66931, -0.560614, -245.884, 3.61299e-16, 3.74901e-16],
[ 6.11871e-17, 23.3168, -15.7333, -2552.04, 6.35079e-15, -1.40983e-15],
])
# consecutive calculated forces captured from a Gazebo run of the bluerov2
force_obs = np.array([
[6.40779e-33, 1.05431, 0.311985, 619.132, 4.89938e-32, 2.43265e-30],
[5.81005e-33, 2.30125, 0.392537, 1345.91, 1.27521e-31, 2.40039e-30],
[3.93828e-33, 6.65316, 0.423365, 3887.59, 2.00674e-31, 2.25781e-30],
[3.58386e-33, 31.6952, 0.146263, 18628.8, 2.00793e-31, 1.79838e-30],
[5.12449e-33, 386.676, 23.0597, 241835, 5.22149e-31, 5.62203e-31],
[1.26543e-31, 29504.6, 18162, 2.60517e+07, 1.6133e-28, 7.95044e-30],
])
# select state and observed force to check
X = states[5]
Fobs = force_obs[5]
# map params to coefficient array
quadDamp = np.zeros([6, 6, 6])
for i in range(6):
for j in range(6):
prefix = snameM[i]
prefix += snameV[j]
for k in range(6):
key = prefix + snameV[k]
if key in param:
# print("[{}, {}, {}] {} {}".format(i, j, k, key, param[key]))
quadDamp[i, j, k] = param[key]
# force coefficients
D = np.zeros([6, 6])
for i in range(6):
for j in range(6):
coeff = 0
for k in range(6):
coeff -= quadDamp[i, j, k] * X[k]
D[i, j] = coeff
# force-torque 6-vector
F = np.matmul(D, X)
# compare
with np.printoptions(precision=3, suppress=True):
print("D:\n{}".format(D))
print("F:\n{}".format(F))
print("Fobs:\n{}".format(Fobs))
D:
[[ 0. 0. 0. 0. 0. 0. ]
[ 0. 1265.374 0. 0. 0. 0. ]
[ 0. 0. -1154.373 0. 0. 0. ]
[ 0. 0. 0. -10208.16 0. 0. ]
[ 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. -0. ]]
F:
[ 0. 29504.463 18162.104 26051632.646 0.
0. ]
Fobs:
[ 0. 29504.6 18162. 26051700. 0. 0. ]
The result is expected given the plugin formula, the input parameters and the initial rotation imparted to the sub during initialisation.
Final note:
The formula for the quadratic damping changed in https://github.com/gazebosim/gz-sim/commit/fde6ec608bbd1679178c70c62bdc08db41eb38d6.
// Previous calculation was effectively (different formulation as the result had not been generalised).
auto velCoeff =
this->dataPtr->stabilityQuadraticDerivative[index] * abs(state(k));
// Latest version has abs() removed.
auto velCoeff =
this->dataPtr->stabilityQuadraticDerivative[index] * state(k);
This needs to go back to the Gazebo team for resolution.
Closing as needs to be resolved upstream.
Wow, thanks for the detailed analyzes and explanation @srmainwaring! I really appreciate it.
Bug report
When I run a gazebo simulation with the ardupilot plugin loaded, it crashes immediately when ardupilot connects via mavros. This started to happen after gazebo got upgraded to version 7.2. More info at: https://github.com/gazebosim/gz-sim/issues/1871 and https://github.com/gazebosim/gz-sim/issues/15
I am not sure if this is a bug related to the plugin or gazebo.
gazebo output:
Version
Platform [ ] All [ ] AntennaTracker [ ] Copter [ ] Plane [ ] Rover [ X ] Submarine
Hardware type ArduSub SITL commit https://github.com/ArduPilot/ardupilot/tree/9f1c4df5e7