Error: [ruby $(which gz) sim-1] ODE INTERNAL ERROR 1: assertion "aabbBound >= dMinIntExact && aabbBound < dMaxIntExact" failed in collide() [collision_space.cpp:460] #45
Comments
|
@Rezenders I can replicate this with a minimal setup:
System: macOS Monterey 12.6.2 Gazebo gz sim -v4 -s -r underwater.worldSITL sim_vehicle.py -v ArduSub --model=JSON -L RATBeach --console --mapThe 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] -50The Blue ROV has the 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 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 ( 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 gazebosim/gz-sim@fde6ec6. // 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! |
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: gazebosim/gz-sim#1871 and gazebosim/gz-sim#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
The text was updated successfully, but these errors were encountered: