From ab5960b947c9bfb22aa0cb3f754cc748c80d7dda Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 26 Jun 2023 11:25:24 -0700 Subject: [PATCH 01/26] update spring to match at-sea polytropic process Signed-off-by: Michael Anderson --- .../models/mbari_wec/model.sdf.em | 26 +- .../scripts/compute_polytropic.py | 350 +++++++++++++++++- buoy_gazebo/scripts/mbari_wec_batch | 2 +- .../PolytropicPneumaticSpring.cpp | 8 +- 4 files changed, 352 insertions(+), 34 deletions(-) diff --git a/buoy_description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf.em index de5636a4..9f018297 100644 --- a/buoy_description/models/mbari_wec/model.sdf.em +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -148,19 +148,19 @@ m = m_hc + m_piston # total mass Ap_u = 0.0127 # Area of piston in upper chamber Ap_l = 0.0115 # Area of piston in lower -Vd_u = 0.0226 # Dead volume of upper -Vd_l = 0.0463 # Dead volume of lower +Vd_u = 0.0266 # Dead volume of upper +Vd_l = 0.0523 # Dead volume of lower # TODO(andermi) unknown why 108.56 fudge factor is necessary c = 107.56*C(g, R_specific, T_ocean, rho, V_hc, m) # RHS of equation above -V0_u = 0.0338 # Volume setpoint from upper chamber polytropic PV curves -V0_l = 0.0537 # Volume setpoint from lower chamber polytropic PV curves -P0_u = 409962 # Pressure setpoint from upper PV -P0_l = 1211960 # Pressure setpoint from lower PV -T0_u = 273.15 # Tempurature setpoint for upper heat transfer -T0_l = 283.15 # Tempurature setpoint for lower heat transfer +V0_u = 0.0397 # Volume setpoint from upper chamber polytropic PV curves +V0_l = 0.0661 # Volume setpoint from lower chamber polytropic PV curves +P0_u = 422156 # Pressure setpoint from upper PV +P0_l = 1212098 # Pressure setpoint from lower PV +T0_u = 283.15 # Temperature setpoint for upper heat transfer +T0_l = 283.15 # Temperature setpoint for lower heat transfer -ignore_piston_mean_pos = False +ignore_piston_mean_pos = True if not ignore_piston_mean_pos: m_u = P0_u*V0_u / (R_specific*T0_u) # mass of N2 in upper, Ideal Gas Law m_l = P0_l*V0_l / (R_specific*T0_l) # mass of N2 in lower, Ideal Gas Law @@ -217,8 +217,8 @@ if not ignore_piston_mean_pos: true -0.10 0.05 - 1.1084 - 1.1445 + 1.4309 + 1.4367 @(V0_u) @(print(f'{P0_u:.00f}', end='')) @@ -242,8 +242,8 @@ if not ignore_piston_mean_pos: true -0.10 0.05 - 1.1079 - 1.1332 + 1.3771 + 1.3755 @(V0_l) @(print(f'{P0_l:.00f}', end='')) diff --git a/buoy_description/scripts/compute_polytropic.py b/buoy_description/scripts/compute_polytropic.py index 0b80193b..afb5b339 100755 --- a/buoy_description/scripts/compute_polytropic.py +++ b/buoy_description/scripts/compute_polytropic.py @@ -13,8 +13,31 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pandas as pd +import math + +import matplotlib.pyplot as plt import numpy as np +import pandas as pd +import scipy.optimize as spo + + +# Constants +g = 9.81 # m/s^2 +R_specific = 0.2968 # mass-specific gas constant for N2 +T_ocean = Tenv = 283.15 # K, soak temp for equilibrium +rho = 1025 # density of seawater +V_hc = 0.12 # Displacement of heave cone +m_hc = 820 # mass of heave cone +m_piston = 48 # mass of piston +m = m_hc + m_piston # total mass + +Ap_u = 0.0127 # Area of piston in upper chamber +Ap_l = 0.0115 # Area of piston in lower +Vd_u = 0.0266 # Dead volume of upper +Vd_l = 0.0523 # Dead volume of lower + +r = 0.5 # coef of heat transfer (Newton's law of cooling) +c_p = 1.04 # for N2 def find_nearest(array, value): @@ -28,39 +51,334 @@ def compute_n(P, V): X = np.hstack((np.ones_like(x), x)) y = np.log(P[:, np.newaxis]) b = np.linalg.lstsq(X, y, rcond=None)[0] + # print(f'{b=}') n = -b[1] - return n + C = b[0] + return n, C -def compute_polytropic(csv, target_P): - PV = pd.read_csv(csv).to_numpy() +def compute_polytropic(PV, target_P): P = PV[:, 1] - P *= 6894.757 V = PV[:, 0] inc = np.where(np.diff(V) >= 0) dec = np.where(np.diff(V) < 0) - n1 = compute_n(P[inc], V[inc]) - n2 = compute_n(P[dec], V[dec]) + n1, C1 = compute_n(P[inc], V[inc]) + n2, C2 = compute_n(P[dec], V[dec]) P0, nearest_idx = find_nearest(P[inc], target_P) V0 = V[inc][nearest_idx] - return float(n1), float(n2), float(P0), float(V0) + return float(n1), float(n2), float(P0), float(V0), float(C1), float(C2) + + +def load_atsea_logs(logs, plot=False): + # atsea_filenm = ['2022.09.15T12.29.14', + # '2022.09.14T16.27.55', + # '2022.09.06T20.15.25', + # '2022.09.06T19.15.21', + # '2022.09.06T03.14.17', + # '2022.09.05T23.14.01', + # '2022.09.06T17.15.13', + # '2022.09.06T16.15.09', + # '2022.09.11T19.23.19', + # '2022.09.12T13.24.31', + # '2022.09.13T17.26.23', + # '2022.09.16T19.31.18', + # '2022.09.13T20.26.35', + # '2022.09.15T19.29.42'] + + df = [pd.read_csv(csv) for csv in sorted(logs)] + df = [df_.set_index('Source ID') for df_ in df] + sc_df = [df_.loc[1] for df_ in df] # SpringController: Source ID == 1 + spring_data = [df_.loc[(1, [' Timestamp', + ' SC Range Finder (in)', + ' SC Upper PSI', + 'SC Lower PSI'])] for df_ in sc_df] + spring_data = pd.concat(spring_data) + spring_data.rename(columns={k: v for k, v in zip(spring_data.columns, + ['t', 'p', 'up', 'lp'])}, inplace=True) + spring_data['uv'] = None + spring_data['lv'] = None + spring_data.p *= 0.0254 + spring_data.up *= 6894.757 + spring_data.lp *= 6894.757 + # spring_data.uv = spring_data.p*0.0127+0.0226 # but maybe 0.0266 does better? + # spring_data.lv = (2.03 - spring_data.p)*0.0115+0.0463 + spring_data.uv = spring_data.p*Ap_u + Vd_u + spring_data.lv = (2.03 - spring_data.p)*Ap_l + Vd_l + + ax = None + if plot: + fig, ax = plt.subplots(2, 1) + ax[0].plot(spring_data.uv, spring_data.up) + ax[1].plot(spring_data.lv, spring_data.lp) + # plt.show() + + return spring_data, ax + + +def set_piston_position(V0_u, # Volume setpoint from upper chamber polytropic PV curves + V0_l, # Volume setpoint from lower chamber polytropic PV curves + P0_u, # Pressure setpoint from upper PV + P0_l, # Pressure setpoint from lower PV + T0_u, # Tempurature setpoint for upper heat transfer + T0_l, # Tempurature setpoint for lower heat transfer + initial_piston_position=0.7, x_mean_pos=0.7): + ############################ + # Piston Position + ############################ + + # Would like to specify piston mean position and set all other + # air spring values from that. Start from curve-fitting PV + # for polytropic index and initial setpoints. Determine piston + # equilibrium position. Solve for mass shift between chambers + # that brings the equilibrium to the desired mean piston position. + # Recompute pressure and volume based on Ideal Gas Law. + + # Balance of forces on piston at equilibrium + # P_l + rho*g*V = P_u + m*g + # P_u - P_l = g*(rho*V - m) + # + # m_u*R*T m_l*R*T + # ------- - ------- = g*(rho*V - m) + # V_u V_l + # + # m_u m_l g + # ---------------- - ---------------- = ---*(rho*V - m) = C + # Ap_u*x_eq + Vd_u Ap_l*x_eq + Vd_l R*T + + C = lambda g, R_specific, T_ocean, rho, V_hc, m: (g / (R_specific*T_ocean))*(rho*V_hc - m) + x_eq = lambda Ap_u, Ap_l, m_u, m_l, Vd_u, Vd_l, C: -(0.005*( + math.sqrt( + ( + 203*Ap_u*Ap_l*C + 100*Ap_u*C*Vd_l + 100*Ap_u*m_l - \ + 100*Ap_l*C*Vd_u + 100*Ap_l*m_u + )**2 + \ + 400*Ap_u*Ap_l*C*( + 203*Ap_l*C*Vd_u - 203*Ap_l*m_u + 100*C*Vd_u*Vd_l + 100*m_l*Vd_u - 100*m_u*Vd_l + ) + ) - \ + 203*Ap_u*Ap_l*C - 100*Ap_u*C*Vd_l - 100*Ap_u*m_l + 100*Ap_l*C*Vd_u - 100*Ap_l*m_u + ) + ) / (Ap_u*Ap_l*C) + + # TODO(andermi) unknown why 108.56 fudge factor is necessary + c = 107.56*C(g, R_specific, T_ocean, rho, V_hc, m) # RHS of equation above + + ignore_piston_mean_pos = False + if not ignore_piston_mean_pos: + m_u = P0_u*V0_u / (R_specific*T0_u) # mass of N2 in upper, Ideal Gas Law + m_l = P0_l*V0_l / (R_specific*T0_l) # mass of N2 in lower, Ideal Gas Law + + # shift mass between upper and lower to bring equilibrium point to desired x_mean_pos + m_delta_guess = 0.0 + m_delta = spo.fsolve(lambda m_delta: x_mean_pos - x_eq(Ap_u, Ap_l, + m_u - m_delta, + m_l + m_delta, + Vd_u, Vd_l, c), m_delta_guess)[0] + # recompute setpoints based on new x_mean_pos and new masses + V0_u = Ap_u*x_mean_pos + Vd_u + V0_l = Ap_l*(2.03 - x_mean_pos) + Vd_l + m_u = m_u - m_delta + m_l = m_l + m_delta + P0_u = m_u*R_specific*T0_u / V0_u + P0_l = m_l*R_specific*T0_l / V0_l + + return V0_u, V0_l, P0_u, P0_l + + +def computeLawOfCoolingForce(V_, T, c, dt, is_upper): + piston_area = Ap_u if is_upper else Ap_l + V = V_ + + # Newton's Law of Cooling (non-dimensionalized): + # Tdot = r*(T_env - T(t)) -> T[n] = dt*r*(Tenv - T[n-1]) + T[n-1] (using forward difference) + dT = dt * r * (Tenv - T) + T += dT + + # TODO(andermi) find Qdot (rate of heat transfer) from h, A, dT (Qdot = h*A*dT) + # radius = 0.045; + # A = (2.0 * piston_area) * radius * x # TODO(andermi) compute x from V_ + # h = 11.3; # (W/(m^2*K)) -- Water<->Mild Steel<->Gas + # Q_rate = h * A * dT + + # Ideal Gas Law: P = (m*R)*T/V + P = c * T / V + + # F = P*A + F = P * piston_area + + return F, P, V, T + + +def computePolytropicForce(V_, P, V, n, c, v, is_upper): + piston_area = Ap_u if is_upper else Ap_l + V0, V = V, V_ + P0 = P + # polytropic relationship: P = P0*(V0/V)^n + P = P0 * pow(V0 / V, n) + # Ideal Gas Law: T = P*V/(m*R) + T = P * V / c + + ''' + # no heat loss if adiabatic + cp_R = c_p / R + Q_rate = 0.0 + is_adiabatic = False + if !is_adiabatic: + # Rodrigues, M. J. (June 5, 2014). Heat Transfer During the Piston-Cylinder Expansion of a Gas + # (Master's thesis, Oregon State University). + # Retrieved from https://ir.library.oregonstate.edu/downloads/ww72bf399 + # heat loss rate for polytropic ideal gas: + # dQ/dt = (1 - n/gamma)*(c_p/R)*P*A*dx/dt + # TODO(andermi) A != piston_area... it's the chamber surface area + r_ = 0.045 + A = (2.0 * piston_area) * r_ * x # TODO(andermi) compute x from V_ + Q_rate = (1.0 - n / ADIABATIC_INDEX) * cp_R * P * A * v + ''' + + # F = P*A + F = P * piston_area + + return F, P, V, T # , Q_rate + + +def model_thermal(PV, dt, + P0, V0, n1, n2, + T0, vel_dz_u, vel_dz_l, + is_upper=True, + ax1=None, + ax2=None): + n = n1 + c = P0 * V0 / T0 # m*R_specific + mass = c / R_specific + + V = V0 + P = P0 + # Ideal Gas Law: T = P*V/(m*R_specific) + T = P * V / c + F = 0 + + P_data = [] + V_data = [] + T_data = [] + F_data = [] + for idx, V_ in enumerate(PV[:, 0]): + if idx > 0: + v = V_ - PV[idx-1, 0] + v -= Vd_u if is_upper else Vd_l + v /= Ap_u if is_upper else Ap_l + else: + v = 1 + + if is_upper: + v *= -1.0 + + if v >= vel_dz_u: + n = n1 + F, P, V, T = computePolytropicForce(V_, P, V, n, c, v, is_upper) + elif v <= vel_dz_l: + n = n2 + F, P, V, T = computePolytropicForce(V_, P, V, n, c, v, is_upper) + else: + print('cooling') + F, P, V, T = computeLawOfCoolingForce(V_, T, c, dt, is_upper) + + if is_upper: + F *= -1.0 + P_data.append(P) + V_data.append(V) + T_data.append(T) + F_data.append(F) + + ax1[0 if is_upper else 1].plot(V_data, P_data) + ax2[0 if is_upper else 1].plot(dt*np.arange(len(P_data)), P_data) + ax2[0 if is_upper else 1].plot(dt*np.arange(PV[:, 1].shape[0]), PV[:, 1]) + + print('Diff in mean pressure (PSI):', (np.mean(PV[:, 1]) - np.mean(P_data))/6894.757) + print('RMSE pressure (PSI):', np.sqrt(np.mean((PV[:, 1] - np.array(P_data))**2.0))/6894.757) + + x = PV[:, 0] + x -= Vd_u if is_upper else Vd_l + x /= Ap_u if is_upper else Ap_l + + x_ = np.array(V_data) + x_ -= Vd_u if is_upper else Vd_l + x_ /= Ap_u if is_upper else Ap_l + + print('Diff in mean piston (should be zero):', np.mean(x) - np.mean(x_)) def main(): import argparse parser = argparse.ArgumentParser(description='Compute polytropic curves for Pneumatic Spring') - parser.add_argument('lower_pv_csv', - help='csv for PV curves in lower spring') - parser.add_argument('upper_pv_csv', - help='csv for PV curves in upper spring') + parser.add_argument('atsea_logs', nargs='+') + parser.add_argument('--plot', action='store_true') + parser.add_argument('--model_thermal', action='store_true') + parser.add_argument('--thermal_params_u', nargs=3, type=float) # T0 + # velocity_deadzone_upper + # velocity_deadzone_lower + parser.add_argument('--thermal_params_l', nargs=3, type=float) # T0 + # velocity_deadzone_upper + # velocity_deadzone_lower + parser.add_argument('--set_piston_pos', action='store_true') + # parser.add_argument('--piston_pos', nargs=2, type=float) # initial_piston_position + # # x_mean_pos args, unknown = parser.parse_known_args() - n1_l, n2_l, P0_l, V0_l = compute_polytropic(args.lower_pv_csv, target_P=1212000) - print('Lower Polytropic Parameters:' + - f'n1={n1_l:.04f} n2={n2_l:.04f} P0={P0_l:.00f} V0={V0_l:.04f}') - n1_u, n2_u, P0_u, V0_u = compute_polytropic(args.upper_pv_csv, target_P=410000) + + print(f'Loading: {args.atsea_logs}') + spring_data, plot_ax = load_atsea_logs(args.atsea_logs, + True if args.model_thermal else args.plot) + + n1_u, n2_u, P0_u, V0_u, C1_u, C2_u = compute_polytropic( + spring_data.loc[(1, ('uv', 'up'))].to_numpy(), + target_P=410000) print('Upper Polytropic Parameters:' + f'n1={n1_u:.04f} n2={n2_u:.04f} P0={P0_u:.00f} V0={V0_u:.04f}') + n1_l, n2_l, P0_l, V0_l, C1_l, C2_l = compute_polytropic( + spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), + target_P=1212000) + print('Lower Polytropic Parameters:' + + f'n1={n1_l:.04f} n2={n2_l:.04f} P0={P0_l:.00f} V0={V0_l:.04f}') + + if plot_ax is not None: + inc_u = np.where(np.diff(spring_data.uv) >= 0) + dec_u = np.where(np.diff(spring_data.uv) < 0) + inc_l = np.where(np.diff(spring_data.lv) >= 0) + dec_l = np.where(np.diff(spring_data.lv) < 0) + plot_ax[0].plot(spring_data.uv.to_numpy()[inc_u], + np.exp(C1_u)/spring_data.uv.to_numpy()[inc_u]**n1_u) + plot_ax[0].plot(spring_data.uv.to_numpy()[dec_u], + np.exp(C2_u)/spring_data.uv.to_numpy()[dec_u]**n2_u) + plot_ax[1].plot(spring_data.lv.to_numpy()[inc_l], + np.exp(C1_l)/spring_data.lv.to_numpy()[inc_l]**n1_l) + plot_ax[1].plot(spring_data.lv.to_numpy()[dec_l], + np.exp(C2_l)/spring_data.lv.to_numpy()[dec_l]**n2_l) + + if args.set_piston_pos: + init_pos = x_mean_pos = np.mean(spring_data.p) + V0_u, V0_l, P0_u, P0_l = set_piston_position(V0_u, V0_l, + P0_u, P0_l, + args.thermal_params_u[0], + args.thermal_params_l[0], + init_pos, x_mean_pos) + + if args.model_thermal: + fig2, ax2 = plt.subplots(2, 1) + dt = np.mean(np.diff(spring_data.t)) + model_thermal(spring_data.loc[(1, ('uv', 'up'))].to_numpy(), + dt, + P0_u, V0_u, n1_u, n2_u, + *args.thermal_params_u, + is_upper=True, ax1=plot_ax, ax2=ax2) + model_thermal(spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), + dt, + P0_l, V0_l, n1_l, n2_l, + *args.thermal_params_l, + is_upper=False, ax1=plot_ax, ax2=ax2) + + if plot_ax is not None: + plt.show() + if __name__ == '__main__': main() diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index eba8f864..f7bf4b3c 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -495,7 +495,7 @@ def generate_simulations(sim_params_yaml): # record all topics with rosbag2 rosbag2 = ExecuteProcess( - cmd=['ros2', 'bag', 'record', + cmd=['ros2', 'bag', 'record', '-s', 'mcap', '-o', os.path.join(batch_results_dir, run_results_dir, rosbag2_dir), '-a'], output='screen' ) diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 9544972f..0e94bd26 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -74,7 +74,7 @@ struct PolytropicPneumaticSpringConfig double piston_area{0.0127}; /// \brief Piston-End Dead Volume (m^3) - double dead_volume{0.0266}; + double dead_volume{0.0226}; /// \brief measure of valve opening cross-section and duration (meter-seconds) double valve_absement{49e-7}; @@ -337,22 +337,22 @@ void PolytropicPneumaticSpring::Configure( } else { config.is_adiabatic = false; } + this->dataPtr->n = config.n0; } - this->dataPtr->n = config.n0; this->dataPtr->P0 = SdfParamDouble(_sdf, "P0", this->dataPtr->P0); config.V0 = SdfParamDouble(_sdf, "V0", config.V0); this->dataPtr->V0 = config.V0; gzdbg << "V0: " << config.V0 << std::endl; - this->dataPtr->c = this->dataPtr->P0 * config.V0 / config.T0; + this->dataPtr->c = this->dataPtr->P0 * config.V0 / config.T0; // m*R_specific gzdbg << "c: " << this->dataPtr->c << std::endl; this->dataPtr->mass = this->dataPtr->c / config.R; gzdbg << "mass: " << this->dataPtr->mass << std::endl; this->dataPtr->V = this->dataPtr->V0; this->dataPtr->P = this->dataPtr->P0; - // Ideal Gas Law: T = P*V/(m*R) + // Ideal Gas Law: T = P*V/(m*R_specific) this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / this->dataPtr->c; config.model = gz::sim::Model(_entity); From 605c60d1278a8e3f63d346c93ea143f66e717eed Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 27 Jun 2023 15:56:57 -0700 Subject: [PATCH 02/26] can now specify spring params per batch Signed-off-by: Michael Anderson --- .../models/mbari_wec/model.sdf.em | 39 ++-- .../scripts/compute_polytropic.py | 174 +++++++++++++----- buoy_gazebo/scripts/example_sim_params.yaml | 11 ++ buoy_gazebo/scripts/mbari_wec_batch | 96 ++++++++-- 4 files changed, 249 insertions(+), 71 deletions(-) diff --git a/buoy_description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf.em index 9f018297..1e3567cf 100644 --- a/buoy_description/models/mbari_wec/model.sdf.em +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -150,15 +150,32 @@ Ap_u = 0.0127 # Area of piston in upper chamber Ap_l = 0.0115 # Area of piston in lower Vd_u = 0.0266 # Dead volume of upper Vd_l = 0.0523 # Dead volume of lower -# TODO(andermi) unknown why 108.56 fudge factor is necessary +# TODO(andermi) unknown why fudge factor is necessary (prev 108.56) c = 107.56*C(g, R_specific, T_ocean, rho, V_hc, m) # RHS of equation above -V0_u = 0.0397 # Volume setpoint from upper chamber polytropic PV curves -V0_l = 0.0661 # Volume setpoint from lower chamber polytropic PV curves -P0_u = 422156 # Pressure setpoint from upper PV -P0_l = 1212098 # Pressure setpoint from lower PV -T0_u = 283.15 # Temperature setpoint for upper heat transfer -T0_l = 283.15 # Temperature setpoint for lower heat transfer +# Check if upper_polytropic_params was passed in via empy +try: + upper_polytropic_params +except NameError: + upper_polytropic_params = [1.4309, 1.4367, 422156, 0.0397, 283.15] +(n1_u, # upper polytropic index for increasing volume +n2_u, # upper polytropic index for decreasing volume +P0_u, # Pressure (Pa) setpoint from upper PV +V0_u, # Volume (m^3) setpoint from upper chamber polytropic PV curves +T0_u # Temperature (K) setpoint for upper heat transfer +) = upper_polytropic_params + +# Check if lower_polytropic_params was passed in via empy +try: + lower_polytropic_params +except NameError: + lower_polytropic_params = [1.3771, 1.3755, 1212098, 0.0661, 283.15] +(n1_l, # lower polytropic index for increasing volume +n2_l, # lower polytropic index for decreasing volume +P0_l, # Pressure (Pa) setpoint from lower PV +V0_l, # Volume (m^3) setpoint from lower chamber polytropic PV curves +T0_l # Temperature (K) setpoint for lower heat transfer +) = lower_polytropic_params ignore_piston_mean_pos = True if not ignore_piston_mean_pos: @@ -217,8 +234,8 @@ if not ignore_piston_mean_pos: true -0.10 0.05 - 1.4309 - 1.4367 + @(n1_u) + @(n2_u) @(V0_u) @(print(f'{P0_u:.00f}', end='')) @@ -242,8 +259,8 @@ if not ignore_piston_mean_pos: true -0.10 0.05 - 1.3771 - 1.3755 + @(n1_l) + @(n2_l) @(V0_l) @(print(f'{P0_l:.00f}', end='')) diff --git a/buoy_description/scripts/compute_polytropic.py b/buoy_description/scripts/compute_polytropic.py index afb5b339..c944fba1 100755 --- a/buoy_description/scripts/compute_polytropic.py +++ b/buoy_description/scripts/compute_polytropic.py @@ -31,8 +31,12 @@ m_piston = 48 # mass of piston m = m_hc + m_piston # total mass -Ap_u = 0.0127 # Area of piston in upper chamber -Ap_l = 0.0115 # Area of piston in lower +Ap_u = 0.25*np.pi*5.0**2.0 +Ap_l = Ap_u - 0.25*np.pi*1.5**2.0 +Ap_u *= 0.0254**2.0 +Ap_l *= 0.0254**2.0 +# Ap_u = 0.0127 # Area of piston in upper chamber +# Ap_l = 0.0115 # Area of piston in lower Vd_u = 0.0266 # Dead volume of upper Vd_l = 0.0523 # Dead volume of lower @@ -85,7 +89,7 @@ def load_atsea_logs(logs, plot=False): # '2022.09.13T20.26.35', # '2022.09.15T19.29.42'] - df = [pd.read_csv(csv) for csv in sorted(logs)] + df = [pd.read_csv(csv) for csv in logs] df = [df_.set_index('Source ID') for df_ in df] sc_df = [df_.loc[1] for df_ in df] # SpringController: Source ID == 1 spring_data = [df_.loc[(1, [' Timestamp', @@ -105,14 +109,14 @@ def load_atsea_logs(logs, plot=False): spring_data.uv = spring_data.p*Ap_u + Vd_u spring_data.lv = (2.03 - spring_data.p)*Ap_l + Vd_l - ax = None + fig, ax = None, None if plot: fig, ax = plt.subplots(2, 1) - ax[0].plot(spring_data.uv, spring_data.up) - ax[1].plot(spring_data.lv, spring_data.lp) + ax[0].plot(spring_data.uv, spring_data.up, label='AtSea') + ax[1].plot(spring_data.lv, spring_data.lp, label='AtSea') # plt.show() - return spring_data, ax + return spring_data, fig, ax def set_piston_position(V0_u, # Volume setpoint from upper chamber polytropic PV curves @@ -269,8 +273,8 @@ def model_thermal(PV, dt, else: v = 1 - if is_upper: - v *= -1.0 + #if is_upper: + # v *= -1.0 if v >= vel_dz_u: n = n1 @@ -282,19 +286,25 @@ def model_thermal(PV, dt, print('cooling') F, P, V, T = computeLawOfCoolingForce(V_, T, c, dt, is_upper) - if is_upper: - F *= -1.0 + #if is_upper: + # F *= -1.0 P_data.append(P) V_data.append(V) T_data.append(T) F_data.append(F) - ax1[0 if is_upper else 1].plot(V_data, P_data) - ax2[0 if is_upper else 1].plot(dt*np.arange(len(P_data)), P_data) - ax2[0 if is_upper else 1].plot(dt*np.arange(PV[:, 1].shape[0]), PV[:, 1]) + ax1[0 if is_upper else 1].plot(V_data, P_data, label='Sim') + ax2[0 if is_upper else 1].plot(dt*np.arange(len(P_data)), P_data, label='Sim') + ax2[0 if is_upper else 1].plot(dt*np.arange(PV[:, 1].shape[0]), PV[:, 1], label='AtSea') - print('Diff in mean pressure (PSI):', (np.mean(PV[:, 1]) - np.mean(P_data))/6894.757) - print('RMSE pressure (PSI):', np.sqrt(np.mean((PV[:, 1] - np.array(P_data))**2.0))/6894.757) + diff_means = (np.mean(PV[:, 1]) - np.mean(P_data))/6894.757 + rmse = np.sqrt(np.mean((PV[:, 1] - np.array(P_data))**2.0))/6894.757 + ax2[0 if is_upper else 1].set_title(f"{'Upper' if is_upper else 'Lower'} Pressure\n" + f'Diff means (PSI) = {diff_means:.2f}\n' + f'RMSE (PSI) = {rmse:.2f}') + + print('Diff in mean pressure (PSI):', diff_means) + print('RMSE pressure (PSI):', rmse) x = PV[:, 0] x -= Vd_u if is_upper else Vd_l @@ -306,12 +316,21 @@ def model_thermal(PV, dt, print('Diff in mean piston (should be zero):', np.mean(x) - np.mean(x_)) + return np.array(F_data) + def main(): import argparse parser = argparse.ArgumentParser(description='Compute polytropic curves for Pneumatic Spring') parser.add_argument('atsea_logs', nargs='+') parser.add_argument('--plot', action='store_true') + parser.add_argument('--override', action='store_true') + parser.add_argument('--polytropic_u', nargs=6, type=float) # n1_u, n2_u, + # P0_u, V0_u, + # C1_u, C2_u + parser.add_argument('--polytropic_l', nargs=6, type=float) # n1_l, n2_l + # P0_l, V0_l, + # C1_l, C2_l parser.add_argument('--model_thermal', action='store_true') parser.add_argument('--thermal_params_u', nargs=3, type=float) # T0 # velocity_deadzone_upper @@ -325,34 +344,46 @@ def main(): args, unknown = parser.parse_known_args() print(f'Loading: {args.atsea_logs}') - spring_data, plot_ax = load_atsea_logs(args.atsea_logs, - True if args.model_thermal else args.plot) - - n1_u, n2_u, P0_u, V0_u, C1_u, C2_u = compute_polytropic( - spring_data.loc[(1, ('uv', 'up'))].to_numpy(), - target_P=410000) + spring_data, fig1, ax1 = load_atsea_logs(args.atsea_logs, + True if args.model_thermal else args.plot) + + if args.override: + n1_u, n2_u, P0_u, V0_u, C1_u, C2_u = args.polytropic_u + else: + n1_u, n2_u, P0_u, V0_u, C1_u, C2_u = compute_polytropic( + spring_data.loc[(1, ('uv', 'up'))].to_numpy(), + target_P=410000) print('Upper Polytropic Parameters:' + - f'n1={n1_u:.04f} n2={n2_u:.04f} P0={P0_u:.00f} V0={V0_u:.04f}') - - n1_l, n2_l, P0_l, V0_l, C1_l, C2_l = compute_polytropic( - spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), - target_P=1212000) + f'n1={n1_u:.04f} n2={n2_u:.04f} P0={P0_u:.00f}' + + f' V0={V0_u:.04f} C1={C1_u:.04f} C2={C2_u:.04f}') + + if args.override: + n1_l, n2_l, P0_l, V0_l, C1_l, C2_l = args.polytropic_l + else: + n1_l, n2_l, P0_l, V0_l, C1_l, C2_l = compute_polytropic( + spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), + target_P=1212000) print('Lower Polytropic Parameters:' + - f'n1={n1_l:.04f} n2={n2_l:.04f} P0={P0_l:.00f} V0={V0_l:.04f}') + f'n1={n1_l:.04f} n2={n2_l:.04f} P0={P0_l:.00f}' + + f' V0={V0_l:.04f} C1={C1_l:.04f} C2={C2_l:.04f}') - if plot_ax is not None: + if ax1 is not None: inc_u = np.where(np.diff(spring_data.uv) >= 0) dec_u = np.where(np.diff(spring_data.uv) < 0) inc_l = np.where(np.diff(spring_data.lv) >= 0) dec_l = np.where(np.diff(spring_data.lv) < 0) - plot_ax[0].plot(spring_data.uv.to_numpy()[inc_u], - np.exp(C1_u)/spring_data.uv.to_numpy()[inc_u]**n1_u) - plot_ax[0].plot(spring_data.uv.to_numpy()[dec_u], - np.exp(C2_u)/spring_data.uv.to_numpy()[dec_u]**n2_u) - plot_ax[1].plot(spring_data.lv.to_numpy()[inc_l], - np.exp(C1_l)/spring_data.lv.to_numpy()[inc_l]**n1_l) - plot_ax[1].plot(spring_data.lv.to_numpy()[dec_l], - np.exp(C2_l)/spring_data.lv.to_numpy()[dec_l]**n2_l) + ax1[0].plot(spring_data.uv.to_numpy()[inc_u], + np.exp(C1_u)/spring_data.uv.to_numpy()[inc_u]**n1_u, + label='Polytropic Fit (Increasing Volume)') + ax1[0].plot(spring_data.uv.to_numpy()[dec_u], + np.exp(C2_u)/spring_data.uv.to_numpy()[dec_u]**n2_u, + label='Polytropic Fit (Decreasing Volume)') + ax1[1].plot(spring_data.lv.to_numpy()[inc_l], + np.exp(C1_l)/spring_data.lv.to_numpy()[inc_l]**n1_l, + label='Polytropic Fit (Increasing Volume)') + ax1[1].plot(spring_data.lv.to_numpy()[dec_l], + np.exp(C2_l)/spring_data.lv.to_numpy()[dec_l]**n2_l, + label='Polytropic Fit (Decreasing Volume)') if args.set_piston_pos: init_pos = x_mean_pos = np.mean(spring_data.p) @@ -365,18 +396,63 @@ def main(): if args.model_thermal: fig2, ax2 = plt.subplots(2, 1) dt = np.mean(np.diff(spring_data.t)) - model_thermal(spring_data.loc[(1, ('uv', 'up'))].to_numpy(), - dt, - P0_u, V0_u, n1_u, n2_u, - *args.thermal_params_u, - is_upper=True, ax1=plot_ax, ax2=ax2) - model_thermal(spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), - dt, - P0_l, V0_l, n1_l, n2_l, - *args.thermal_params_l, - is_upper=False, ax1=plot_ax, ax2=ax2) - - if plot_ax is not None: + F_u = model_thermal(spring_data.loc[(1, ('uv', 'up'))].to_numpy(), + dt, + P0_u, V0_u, n1_u, n2_u, + *args.thermal_params_u, + is_upper=True, ax1=ax1, ax2=ax2) + F_l = model_thermal(spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), + dt, + P0_l, V0_l, n1_l, n2_l, + *args.thermal_params_l, + is_upper=False, ax1=ax1, ax2=ax2) + + fig3, ax3 = plt.subplots(1, 1) + ax3.plot(spring_data.p/0.0254, + (Ap_l*spring_data.lp - Ap_u*spring_data.up)*0.2248, + label='AtSea') + ax3.plot(spring_data.p/0.0254, + (F_l - F_u)*0.2248, '--', + label='Sim') + atsea_stiffness, _ = np.polyfit(spring_data.p/0.0254, + (Ap_l*spring_data.lp - Ap_u*spring_data.up)*0.2248, 1) + sim_stiffness, _ = np.polyfit(spring_data.p/0.0254, + (F_l - F_u)*0.2248, 1) + print(f'Stiffness: atsea={atsea_stiffness} sim={sim_stiffness}') + ax3.set_title(f'Stiffness: AtSea={atsea_stiffness:.2f} Sim={sim_stiffness:.2f}') + + if ax1 is not None: + import os + for ax1_ in ax1: + ax1_.legend() + ax1_.set_xlabel('Volume (m^3)') + ax1_.set_ylabel('Pressure (Pascals)') + ax1[0].set_title(f'Upper Pressure vs Volume\n' + f'n1={n1_u:.4f} n2={n2_u:.4f} P0={int(round(P0_u))} V0={V0_u:.4f}\n' + f'T0={args.thermal_params_u[0]} Tenv={Tenv}' + f' vel_dz_u={args.thermal_params_u[1]}' + f' vel_dz_l={args.thermal_params_u[2]}') + ax1[1].set_title(f'Lower Pressure vs Volume\n' + f'n1={n1_l:.4f} n2={n2_l:.4f} P0={int(round(P0_l))} V0={V0_l:.4f}\n' + f'T0={args.thermal_params_l[0]} Tenv={Tenv}' + f' vel_dz_u={args.thermal_params_l[1]}' + f' vel_dz_l={args.thermal_params_l[2]}') + fig1.tight_layout() + #fig1.savefig(os.path.basename(args.atsea_logs[0])+'.PVPolytropicFit.png', + # dpi=fig1.dpi, bbox_inches='tight') + for ax2_ in ax2: + ax2_.legend() + ax2_.set_xlabel('Time from start (sec)') + ax2_.set_ylabel('Pressure (Pascals)') + fig2.tight_layout() + #fig2.savefig(os.path.basename(args.atsea_logs[0])+'.PressureDiff.png', + # dpi=fig2.dpi, bbox_inches='tight') + ax3.set_ylabel('Piston Position (in)') + ax3.set_xlabel('Force (pound-force)') + ax3.legend() + fig3.tight_layout() + #fig3.savefig(os.path.basename(args.atsea_logs[0])+'.Stiffness.png', + # dpi=fig3.dpi, bbox_inches='tight') plt.show() diff --git a/buoy_gazebo/scripts/example_sim_params.yaml b/buoy_gazebo/scripts/example_sim_params.yaml index 21ad32f5..2b7d92bd 100644 --- a/buoy_gazebo/scripts/example_sim_params.yaml +++ b/buoy_gazebo/scripts/example_sim_params.yaml @@ -5,6 +5,17 @@ seed: 42 # Set `seed: 0` to have different seed per run duration: 5 physics_rtf: 11 enable_gui: False +# Air spring consists of upper/lower N2-filled chambers +upper_spring_params: [1.4309, # upper polytropic index for increasing volume + 1.4367, # upper polytropic index for decreasing volume + 422156, # Pressure (Pa) setpoint from upper PV + 0.0397, # Volume (m^3) setpoint from upper chamber polytropic PV curves + 283.15] # Temperature (K) setpoint for upper heat transfer +lower_spring_params: [1.3771, # lower polytropic index for increasing volume + 1.3755, # lower polytropic index for decreasing volume + 1212098, # Pressure (Pa) setpoint from lower PV + 0.0661, # Volume (m^3) setpoint from lower chamber polytropic PV curves + 283.15] # Temperature (K) setpoint for lower heat transfer # # Run-Specific Parameters (Test Matrix) # diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index f7bf4b3c..a5ffe62a 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -83,6 +83,19 @@ class Custom(object): return f'{self.name};f:defaults;Szz:defaults' +class SpringParams(object): + def __init__(self, name, params): + self.name = name + self.params = params + + def __str__(self): + if self.params is not None: + return f'{self.name};' + \ + f'{";".join([f"{p}:{v}" for p, v in zip(["n1", "n2", "P0", "V0", "T0"], self.params)])}' + else: + return f'{self.name};defaults' + + def generate_simulations(sim_params_yaml): rclpy.init() @@ -91,18 +104,14 @@ def generate_simulations(sim_params_yaml): with open(sim_params_yaml, 'r') as fd: sim_params = yaml.load(fd, Loader=yaml.CLoader) + # don't print if param is None print_optional = lambda param: str(param) if param is not None else '' # noqa: E731 # Grab params from yaml - # physics time step (seconds) - if 'physics_step' in sim_params: - physics_step = np.array(sim_params['physics_step'], dtype=float) - physics_step = np.atleast_1d(physics_step) - else: - physics_step = [None] - node.get_logger().warn( - 'sim_params_yaml: optional physics_step parameter not specified -- defaulting' - ) + + ######################################## + # Batch-Specific (Non-Matrix) Parameters + ######################################## # physics real-time factor (RTF) try: @@ -156,6 +165,58 @@ def generate_simulations(sim_params_yaml): ' must be a single value in seconds') sys.exit(-1) + # upper polytropic params: [n1, # polytropic index for increasing volume + # n2, # polytropic index for decreasing volume + # P0, # Pressure (Pa) setpoint from polytropic PV curves + # V0, # Volume (m^3) setpoint from polytropic PV curves + # T0] # Temperature (K) setpoint for heat transfer + if 'upper_spring_params' in sim_params: + usp = np.array(sim_params['upper_spring_params'], dtype=float) + usp = np.atleast_1d(usp) + if usp.shape[0] != 5: + node.get_logger().error( + 'sim_params_yaml: upper_spring_params must have 5 values -- n1, n2, P0, V0, T0' + ) + sys.exit(-1) + else: + node.get_logger().warn( + 'sim_params_yaml: upper_spring_params parameter not specified -- defaulting' + ) + usp = None + + # lower polytropic params: [n1, # polytropic index for increasing volume + # n2, # polytropic index for decreasing volume + # P0, # Pressure (Pa) setpoint from polytropic PV curves + # V0, # Volume (m^3) setpoint from polytropic PV curves + # T0] # Temperature (K) setpoint for heat transfer + if 'lower_spring_params' in sim_params: + lsp = np.array(sim_params['lower_spring_params'], dtype=float) + lsp = np.atleast_1d(lsp) + if usp.shape[0] != 5: + node.get_logger().error( + 'sim_params_yaml: lower_spring_params must have 5 values -- n1, n2, P0, V0, T0' + ) + sys.exit(-1) + else: + node.get_logger().warn( + 'sim_params_yaml: lower_spring_params parameter not specified -- defaulting' + ) + lsp = None + + #################################### + # Run-Specific (Matrix) Parameters + #################################### + + # physics time step (seconds) + if 'physics_step' in sim_params: + physics_step = np.array(sim_params['physics_step'], dtype=float) + physics_step = np.atleast_1d(physics_step) + else: + physics_step = [None] + node.get_logger().warn( + 'sim_params_yaml: optional physics_step parameter not specified -- defaulting' + ) + # door state ('open', 'closed') if 'door_state' in sim_params: door_state = sim_params['door_state'] @@ -202,7 +263,7 @@ def generate_simulations(sim_params_yaml): elif 'battery_emf' in sim_params: battery_state = np.array(sim_params['battery_emf'], dtype=float) battery_state = np.atleast_1d(battery_state) - if np.all((270.0 <= battery_state) & (battery_state <= 300.0)): + if np.all((270.0 <= battery_state) & (battery_state <= 320.0)): batt_state_type = 'EMF' else: node.get_logger().error( @@ -360,11 +421,14 @@ def generate_simulations(sim_params_yaml): node.get_logger().info(f'Generated {len(batch_params)} simulation runs') node.get_logger().debug('PhysicsStep, PhysicsRTF, Seed, Duration, DoorState, ScaleFactor' + ', BatteryState, MeanPistonPosition' + + ', UpperSpringParams, LowerSpringParams' + ', IncWaveSpectrumType;IncWaveSpectrumParams') [node.get_logger().debug(f'{print_optional(ps)}, {print_optional(physics_rtf)}' + f', {print_optional(seed_)}, {duration}, {ds}, {sf}' + f', {print_optional(bs)}' + f', {print_optional(mpp)}' + + f', {print_optional(SpringParams("upper", usp))}' + + f', {print_optional(SpringParams("lower", lsp))}' + f', {print_optional(iw)}') for ps, ds, sf, bs, mpp, iw in batch_params] @@ -375,7 +439,9 @@ def generate_simulations(sim_params_yaml): fd.write('RunIndex, SimReturnCode, StartTime, rosbag2Filename, pblogFilename' + ', PhysicsStep, PhysicsRTF' + ', Seed, Duration, DoorState, ScaleFactor, BatteryState' + - ', MeanPistonPosition, IncWaveSpectrumType;IncWaveSpectrumParams\n') + ', MeanPistonPosition' + + ', UpperSpringParams, LowerSpringParams' + + ', IncWaveSpectrumType;IncWaveSpectrumParams\n') # Find packages pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo') @@ -404,6 +470,10 @@ def generate_simulations(sim_params_yaml): f" door state='{ds}', scale factor={sf}" + f", battery state={bs if bs is not None else 'None'}" + f", mean piston position={mpp if mpp is not None else 'None'}" + + ', UpperSpringParams=' + + f"{str(SpringParams('upper', usp))}" + + ', LowerSpringParams=' + + f"{str(SpringParams('lower', lsp))}" + ', IncidentWaveSpectrumType=' + f"{str(iw) if iw is not None else 'None'}\n") @@ -433,6 +503,10 @@ def generate_simulations(sim_params_yaml): mbari_wec_model_params.extend(['-D', f'battery_emf = {bs}']) if mpp is not None: mbari_wec_model_params.extend(['-D', f'x_mean_pos = {mpp}']) + if usp is not None: + mbari_wec_model_params.extend(['-D', f'upper_polytropic_params = {usp.tolist()}']) + if lsp is not None: + mbari_wec_model_params.extend(['-D', f'lower_polytropic_params = {lsp.tolist()}']) if iw is not None: mbari_wec_model_params.extend(['-D', f'inc_wave_spectrum_type = "{iw.name}"']) if 'MonoChromatic' in iw.name: From 36c4eb9f29564d1a3f28739e1d52dc922bdfbdae Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 5 Jul 2023 22:44:19 -0700 Subject: [PATCH 03/26] tune spring to match atsea Signed-off-by: Michael Anderson --- .../models/mbari_wec/model.sdf.em | 14 ++-- .../scripts/compute_polytropic.py | 69 +++++++++---------- buoy_gazebo/scripts/mbari_wec_batch | 2 + .../PolytropicPneumaticSpring.cpp | 2 +- 4 files changed, 44 insertions(+), 43 deletions(-) diff --git a/buoy_description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf.em index 1e3567cf..3faa2b15 100644 --- a/buoy_description/models/mbari_wec/model.sdf.em +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -148,8 +148,8 @@ m = m_hc + m_piston # total mass Ap_u = 0.0127 # Area of piston in upper chamber Ap_l = 0.0115 # Area of piston in lower -Vd_u = 0.0266 # Dead volume of upper -Vd_l = 0.0523 # Dead volume of lower +Vd_u = 0.0226 # 0.0172 # 0.0226 # Dead volume of upper +Vd_l = 0.0463 # 0.0546 # 0.0463 # Dead volume of lower # TODO(andermi) unknown why fudge factor is necessary (prev 108.56) c = 107.56*C(g, R_specific, T_ocean, rho, V_hc, m) # RHS of equation above @@ -177,7 +177,7 @@ V0_l, # Volume (m^3) setpoint from lower chamber polytropic PV curves T0_l # Temperature (K) setpoint for lower heat transfer ) = lower_polytropic_params -ignore_piston_mean_pos = True +ignore_piston_mean_pos = False if not ignore_piston_mean_pos: m_u = P0_u*V0_u / (R_specific*T0_u) # mass of N2 in upper, Ideal Gas Law m_l = P0_l*V0_l / (R_specific*T0_l) # mass of N2 in lower, Ideal Gas Law @@ -232,8 +232,8 @@ if not ignore_piston_mean_pos: 0.2968 1.04 true - -0.10 - 0.05 + -0.005 + 0.005 @(n1_u) @(n2_u) @(V0_u) @@ -257,8 +257,8 @@ if not ignore_piston_mean_pos: 0.2968 1.04 true - -0.10 - 0.05 + -0.005 + 0.005 @(n1_l) @(n2_l) @(V0_l) diff --git a/buoy_description/scripts/compute_polytropic.py b/buoy_description/scripts/compute_polytropic.py index c944fba1..5f31427a 100755 --- a/buoy_description/scripts/compute_polytropic.py +++ b/buoy_description/scripts/compute_polytropic.py @@ -37,8 +37,8 @@ Ap_l *= 0.0254**2.0 # Ap_u = 0.0127 # Area of piston in upper chamber # Ap_l = 0.0115 # Area of piston in lower -Vd_u = 0.0266 # Dead volume of upper -Vd_l = 0.0523 # Dead volume of lower +Vd_u = 0.0226 # est from cad 0.0172 # 0.015 # 0.0226 # 0.0266 # Dead volume of upper +Vd_l = 0.0463 # est from cad 0.0546 # 0.025 # 0.0463 # 0.0523 # Dead volume of lower r = 0.5 # coef of heat transfer (Newton's law of cooling) c_p = 1.04 # for N2 @@ -245,7 +245,7 @@ def computePolytropicForce(V_, P, V, n, c, v, is_upper): return F, P, V, T # , Q_rate -def model_thermal(PV, dt, +def model_thermal(PV, p, dt, P0, V0, n1, n2, T0, vel_dz_u, vel_dz_l, is_upper=True, @@ -265,25 +265,22 @@ def model_thermal(PV, dt, V_data = [] T_data = [] F_data = [] - for idx, V_ in enumerate(PV[:, 0]): - if idx > 0: - v = V_ - PV[idx-1, 0] - v -= Vd_u if is_upper else Vd_l - v /= Ap_u if is_upper else Ap_l - else: - v = 1 + v = np.diff(p, prepend=np.mean(p)) + for v_, V_ in zip(v, PV[:, 0]): - #if is_upper: - # v *= -1.0 + if not is_upper: + v *= -1.0 - if v >= vel_dz_u: + if v_ >= vel_dz_u: + #print(f'vel above: {v_}') n = n1 - F, P, V, T = computePolytropicForce(V_, P, V, n, c, v, is_upper) - elif v <= vel_dz_l: + F, P, V, T = computePolytropicForce(V_, P, V, n, c, v_, is_upper) + elif v_ <= vel_dz_l: + #print(f'vel below: {v_}') n = n2 - F, P, V, T = computePolytropicForce(V_, P, V, n, c, v, is_upper) + F, P, V, T = computePolytropicForce(V_, P, V, n, c, v_, is_upper) else: - print('cooling') + #print(f'cooling: {v_}') F, P, V, T = computeLawOfCoolingForce(V_, T, c, dt, is_upper) #if is_upper: @@ -294,15 +291,15 @@ def model_thermal(PV, dt, F_data.append(F) ax1[0 if is_upper else 1].plot(V_data, P_data, label='Sim') - ax2[0 if is_upper else 1].plot(dt*np.arange(len(P_data)), P_data, label='Sim') ax2[0 if is_upper else 1].plot(dt*np.arange(PV[:, 1].shape[0]), PV[:, 1], label='AtSea') + ax2[0 if is_upper else 1].plot(dt*np.arange(len(P_data[600:])), P_data[600:], label='Sim') - diff_means = (np.mean(PV[:, 1]) - np.mean(P_data))/6894.757 - rmse = np.sqrt(np.mean((PV[:, 1] - np.array(P_data))**2.0))/6894.757 + diff_means = (np.mean(PV[:, 1]) - np.mean(P_data[600:]))/6894.757 + rmse = np.sqrt(np.mean((PV[600:, 1] - np.array(P_data[600:]))**2.0))/6894.757 ax2[0 if is_upper else 1].set_title(f"{'Upper' if is_upper else 'Lower'} Pressure\n" f'Diff means (PSI) = {diff_means:.2f}\n' f'RMSE (PSI) = {rmse:.2f}') - + print('Diff in mean pressure (PSI):', diff_means) print('RMSE pressure (PSI):', rmse) @@ -353,9 +350,10 @@ def main(): n1_u, n2_u, P0_u, V0_u, C1_u, C2_u = compute_polytropic( spring_data.loc[(1, ('uv', 'up'))].to_numpy(), target_P=410000) - print('Upper Polytropic Parameters:' + - f'n1={n1_u:.04f} n2={n2_u:.04f} P0={P0_u:.00f}' + - f' V0={V0_u:.04f} C1={C1_u:.04f} C2={C2_u:.04f}') + print('Upper Polytropic Parameters [n1, n2, P0, V0, T0]: ' + + f'{n1_u:.04f}, {n2_u:.04f}, {P0_u:.00f},' + + f' {V0_u:.04f}, {args.thermal_params_u[0] if args.model_thermal else Tenv},' + f' C1={C1_u:.04f} C2={C2_u:.04f}') if args.override: n1_l, n2_l, P0_l, V0_l, C1_l, C2_l = args.polytropic_l @@ -363,9 +361,10 @@ def main(): n1_l, n2_l, P0_l, V0_l, C1_l, C2_l = compute_polytropic( spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), target_P=1212000) - print('Lower Polytropic Parameters:' + - f'n1={n1_l:.04f} n2={n2_l:.04f} P0={P0_l:.00f}' + - f' V0={V0_l:.04f} C1={C1_l:.04f} C2={C2_l:.04f}') + print('Lower Polytropic Parameters [n1, n2, P0, V0, T0]: ' + + f'{n1_l:.04f}, {n2_l:.04f}, {P0_l:.00f},' + + f' {V0_l:.04f}, {args.thermal_params_l[0] if args.model_thermal else Tenv},' + f' C1={C1_l:.04f} C2={C2_l:.04f}') if ax1 is not None: inc_u = np.where(np.diff(spring_data.uv) >= 0) @@ -397,12 +396,12 @@ def main(): fig2, ax2 = plt.subplots(2, 1) dt = np.mean(np.diff(spring_data.t)) F_u = model_thermal(spring_data.loc[(1, ('uv', 'up'))].to_numpy(), - dt, + spring_data.p, dt, P0_u, V0_u, n1_u, n2_u, *args.thermal_params_u, is_upper=True, ax1=ax1, ax2=ax2) F_l = model_thermal(spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), - dt, + spring_data.p, dt, P0_l, V0_l, n1_l, n2_l, *args.thermal_params_l, is_upper=False, ax1=ax1, ax2=ax2) @@ -411,13 +410,13 @@ def main(): ax3.plot(spring_data.p/0.0254, (Ap_l*spring_data.lp - Ap_u*spring_data.up)*0.2248, label='AtSea') - ax3.plot(spring_data.p/0.0254, - (F_l - F_u)*0.2248, '--', + ax3.plot(spring_data.p[600:]/0.0254, + (F_l[600:] - F_u[600:])*0.2248, '--', label='Sim') atsea_stiffness, _ = np.polyfit(spring_data.p/0.0254, (Ap_l*spring_data.lp - Ap_u*spring_data.up)*0.2248, 1) - sim_stiffness, _ = np.polyfit(spring_data.p/0.0254, - (F_l - F_u)*0.2248, 1) + sim_stiffness, _ = np.polyfit(spring_data.p[600:]/0.0254, + (F_l[600:] - F_u[600:])*0.2248, 1) print(f'Stiffness: atsea={atsea_stiffness} sim={sim_stiffness}') ax3.set_title(f'Stiffness: AtSea={atsea_stiffness:.2f} Sim={sim_stiffness:.2f}') @@ -447,8 +446,8 @@ def main(): fig2.tight_layout() #fig2.savefig(os.path.basename(args.atsea_logs[0])+'.PressureDiff.png', # dpi=fig2.dpi, bbox_inches='tight') - ax3.set_ylabel('Piston Position (in)') - ax3.set_xlabel('Force (pound-force)') + ax3.set_xlabel('Piston Position (in)') + ax3.set_ylabel('Force (pound-force)') ax3.legend() fig3.tight_layout() #fig3.savefig(os.path.basename(args.atsea_logs[0])+'.Stiffness.png', diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index a5ffe62a..9c15aa29 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -643,6 +643,8 @@ def generate_simulations(sim_params_yaml): f', {print_optional(seed)}, {duration}, {ds}, {sf}' + f', {print_optional(bs)}' + f', {print_optional(mpp)}' + + f', {print_optional(SpringParams("upper", usp))}' + + f', {print_optional(SpringParams("lower", lsp))}' + f', {print_optional(iw)}\n') # Return all files to defaults diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 0e94bd26..d334549e 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -74,7 +74,7 @@ struct PolytropicPneumaticSpringConfig double piston_area{0.0127}; /// \brief Piston-End Dead Volume (m^3) - double dead_volume{0.0226}; + double dead_volume{0.0266}; /// \brief measure of valve opening cross-section and duration (meter-seconds) double valve_absement{49e-7}; From b831d13a968b3b822494b1b7a1a9488d85a45e31 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 12 Jul 2023 11:48:09 -0700 Subject: [PATCH 04/26] ros2 service for incwave height; issues so replace LinearIncidentWave with non-pointer in ECM Signed-off-by: Michael Anderson --- .../models/mbari_wec_ros/model.sdf | 5 + buoy_gazebo/src/IncidentWaves/CMakeLists.txt | 8 +- .../src/IncidentWaves/IncWaveState.hpp | 5 +- .../src/IncidentWaves/IncidentWaves.cpp | 17 +- .../src/WaveBodyInteractions/CMakeLists.txt | 7 +- .../WaveBodyInteractions.cpp | 8 +- buoy_gazebo/src/latent_data/CMakeLists.txt | 6 + .../latent_data/IncWaveHeight/CMakeLists.txt | 15 + .../IncWaveHeight/IncWaveHeight.cpp | 264 ++++++++++++++++++ .../IncWaveHeight/IncWaveHeight.hpp | 65 +++++ .../wave_body_interactions_heaveonly.cpp | 4 +- .../wave_body_interactions_pitchonly.cpp | 4 +- 12 files changed, 382 insertions(+), 26 deletions(-) create mode 100644 buoy_gazebo/src/latent_data/CMakeLists.txt create mode 100644 buoy_gazebo/src/latent_data/IncWaveHeight/CMakeLists.txt create mode 100644 buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.cpp create mode 100644 buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.hpp diff --git a/buoy_description/models/mbari_wec_ros/model.sdf b/buoy_description/models/mbari_wec_ros/model.sdf index 2b832153..205ee11b 100644 --- a/buoy_description/models/mbari_wec_ros/model.sdf +++ b/buoy_description/models/mbari_wec_ros/model.sdf @@ -53,6 +53,11 @@ 10 + + / + inc_wave_service + + diff --git a/buoy_gazebo/src/IncidentWaves/CMakeLists.txt b/buoy_gazebo/src/IncidentWaves/CMakeLists.txt index fb64e6ff..e1aa31d6 100644 --- a/buoy_gazebo/src/IncidentWaves/CMakeLists.txt +++ b/buoy_gazebo/src/IncidentWaves/CMakeLists.txt @@ -6,10 +6,10 @@ find_package(Boost 1.71.0 COMPONENTS system iostreams filesystem) gz_add_plugin(IncidentWaves SOURCES - IncidentWaves.cpp + IncidentWaves.cpp + PUBLIC_LINK_LIBS + FreeSurfaceHydrodynamics INCLUDE_DIRS .. + ../.. ) - -target_link_libraries(IncidentWaves PUBLIC FreeSurfaceHydrodynamics) - diff --git a/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp b/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp index 018b551a..93fe16c8 100644 --- a/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp +++ b/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp @@ -20,14 +20,15 @@ #include #include #include -#include "LinearIncidentWave.hpp" + +#include namespace buoy_gazebo { /// \brief Structure that holds shared ptr to incident wave object(s), and other data struct IncWaveState { - std::shared_ptr Inc; + LinearIncidentWave Inc; double x; double y; }; diff --git a/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp b/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp index 49077e0a..08f1c69f 100644 --- a/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp +++ b/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp @@ -32,7 +32,7 @@ #include #include -#include "LinearIncidentWave.hpp" +#include #include "IncWaveState.hpp" @@ -47,7 +47,7 @@ class IncidentWavesPrivate /// \brief Model interface gz::sim::Model model{gz::sim::kNullEntity}; - std::shared_ptr Inc = std::make_shared(); + LinearIncidentWave Inc{}; gz::sim::Entity IncWaveEntity; buoy_gazebo::IncWaveState inc_wave_state; @@ -82,7 +82,7 @@ void IncidentWaves::Configure( } double seed = SdfParamDouble(_sdf, "IncWaveSeed", 0.0); - this->dataPtr->Inc->SetSeed(seed); + this->dataPtr->Inc.SetSeed(seed); auto linkName = _sdf->Get("LinkName"); @@ -96,7 +96,7 @@ void IncidentWaves::Configure( double A = SdfParamDouble(_sdf, "A", 0.0); double T = SdfParamDouble(_sdf, "T", 14.0); double phase = SdfParamDouble(_sdf, "Phase", 0.0); - this->dataPtr->Inc->SetToMonoChromatic(A, T, phase, beta); + this->dataPtr->Inc.SetToMonoChromatic(A, T, phase, beta); } if (!SpectrumType.compare("Bretschneider")) { @@ -104,7 +104,7 @@ void IncidentWaves::Configure( double Hs = SdfParamDouble(_sdf, "Hs", 0.0); double Tp = SdfParamDouble(_sdf, "Tp", 14.0); gzdbg << "Hs = " << Hs << " Tp = " << Tp << std::endl; - this->dataPtr->Inc->SetToBretschneiderSpectrum(Hs, Tp, beta); + this->dataPtr->Inc.SetToBretschneiderSpectrum(Hs, Tp, beta); } if (!SpectrumType.compare("Custom")) { @@ -124,7 +124,7 @@ void IncidentWaves::Configure( } if (freq.size() > 2) { // \TODO(anyone): Add more checks on validity of spectrum - this->dataPtr->Inc->SetToCustomSpectrum(freq, S, beta); + this->dataPtr->Inc.SetToCustomSpectrum(freq, S, beta); } else { gzwarn << "Ill-formed custom wave-spectrum specification, no waves added" << std::endl; } @@ -153,8 +153,7 @@ void IncidentWaves::PreUpdate( } auto SimTime = std::chrono::duration(_info.simTime).count(); - -// buoy_gazebo::IncWaveState inc_wave_state; + // buoy_gazebo::IncWaveState inc_wave_state; if (_ecm.EntityHasComponentType( this->dataPtr->IncWaveEntity, buoy_gazebo::components::IncWaveState().TypeId())) @@ -165,7 +164,7 @@ void IncidentWaves::PreUpdate( this->dataPtr->inc_wave_state = buoy_gazebo::IncWaveState(inc_wave_state_comp->Data()); } double deta_dx{0.0}, deta_dy{0.0}; - double eta = this->dataPtr->Inc->eta( + double eta = this->dataPtr->Inc.eta( this->dataPtr->inc_wave_state.x, this->dataPtr->inc_wave_state.y, SimTime, &deta_dx, &deta_dy); diff --git a/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt b/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt index 09507a1d..4c03dfc1 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt +++ b/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt @@ -6,16 +6,15 @@ set(BOOST_USE_MULTITHREADED ON) set(BOOST_USE_STATIC_RUNTIME OFF) find_package(Boost 1.71.0 COMPONENTS system iostreams filesystem) - - gz_add_plugin(WaveBodyInteractions SOURCES WaveBodyInteractions.cpp + PUBLIC_LINK_LIBS + FreeSurfaceHydrodynamics INCLUDE_DIRS .. ../.. EXTRA_ROS_PKGS simple_interp + ament_index_cpp ) - -target_link_libraries(WaveBodyInteractions PUBLIC FreeSurfaceHydrodynamics ament_index_cpp::ament_index_cpp) diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index f097fa9c..9ffa22b1 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -45,8 +45,8 @@ #include "IncidentWaves/IncWaveState.hpp" -#include "FS_Hydrodynamics.hpp" -#include "LinearIncidentWave.hpp" +#include +#include #include @@ -209,7 +209,9 @@ void WaveBodyInteractions::PreUpdate( _ecm.Component( this->dataPtr->IncWaveEntity); inc_wave_state = buoy_gazebo::IncWaveState(inc_wave_state_comp->Data()); - this->dataPtr->FloatingBody.AssignIncidentWave(inc_wave_state.Inc); + const std::shared_ptr Inc(dynamic_cast(new LinearIncidentWave( + inc_wave_state.Inc))); + this->dataPtr->FloatingBody.AssignIncidentWave(Inc); } gz::sim::Link baseLink(this->dataPtr->linkEntity); diff --git a/buoy_gazebo/src/latent_data/CMakeLists.txt b/buoy_gazebo/src/latent_data/CMakeLists.txt new file mode 100644 index 00000000..ce907053 --- /dev/null +++ b/buoy_gazebo/src/latent_data/CMakeLists.txt @@ -0,0 +1,6 @@ +file(GLOB dirs LIST_DIRECTORIES true *) +foreach(dir ${dirs}) + if(IS_DIRECTORY ${dir}) + add_subdirectory(${dir}) + endif() +endforeach() diff --git a/buoy_gazebo/src/latent_data/IncWaveHeight/CMakeLists.txt b/buoy_gazebo/src/latent_data/IncWaveHeight/CMakeLists.txt new file mode 100644 index 00000000..32741683 --- /dev/null +++ b/buoy_gazebo/src/latent_data/IncWaveHeight/CMakeLists.txt @@ -0,0 +1,15 @@ +find_package(FreeSurfaceHydrodynamics REQUIRED) +set(BOOST_USE_STATIC_LIBS OFF) +set(BOOST_USE_MULTITHREADED ON) +set(BOOST_USE_STATIC_RUNTIME OFF) +find_package(Boost 1.71.0 COMPONENTS system iostreams filesystem) + +gz_add_plugin(IncWaveHeight + SOURCES + IncWaveHeight.cpp + PUBLIC_LINK_LIBS + FreeSurfaceHydrodynamics + INCLUDE_DIRS + ../.. + ROS +) diff --git a/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.cpp new file mode 100644 index 00000000..a42de009 --- /dev/null +++ b/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.cpp @@ -0,0 +1,264 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "IncWaveHeight.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + + +using namespace std::chrono_literals; + +namespace buoy_gazebo +{ +struct IncWaveHeightROS2 +{ + rclcpp::Node::SharedPtr node_{nullptr}; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_{nullptr}; + bool use_sim_time_{true}; +}; + +struct IncWaveHeightServices +{ + rclcpp::Service::SharedPtr inc_wave_height_service_{nullptr}; + std::function, + std::shared_ptr)> inc_wave_height_handler_; +}; + +struct IncWaveHeightPrivate +{ + gz::sim::Entity IncWaveEntity{gz::sim::kNullEntity}; + buoy_gazebo::IncWaveState inc_wave_state; + + std::chrono::steady_clock::duration current_time_; + + std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; + std::atomic inc_wave_valid_{false}; + + std::thread thread_executor_spin_; + std::atomic stop_{false}, paused_{true}; + + std::unique_ptr ros_; + std::unique_ptr services_; + + void ros2_setup(const std::string & node_name, const std::string & ns) + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + ros_->node_ = rclcpp::Node::make_shared(node_name, ns); + + ros_->node_->set_parameter( + rclcpp::Parameter( + "use_sim_time", + ros_->use_sim_time_)); + + ros_->executor_ = std::make_shared(); + ros_->executor_->add_node(ros_->node_); + stop_ = false; + + auto spin = [this]() + { + rclcpp::Rate rate(50.0); + while (rclcpp::ok() && !stop_) { + ros_->executor_->spin_once(); + rate.sleep(); + } + }; + thread_executor_spin_ = std::thread(spin); + } + + void setup_services() + { + // IncWaveHeight + services_->inc_wave_height_handler_ = + [this](const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Incident Wave Height] IncWaveHeight Request received with [" + << request->points.size() << "] point(s)"); + + // high prio data access + std::unique_lock next(next_access_mutex_); + std::unique_lock data(data_mutex_); + next.unlock(); + + response->valid = inc_wave_valid_; + if (!inc_wave_valid_) { + return; + } + + double SimTime = std::chrono::duration(current_time_).count(); + + response->poses.resize(request->points.size()); + for (std::size_t idx = 0U; idx < request->points.size(); ++idx) { + double x = request->points[idx].x; + double y = request->points[idx].y; + if (request->use_buoy_origin) { + x += inc_wave_state.x; + y += inc_wave_state.y; + } + + double deta_dx{0.0}, deta_dy{0.0}; + double eta = inc_wave_state.Inc.eta( + x, y, SimTime, &deta_dx, &deta_dy); + + response->poses[idx].position = request->points[idx]; + response->poses[idx].position.z = eta; + + double roll = atan(deta_dx); + double pitch = atan(deta_dy); + double yaw = 0.0; + + gz::math::Quaternion q = + gz::math::Quaternion::EulerToQuaternion(roll, pitch, yaw); + + response->poses[idx].orientation.x = q.X(); + response->poses[idx].orientation.y = q.Y(); + response->poses[idx].orientation.z = q.Z(); + response->poses[idx].orientation.w = q.W(); + + data.unlock(); + } + }; + services_->inc_wave_height_service_ = + ros_->node_->create_service( + "inc_wave_height", + services_->inc_wave_height_handler_); + } +}; + +////////////////////////////////////////////////// +IncWaveHeight::IncWaveHeight() +: dataPtr(std::make_unique()) +{ + this->dataPtr->ros_ = std::make_unique(); + this->dataPtr->services_ = std::make_unique(); +} + +IncWaveHeight::~IncWaveHeight() +{ + // Stop ros2 threads + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + + this->dataPtr->stop_ = true; + if (this->dataPtr->ros_->executor_) { + this->dataPtr->ros_->executor_->cancel(); + } + this->dataPtr->thread_executor_spin_.join(); +} + +void IncWaveHeight::Configure( + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) +{ + // Make sure the controller is attached to a valid model + auto model = gz::sim::Model(_entity); + if (!model.Valid(_ecm)) { + gzerr << "[ROS 2 Incident Wave Height] Failed to initialize because [" << + model.Name(_ecm) << "] is not a model." << std::endl << + "Please make sure that ROS 2 Incident Wave Height is attached to a valid model." << std::endl; + return; + } + + // controller scoped name + std::string scoped_name = gz::sim::scopedName(_entity, _ecm, "/", false); + + // ROS node + std::string ns = _sdf->Get("namespace", scoped_name).first; + if (ns.empty() || ns[0] != '/') { + ns = '/' + ns; + } + std::string node_name = _sdf->Get("node_name", "inc_wave_service").first; + + this->dataPtr->ros2_setup(node_name, ns); + + RCLCPP_INFO_STREAM( + this->dataPtr->ros_->node_->get_logger(), + "[ROS 2 Incident Wave Height] Setting up service for [" << model.Name(_ecm) << "]"); + + this->dataPtr->setup_services(); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void IncWaveHeight::PostUpdate( + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) +{ + GZ_PROFILE("IncWaveHeight::Update"); + + this->dataPtr->paused_ = _info.paused; + this->dataPtr->current_time_ = _info.simTime; + + if (_info.paused) { + return; + } + + // low prio data access + std::unique_lock low(this->dataPtr->low_prio_mutex_); + std::unique_lock next(this->dataPtr->next_access_mutex_); + std::unique_lock data(this->dataPtr->data_mutex_); + next.unlock(); + + this->dataPtr->IncWaveEntity = + _ecm.EntityByComponents(gz::sim::components::Name("IncidentWaves")); + + if (this->dataPtr->IncWaveEntity == gz::sim::kNullEntity) { + this->dataPtr->inc_wave_valid_ = false; + return; + } + + // buoy_gazebo::IncWaveState inc_wave_state; + if (_ecm.EntityHasComponentType( + this->dataPtr->IncWaveEntity, + buoy_gazebo::components::IncWaveState().TypeId())) + { + auto inc_wave_state_comp = + _ecm.Component( + this->dataPtr->IncWaveEntity); + + this->dataPtr->inc_wave_state = buoy_gazebo::IncWaveState(inc_wave_state_comp->Data()); + this->dataPtr->inc_wave_valid_ = true; + } else { + this->dataPtr->inc_wave_valid_ = false; + } + data.unlock(); +} +} // namespace buoy_gazebo + +GZ_ADD_PLUGIN( + buoy_gazebo::IncWaveHeight, + gz::sim::System, + buoy_gazebo::IncWaveHeight::ISystemConfigure, + buoy_gazebo::IncWaveHeight::ISystemPostUpdate) diff --git a/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.hpp b/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.hpp new file mode 100644 index 00000000..ecd7098b --- /dev/null +++ b/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.hpp @@ -0,0 +1,65 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LATENT_DATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ +#define LATENT_DATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ + +#include + +#include + +namespace buoy_gazebo +{ +// Forward declarations. +struct IncWaveHeightPrivate; + +/// \brief ROS 2 Incident Wave Height node for requesting wave heights at given positions +/// Currently accepts incident wave height requests. +/// Uses ros_gz_bridge + +/// SDF parameters: +/// * ``: Namespace for ROS node, defaults to scoped name +/// * ``: ROS2 node name, defaults to "inc_wave_height" +/// * ``: ROS2 service name, defaults to "inc_wave_height" +class IncWaveHeight + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate +{ +public: + /// \brief Constructor + IncWaveHeight(); + + /// \brief Destructor + ~IncWaveHeight() override; + + // Documentation inherited + void Configure( + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; + + // Documentation inherited + void PostUpdate( + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) override; + +private: + /// \brief Private data pointer. + std::unique_ptr dataPtr; +}; +} // namespace buoy_gazebo + +#endif // LATENT_DATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ diff --git a/buoy_tests/tests/wave_body_interactions_heaveonly.cpp b/buoy_tests/tests/wave_body_interactions_heaveonly.cpp index ccfeefa7..425eff8f 100644 --- a/buoy_tests/tests/wave_body_interactions_heaveonly.cpp +++ b/buoy_tests/tests/wave_body_interactions_heaveonly.cpp @@ -31,8 +31,8 @@ #include -#include "FS_Hydrodynamics.hpp" -#include "LinearIncidentWave.hpp" +#include +#include double last_accel = 0; /* The rhs of x' = f(x) defined as a class */ diff --git a/buoy_tests/tests/wave_body_interactions_pitchonly.cpp b/buoy_tests/tests/wave_body_interactions_pitchonly.cpp index e1763c5f..44fff866 100644 --- a/buoy_tests/tests/wave_body_interactions_pitchonly.cpp +++ b/buoy_tests/tests/wave_body_interactions_pitchonly.cpp @@ -31,8 +31,8 @@ #include -#include "FS_Hydrodynamics.hpp" -#include "LinearIncidentWave.hpp" +#include +#include double last_accel = 0; /* The rhs of x' = f(x) defined as a class */ From 36c6b45cd21bb8c6ffe4441a649a6b79d357deb8 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 9 Aug 2023 11:14:34 -0700 Subject: [PATCH 05/26] use parent folder for FreeSurfaceHydrodynamics includes Signed-off-by: Michael Anderson --- buoy_gazebo/src/IncidentWaves/IncWaveState.hpp | 2 +- buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp | 2 +- buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp | 4 ++-- buoy_tests/tests/wave_body_interactions_heaveonly.cpp | 4 ++-- buoy_tests/tests/wave_body_interactions_pitchonly.cpp | 4 ++-- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp b/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp index 018b551a..2d61e089 100644 --- a/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp +++ b/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp @@ -20,7 +20,7 @@ #include #include #include -#include "LinearIncidentWave.hpp" +#include namespace buoy_gazebo { diff --git a/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp b/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp index 49077e0a..1266d108 100644 --- a/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp +++ b/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp @@ -32,7 +32,7 @@ #include #include -#include "LinearIncidentWave.hpp" +#include #include "IncWaveState.hpp" diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index f097fa9c..4fa22ae4 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -45,8 +45,8 @@ #include "IncidentWaves/IncWaveState.hpp" -#include "FS_Hydrodynamics.hpp" -#include "LinearIncidentWave.hpp" +#include +#include #include diff --git a/buoy_tests/tests/wave_body_interactions_heaveonly.cpp b/buoy_tests/tests/wave_body_interactions_heaveonly.cpp index ccfeefa7..425eff8f 100644 --- a/buoy_tests/tests/wave_body_interactions_heaveonly.cpp +++ b/buoy_tests/tests/wave_body_interactions_heaveonly.cpp @@ -31,8 +31,8 @@ #include -#include "FS_Hydrodynamics.hpp" -#include "LinearIncidentWave.hpp" +#include +#include double last_accel = 0; /* The rhs of x' = f(x) defined as a class */ diff --git a/buoy_tests/tests/wave_body_interactions_pitchonly.cpp b/buoy_tests/tests/wave_body_interactions_pitchonly.cpp index e1763c5f..44fff866 100644 --- a/buoy_tests/tests/wave_body_interactions_pitchonly.cpp +++ b/buoy_tests/tests/wave_body_interactions_pitchonly.cpp @@ -31,8 +31,8 @@ #include -#include "FS_Hydrodynamics.hpp" -#include "LinearIncidentWave.hpp" +#include +#include double last_accel = 0; /* The rhs of x' = f(x) defined as a class */ From d47aedffe1d03dd7dc67935ccbf812ef50f59850 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 9 Aug 2023 11:21:11 -0700 Subject: [PATCH 06/26] add matrix_mode: False option to sim params yaml to allow vectorized mode Signed-off-by: Michael Anderson --- buoy_gazebo/scripts/mbari_wec_batch | 37 +++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index eba8f864..a61aee49 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -45,6 +45,14 @@ from rclpy.node import Node DEFAULT_PHYSICS_MAX_STEP_SIZE = 0.001 # seconds +def to_shape(a, shape): + y_ = shape[0] + y = a.shape[0] + y_pad = (y_-y) + return np.pad(a, (0, y_pad), + mode = 'constant', constant_values=a[-1]) + + class MonoChromatic(object): def __init__(self, A, T): self.A, self.T = A, T @@ -349,13 +357,28 @@ def generate_simulations(sim_params_yaml): os.path.join(batch_results_dir, sim_params_date_yaml)) - # generate test matrix - batch_params = list(zip(*[param.ravel() for param in np.meshgrid(physics_step, - door_state, - scale_factor, - battery_state, - mean_piston_position, - incident_waves)])) + if 'matrix_mode' not in sim_params or \ + ('matrix_mode' in sim_params and sim_params['matrix_mode']): + # generate test matrix + batch_params = list(zip(*[param.ravel() for param in np.meshgrid(physics_step, + door_state, + scale_factor, + battery_state, + mean_piston_position, + incident_waves)])) + elif 'matrix_mode' in sim_params and not sim_params['matrix_mode']: + # generate test arrays + batch_params = [physics_step, + door_state, + scale_factor, + battery_state, + mean_piston_position, + incident_waves] + shape = max(batch_params, key=len).shape + for idx, param in enumerate(batch_params): + if np.array(param).shape != shape: + batch_params[idx] = to_shape(np.array(param), shape) + batch_params = list(zip(*[np.array(param).ravel() for param in batch_params])) node.get_logger().info(f'Generated {len(batch_params)} simulation runs') node.get_logger().debug('PhysicsStep, PhysicsRTF, Seed, Duration, DoorState, ScaleFactor' + From c3bfe60129c709977d8573e54933fda70ce1a7aa Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 23 Aug 2023 17:28:20 -0700 Subject: [PATCH 07/26] added latent data and tweaked incwaveheight Signed-off-by: Michael Anderson --- .../models/mbari_wec_ros/model.sdf | 7 + .../CMakeLists.txt | 0 .../IncWaveHeight/CMakeLists.txt | 1 + .../IncWaveHeight/IncWaveHeight.cpp | 182 ++++++++++--- .../IncWaveHeight/IncWaveHeight.hpp | 6 + .../src/LatentData/LatentData/CMakeLists.txt | 8 + .../src/LatentData/LatentData/LatentData.hpp | 103 ++++++++ .../LatentData/LatentDataPublisher.cpp | 249 ++++++++++++++++++ .../LatentData/LatentDataPublisher.hpp | 62 +++++ .../TrefoilController/CMakeLists.txt | 2 - 10 files changed, 585 insertions(+), 35 deletions(-) rename buoy_gazebo/src/{latent_data => LatentData}/CMakeLists.txt (100%) rename buoy_gazebo/src/{latent_data => LatentData}/IncWaveHeight/CMakeLists.txt (98%) rename buoy_gazebo/src/{latent_data => LatentData}/IncWaveHeight/IncWaveHeight.cpp (55%) rename buoy_gazebo/src/{latent_data => LatentData}/IncWaveHeight/IncWaveHeight.hpp (92%) create mode 100644 buoy_gazebo/src/LatentData/LatentData/CMakeLists.txt create mode 100644 buoy_gazebo/src/LatentData/LatentData/LatentData.hpp create mode 100644 buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp create mode 100644 buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.hpp diff --git a/buoy_description/models/mbari_wec_ros/model.sdf b/buoy_description/models/mbari_wec_ros/model.sdf index 205ee11b..8ddfee43 100644 --- a/buoy_description/models/mbari_wec_ros/model.sdf +++ b/buoy_description/models/mbari_wec_ros/model.sdf @@ -53,6 +53,13 @@ 10 + + / + latent_data + latent_data + 10 + + / inc_wave_service diff --git a/buoy_gazebo/src/latent_data/CMakeLists.txt b/buoy_gazebo/src/LatentData/CMakeLists.txt similarity index 100% rename from buoy_gazebo/src/latent_data/CMakeLists.txt rename to buoy_gazebo/src/LatentData/CMakeLists.txt diff --git a/buoy_gazebo/src/latent_data/IncWaveHeight/CMakeLists.txt b/buoy_gazebo/src/LatentData/IncWaveHeight/CMakeLists.txt similarity index 98% rename from buoy_gazebo/src/latent_data/IncWaveHeight/CMakeLists.txt rename to buoy_gazebo/src/LatentData/IncWaveHeight/CMakeLists.txt index 32741683..1e24715e 100644 --- a/buoy_gazebo/src/latent_data/IncWaveHeight/CMakeLists.txt +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/CMakeLists.txt @@ -11,5 +11,6 @@ gz_add_plugin(IncWaveHeight FreeSurfaceHydrodynamics INCLUDE_DIRS ../.. + .. ROS ) diff --git a/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp similarity index 55% rename from buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.cpp rename to buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp index a42de009..7205283c 100644 --- a/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.cpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp @@ -29,6 +29,7 @@ #include #include +#include using namespace std::chrono_literals; @@ -54,10 +55,16 @@ struct IncWaveHeightPrivate gz::sim::Entity IncWaveEntity{gz::sim::kNullEntity}; buoy_gazebo::IncWaveState inc_wave_state; + gz::sim::Entity entity{gz::sim::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; + std::chrono::steady_clock::duration current_time_; + buoy_gazebo::IncWaveHeights inc_wave_heights; + std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; std::atomic inc_wave_valid_{false}; + std::atomic got_new_request_{false}; std::thread thread_executor_spin_; std::atomic stop_{false}, paused_{true}; @@ -93,6 +100,32 @@ struct IncWaveHeightPrivate thread_executor_spin_ = std::thread(spin); } + std::tuple > compute_eta(const double & _x, + const double & _y, + const double & SimTime, + const bool use_buoy_origin) + { + double x = _x; + double y = _y; + if (use_buoy_origin) { + x += this->inc_wave_state.x; + y += this->inc_wave_state.y; + } + + double deta_dx{0.0}, deta_dy{0.0}; + double eta = this->inc_wave_state.Inc.eta( + x, y, SimTime, &deta_dx, &deta_dy); + + double roll = atan(deta_dx); + double pitch = atan(deta_dy); + double yaw = 0.0; + + gz::math::Quaternion q = + gz::math::Quaternion::EulerToQuaternion(roll, pitch, yaw); + + return std::make_tuple(eta, q); + } + void setup_services() { // IncWaveHeight @@ -110,43 +143,50 @@ struct IncWaveHeightPrivate std::unique_lock data(data_mutex_); next.unlock(); - response->valid = inc_wave_valid_; - if (!inc_wave_valid_) { + response->valid = this->inc_wave_valid_; + if (!this->inc_wave_valid_) { + data.unlock(); return; } double SimTime = std::chrono::duration(current_time_).count(); - - response->poses.resize(request->points.size()); + auto sec_nsec = gz::math::durationToSecNsec(current_time_); + + this->got_new_request_ = true; + response->stamp.sec = sec_nsec.first; + response->stamp.nanosec = sec_nsec.second; + response->heights.resize(request->points.size()); + this->inc_wave_heights.sec = sec_nsec.first; + this->inc_wave_heights.nsec = sec_nsec.second; + this->inc_wave_heights.points.clear(); + this->inc_wave_heights.points.resize(request->points.size()); for (std::size_t idx = 0U; idx < request->points.size(); ++idx) { + bool use_buoy_origin = request->use_buoy_origin; double x = request->points[idx].x; double y = request->points[idx].y; - if (request->use_buoy_origin) { - x += inc_wave_state.x; - y += inc_wave_state.y; - } - - double deta_dx{0.0}, deta_dy{0.0}; - double eta = inc_wave_state.Inc.eta( - x, y, SimTime, &deta_dx, &deta_dy); - - response->poses[idx].position = request->points[idx]; - response->poses[idx].position.z = eta; - double roll = atan(deta_dx); - double pitch = atan(deta_dy); - double yaw = 0.0; - - gz::math::Quaternion q = - gz::math::Quaternion::EulerToQuaternion(roll, pitch, yaw); - - response->poses[idx].orientation.x = q.X(); - response->poses[idx].orientation.y = q.Y(); - response->poses[idx].orientation.z = q.Z(); - response->poses[idx].orientation.w = q.W(); - - data.unlock(); + double eta{0.0}; + gz::math::Quaternion q; + std::tie(eta, q) = compute_eta(x, y, SimTime, use_buoy_origin); + + response->heights[idx].pose.position = request->points[idx]; + response->heights[idx].pose.position.z = eta; + + response->heights[idx].pose.orientation.x = q.X(); + response->heights[idx].pose.orientation.y = q.Y(); + response->heights[idx].pose.orientation.z = q.Z(); + response->heights[idx].pose.orientation.w = q.W(); + + this->inc_wave_heights.points[idx].use_buoy_origin = use_buoy_origin; + this->inc_wave_heights.points[idx].x = x; + this->inc_wave_heights.points[idx].y = y; + this->inc_wave_heights.points[idx].eta = eta; + this->inc_wave_heights.points[idx].qx = q.X(); + this->inc_wave_heights.points[idx].qy = q.Y(); + this->inc_wave_heights.points[idx].qz = q.Z(); + this->inc_wave_heights.points[idx].qw = q.W(); } + data.unlock(); }; services_->inc_wave_height_service_ = ros_->node_->create_service( @@ -184,10 +224,11 @@ void IncWaveHeight::Configure( gz::sim::EventManager &) { // Make sure the controller is attached to a valid model - auto model = gz::sim::Model(_entity); - if (!model.Valid(_ecm)) { + this->dataPtr->entity = _entity; + this->dataPtr->model = gz::sim::Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) { gzerr << "[ROS 2 Incident Wave Height] Failed to initialize because [" << - model.Name(_ecm) << "] is not a model." << std::endl << + this->dataPtr->model.Name(_ecm) << "] is not a model." << std::endl << "Please make sure that ROS 2 Incident Wave Height is attached to a valid model." << std::endl; return; } @@ -206,11 +247,86 @@ void IncWaveHeight::Configure( RCLCPP_INFO_STREAM( this->dataPtr->ros_->node_->get_logger(), - "[ROS 2 Incident Wave Height] Setting up service for [" << model.Name(_ecm) << "]"); + "[ROS 2 Incident Wave Height] Setting up service for [" + << this->dataPtr->model.Name(_ecm) << "]"); this->dataPtr->setup_services(); } +///////////////////////////////////////////////////////////////////////////////////////////// +void IncWaveHeight::PreUpdate( + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) +{ + GZ_PROFILE("IncWaveHeight::PreUpdate"); + + this->dataPtr->paused_ = _info.paused; + this->dataPtr->current_time_ = _info.simTime; + + if (_info.paused) { + return; + } + + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->entity, + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->entity); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + + // low prio data access + std::unique_lock low(this->dataPtr->low_prio_mutex_); + std::unique_lock next(this->dataPtr->next_access_mutex_); + std::unique_lock data(this->dataPtr->data_mutex_); + next.unlock(); + + if (!this->dataPtr->inc_wave_valid_) { + data.unlock(); + return; + } + + if (this->dataPtr->got_new_request_) { + latent_data.inc_wave_heights = this->dataPtr->inc_wave_heights; + this->dataPtr->got_new_request_ = false; + } + + if (latent_data.inc_wave_heights.points.size() == 0U) { + data.unlock(); + return; + } + + double SimTime = std::chrono::duration(this->dataPtr->current_time_).count(); + auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); + latent_data.inc_wave_heights.sec = sec_nsec.first; + latent_data.inc_wave_heights.nsec = sec_nsec.second; + + std::size_t idx = 0U; + for (; idx < latent_data.inc_wave_heights.points.size(); ++idx) { + double use_buoy_origin = latent_data.inc_wave_heights.points[idx].use_buoy_origin; + double x = latent_data.inc_wave_heights.points[idx].x; + double y = latent_data.inc_wave_heights.points[idx].y; + + double eta{0.0}; + gz::math::Quaternion q; + std::tie(eta, q) = this->dataPtr->compute_eta(x, y, SimTime, use_buoy_origin); + + latent_data.inc_wave_heights.points[idx].eta = eta; + latent_data.inc_wave_heights.points[idx].qx = q.X(); + latent_data.inc_wave_heights.points[idx].qy = q.Y(); + latent_data.inc_wave_heights.points[idx].qz = q.Z(); + latent_data.inc_wave_heights.points[idx].qw = q.W(); + } + data.unlock(); + + _ecm.SetComponentData( + this->dataPtr->entity, + latent_data); +} + ///////////////////////////////////////////////////////////////////////////////////////////// void IncWaveHeight::PostUpdate( const gz::sim::UpdateInfo & _info, @@ -239,7 +355,6 @@ void IncWaveHeight::PostUpdate( return; } - // buoy_gazebo::IncWaveState inc_wave_state; if (_ecm.EntityHasComponentType( this->dataPtr->IncWaveEntity, buoy_gazebo::components::IncWaveState().TypeId())) @@ -261,4 +376,5 @@ GZ_ADD_PLUGIN( buoy_gazebo::IncWaveHeight, gz::sim::System, buoy_gazebo::IncWaveHeight::ISystemConfigure, + buoy_gazebo::IncWaveHeight::ISystemPreUpdate, buoy_gazebo::IncWaveHeight::ISystemPostUpdate) diff --git a/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.hpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.hpp similarity index 92% rename from buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.hpp rename to buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.hpp index ecd7098b..e2d8f711 100644 --- a/buoy_gazebo/src/latent_data/IncWaveHeight/IncWaveHeight.hpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.hpp @@ -35,6 +35,7 @@ struct IncWaveHeightPrivate; class IncWaveHeight : public gz::sim::System, public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, public gz::sim::ISystemPostUpdate { public: @@ -51,6 +52,11 @@ class IncWaveHeight gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) override; + // Documentation inherited + void PreUpdate( + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; + // Documentation inherited void PostUpdate( const gz::sim::UpdateInfo & _info, diff --git a/buoy_gazebo/src/LatentData/LatentData/CMakeLists.txt b/buoy_gazebo/src/LatentData/LatentData/CMakeLists.txt new file mode 100644 index 00000000..3729731f --- /dev/null +++ b/buoy_gazebo/src/LatentData/LatentData/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_plugin(LatentData + SOURCES + LatentDataPublisher.cpp + INCLUDE_DIRS + ../.. + .. + ROS +) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp new file mode 100644 index 00000000..f8250cd4 --- /dev/null +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -0,0 +1,103 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LATENTDATA__LATENTDATA_HPP_ +#define LATENTDATA__LATENTDATA_HPP_ + +#include +#include +#include + + +namespace buoy_gazebo +{ +struct IncWaveHeightPoint +{ + // ==== position ==== + // x, y may be relative to buoy origin + bool use_buoy_origin{false}; // input + double x{0.0}; // input + double y{0.0}; // input + + // z (height) + double eta{0.0}; // output + + // ==== orientation ==== + double qx{0.0}, qy{0.0}, qz{0.0}, qw{0.0}; // output + + bool operator==(const IncWaveHeightPoint & that) const + { + bool equal = fabs(this->x - that.x) < 1e-7F; + equal &= fabs(this->y - that.y) < 1e-7F; + equal &= fabs(this->eta - that.eta) < 1e-7F; + equal &= fabs(this->qx - that.qx) < 1e-7F; + equal &= fabs(this->qy - that.qy) < 1e-7F; + equal &= fabs(this->qz - that.qz) < 1e-7F; + equal &= fabs(this->qw - that.qw) < 1e-7F; + return equal; + } +}; + +struct IncWaveHeights { + int32_t sec{0}; + uint32_t nsec{0U}; + std::vector points; + + bool operator==(const IncWaveHeights & that) const + { + // shortcut different sizes as not equal + bool equal = (this->points.size() == that.points.size()); + if (!equal) { + return false; + } + + // since sizes are equal, iterate over all points (or none) + std::size_t idx = 0U; + for (; idx < this->points.size(); ++idx) + { + equal &= (this->points[idx] == that.points[idx]); + } + + return equal; + } +}; + +/// \brief latent data that is modeled but not directly observed for LatentData message in ROS 2 +struct LatentData +{ + IncWaveHeights inc_wave_heights; + + bool operator==(const LatentData & that) const + { + bool equal = (this->inc_wave_heights == that.inc_wave_heights); + // equal &= fabs(this-> - that.) < 1e-7F; + return equal; + } +}; + +namespace components +{ +/// \brief Latent data as component for that is modeled but not directly observed for LatentData +/// message in ROS 2 +using LatentData = + gz::sim::components::Component; +GZ_SIM_REGISTER_COMPONENT( + "buoy_gazebo.components.LatentData", + LatentData) +} // namespace components + +} // namespace buoy_gazebo + +#endif // LATENTDATA__LATENTDATA_HPP_ diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp new file mode 100644 index 00000000..5d98ea1d --- /dev/null +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -0,0 +1,249 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "LatentDataPublisher.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "buoy_utils/Rate.hpp" + + +struct buoy_gazebo::LatentDataPublisherPrivate +{ + rclcpp::Node::SharedPtr rosnode_{nullptr}; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + + gz::sim::Entity entity_; + gz::transport::Node node_; + bool use_sim_time_{true}; + std::chrono::steady_clock::duration current_time_; + + rclcpp::Publisher::SharedPtr ros2_pub_{nullptr}; + + double pub_rate_hz_{10.0}; + std::unique_ptr pub_rate_{nullptr}; + buoy_interfaces::msg::LatentData latent_data_; + std::atomic data_valid_{false}; + + std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; + + std::thread thread_executor_spin_, thread_publish_; + std::atomic stop_{false}, paused_{true}; + + void copy_inc_wave_height(const buoy_gazebo::IncWaveHeightPoint & in, + buoy_interfaces::msg::IncWaveHeight & out) + { + out.use_buoy_origin = in.use_buoy_origin; + out.pose.position.x = in.x; + out.pose.position.y = in.y; + out.pose.position.z = in.eta; + out.pose.orientation.x = in.qx; + out.pose.orientation.y = in.qy; + out.pose.orientation.z = in.qz; + out.pose.orientation.w = in.qw; + } +}; + + +GZ_ADD_PLUGIN( + buoy_gazebo::LatentDataPublisher, + gz::sim::System, + buoy_gazebo::LatentDataPublisher::ISystemConfigure, + buoy_gazebo::LatentDataPublisher::ISystemPostUpdate) + +namespace buoy_gazebo +{ +////////////////////////////////////////////////// +LatentDataPublisher::LatentDataPublisher() +: dataPtr(std::make_unique()) +{ +} + +LatentDataPublisher::~LatentDataPublisher() +{ + // Stop ros2 threads + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + + this->dataPtr->stop_ = true; + this->dataPtr->executor_->cancel(); + this->dataPtr->thread_executor_spin_.join(); + this->dataPtr->thread_publish_.join(); +} + +void LatentDataPublisher::Configure( + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) +{ + // Make sure the controller is attached to a valid model + auto model = gz::sim::Model(_entity); + if (!model.Valid(_ecm)) { + gzerr << "[ROS 2 LatentData Publisher] Failed to initialize because [" << \ + model.Name(_ecm) << "] is not a model." << std::endl << \ + "Please make sure that ROS 2 LatentData Publisher is attached to a valid model." << std::endl; + return; + } + this->dataPtr->entity_ = _entity; + + // Get params from SDF + // controller scoped name + std::string scoped_name = gz::sim::scopedName(this->dataPtr->entity_, _ecm, "/", false); + + // ROS node + std::string ns = _sdf->Get("namespace", scoped_name).first; + if (ns.empty() || ns[0] != '/') { + ns = '/' + ns; + } + + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + std::string node_name = _sdf->Get("node_name", "latent_data").first; + this->dataPtr->rosnode_ = rclcpp::Node::make_shared(node_name, ns); + this->dataPtr->rosnode_->set_parameter( + rclcpp::Parameter( + "use_sim_time", + this->dataPtr->use_sim_time_)); + + this->dataPtr->executor_ = std::make_shared(); + this->dataPtr->executor_->add_node(this->dataPtr->rosnode_); + this->dataPtr->stop_ = false; + + auto spin = [this]() + { + rclcpp::Rate rate(50.0); + while (rclcpp::ok() && !this->dataPtr->stop_) { + this->dataPtr->executor_->spin_once(); + rate.sleep(); + } + }; + this->dataPtr->thread_executor_spin_ = std::thread(spin); + + RCLCPP_INFO_STREAM( + this->dataPtr->rosnode_->get_logger(), + "[ROS 2 LatentData Publisher] Setting up controller for [" << model.Name(_ecm) << "]"); + + // Set header + auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); + this->dataPtr->latent_data_.header.stamp.sec = sec_nsec.first; + this->dataPtr->latent_data_.header.stamp.nanosec = sec_nsec.second; + + // Publisher + std::string ros2_topic = _sdf->Get("ros2_topic", "latent_data").first; + this->dataPtr->ros2_pub_ = + this->dataPtr->rosnode_->create_publisher(ros2_topic, 10); + + this->dataPtr->pub_rate_hz_ = + _sdf->Get("publish_rate", this->dataPtr->pub_rate_hz_).first; + this->dataPtr->pub_rate_ = std::make_unique( + this->dataPtr->pub_rate_hz_, + this->dataPtr->rosnode_->get_clock()); + + auto publish = [this]() + { + while (rclcpp::ok() && !this->dataPtr->stop_) { + if (this->dataPtr->ros2_pub_->get_subscription_count() <= 0) {continue;} + // Only update and publish if not paused. + if (this->dataPtr->paused_) {continue;} + + buoy_interfaces::msg::LatentData latent_data; + // high prio data access + std::unique_lock next(this->dataPtr->next_access_mutex_); + std::unique_lock data(this->dataPtr->data_mutex_); + next.unlock(); + + if (this->dataPtr->data_valid_) { + latent_data = this->dataPtr->latent_data_; + data.unlock(); + if (this->dataPtr->ros2_pub_->get_subscription_count() > 0) { + this->dataPtr->ros2_pub_->publish(latent_data); + } + } else { + data.unlock(); + } + this->dataPtr->pub_rate_->sleep(); + } + }; + this->dataPtr->thread_publish_ = std::thread(publish); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void LatentDataPublisher::PostUpdate( + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) +{ + this->dataPtr->paused_ = _info.paused; + if (_info.paused) { + return; + } + + this->dataPtr->current_time_ = _info.simTime; + auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); + + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->entity_, + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->entity_); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + this->dataPtr->data_valid_ = true; + } else { + this->dataPtr->data_valid_ = false; + } + + // low prio data access + std::unique_lock low(this->dataPtr->low_prio_mutex_); + std::unique_lock next(this->dataPtr->next_access_mutex_); + std::unique_lock data(this->dataPtr->data_mutex_); + next.unlock(); + + this->dataPtr->latent_data_.header.stamp.sec = sec_nsec.first; + this->dataPtr->latent_data_.header.stamp.nanosec = sec_nsec.second; + + this->dataPtr->latent_data_.inc_wave_heights.clear(); + this->dataPtr->latent_data_.inc_wave_heights.resize(latent_data.inc_wave_heights.points.size()); + + std::size_t idx = 0U; + for (; idx < latent_data.inc_wave_heights.points.size(); ++idx) { + this->dataPtr->copy_inc_wave_height( + latent_data.inc_wave_heights.points[idx], + this->dataPtr->latent_data_.inc_wave_heights[idx]); + } + + // TODO(andermi) fill in other stuff + + data.unlock(); +} +} // namespace buoy_gazebo diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.hpp new file mode 100644 index 00000000..f120d148 --- /dev/null +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.hpp @@ -0,0 +1,62 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LATENTDATA__LATENTDATA__LATENTDATAPUBLISHER_HPP_ +#define LATENTDATA__LATENTDATA__LATENTDATAPUBLISHER_HPP_ + +#include + +#include + +namespace buoy_gazebo +{ +// Forward declarations. +struct LatentDataPublisherPrivate; + +/// SDF parameters: +/// * ``: Namespace for ROS 2 node, defaults to scoped name +/// * ``: ROS 2 node name, defaults to "latent_data" +/// * ``: ROS 2 topic to publish LatentData, defaults to "latent_data" +/// * ``: ROS 2 topic publish rate, defaults to 10Hz +class LatentDataPublisher + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate +{ +public: + /// \brief Constructor + LatentDataPublisher(); + + /// \brief Destructor + ~LatentDataPublisher() override; + + // Documentation inherited + void Configure( + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; + + // Documentation inherited + void PostUpdate( + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) override; + +private: + /// \brief Private data pointer. + std::unique_ptr dataPtr; +}; +} // namespace buoy_gazebo + +#endif // LATENTDATA__LATENTDATA__LATENTDATAPUBLISHER_HPP_ diff --git a/buoy_gazebo/src/controllers/TrefoilController/CMakeLists.txt b/buoy_gazebo/src/controllers/TrefoilController/CMakeLists.txt index 965c6362..819d3ae2 100644 --- a/buoy_gazebo/src/controllers/TrefoilController/CMakeLists.txt +++ b/buoy_gazebo/src/controllers/TrefoilController/CMakeLists.txt @@ -4,6 +4,4 @@ gz_add_plugin(TrefoilController INCLUDE_DIRS ../.. ROS - EXTRA_ROS_PKGS - ros_gz_sim ) From 8fdfc129b14332a4f6433431287b87ec842ea246 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Thu, 24 Aug 2023 13:54:48 -0700 Subject: [PATCH 08/26] can specify incwaveheight points in sdf Signed-off-by: Michael Anderson --- .../models/mbari_wec_ros/model.sdf | 11 +++++ .../IncWaveHeight/IncWaveHeight.cpp | 49 +++++++++++++++++++ 2 files changed, 60 insertions(+) diff --git a/buoy_description/models/mbari_wec_ros/model.sdf b/buoy_description/models/mbari_wec_ros/model.sdf index 8ddfee43..eb72fef3 100644 --- a/buoy_description/models/mbari_wec_ros/model.sdf +++ b/buoy_description/models/mbari_wec_ros/model.sdf @@ -63,6 +63,17 @@ / inc_wave_service + + -1.0 -1.0 + 0.0 -1.0 + 1.0 -1.0 + -1.0 0.0 + 0.0 0.0 + 1.0 0.0 + -1.0 1.0 + 0.0 1.0 + 1.0 1.0 + + // 1.2 3.4 + // 3.1 4.1 + // + if (_sdf->HasElement("points")) { + const sdf::ElementPtr points = _sdf->GetElementImpl("points"); + if (points != nullptr) { + bool use_buoy_origin{false}; + if (points->GetAttributeSet("use_buoy_origin")) { + const sdf::ParamPtr p = points->GetAttribute("use_buoy_origin"); + if (p != nullptr) { + bool result = p->Get(use_buoy_origin); + if (!result) { + use_buoy_origin = false; + } + } + } + this->dataPtr->inc_wave_heights.points.clear(); + bool first = true; + sdf::ElementPtr e{nullptr}; + for(;;) { + if (first) { + e = points->GetElementImpl("xy"); + first = false; + } else { + e = e->GetNextElement(); + } + if (e == nullptr) { + break; + } + + IncWaveHeightPoint pt; + pt.use_buoy_origin = use_buoy_origin; + std::string xy = e->Get(); + std::stringstream ss(xy); + ss >> pt.x; ss >> pt.y; + if (ss.fail()) { + gzerr << "[ROS 2 Incident Wave Height] Could not parse input point from SDF" << std::endl; + continue; + } + this->dataPtr->inc_wave_heights.points.push_back(pt); + + this->dataPtr->got_new_request_ = true; + } + } else { + gzerr << "[ROS 2 Incident Wave Height] Could not parse input points from SDF" << std::endl; + } + } + // controller scoped name std::string scoped_name = gz::sim::scopedName(_entity, _ecm, "/", false); From f20743d5275e5a92ce8415af8f277011993556f2 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Thu, 24 Aug 2023 17:57:47 -0700 Subject: [PATCH 09/26] added spring and pto (still need a couple pto losses) Signed-off-by: Michael Anderson --- .../models/mbari_wec_ros/model.sdf | 11 ++--- .../src/ElectroHydraulicPTO/CMakeLists.txt | 1 + .../ElectroHydraulicPTO.cpp | 35 ++++++++++++-- .../IncWaveHeight/IncWaveHeight.cpp | 1 + .../src/LatentData/LatentData/LatentData.hpp | 46 +++++++++++++++++++ .../LatentData/LatentDataPublisher.cpp | 32 ++++++++++++- .../PolytropicPneumaticSpring/CMakeLists.txt | 1 + .../PolytropicPneumaticSpring.cpp | 44 ++++++++++++++++-- 8 files changed, 154 insertions(+), 17 deletions(-) diff --git a/buoy_description/models/mbari_wec_ros/model.sdf b/buoy_description/models/mbari_wec_ros/model.sdf index eb72fef3..f2825d41 100644 --- a/buoy_description/models/mbari_wec_ros/model.sdf +++ b/buoy_description/models/mbari_wec_ros/model.sdf @@ -64,15 +64,10 @@ / inc_wave_service - -1.0 -1.0 - 0.0 -1.0 - 1.0 -1.0 - -1.0 0.0 0.0 0.0 - 1.0 0.0 - -1.0 1.0 - 0.0 1.0 - 1.0 1.0 + + + diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt b/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt index f136789e..759aa802 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt +++ b/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt @@ -3,6 +3,7 @@ gz_add_plugin(ElectroHydraulicPTO ElectroHydraulicPTO.cpp INCLUDE_DIRS .. + ../LatentData EXTRA_ROS_PKGS simple_interp ) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 4d439b4e..0a08891d 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "ElectroHydraulicPTO.hpp" + #include #include @@ -40,12 +42,13 @@ #include #include -#include "ElectroHydraulicPTO.hpp" #include "ElectroHydraulicSoln.hpp" #include "ElectroHydraulicState.hpp" #include "ElectroHydraulicLoss.hpp" #include "BatteryState.hpp" +#include + namespace buoy_gazebo { @@ -274,6 +277,17 @@ void ElectroHydraulicPTO::PreUpdate( pto_loss = buoy_gazebo::ElectroHydraulicLoss(pto_loss_comp->Data()); } + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->model.Entity()); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + if (pto_state.scale_command) { this->dataPtr->functor.I_Wind.ScaleFactor = pto_state.scale_command.value(); } else { @@ -406,11 +420,20 @@ void ElectroHydraulicPTO::PreUpdate( pto_loss.hydraulic_motor_loss += 1.0; pto_loss.relief_valve_loss += 2.0; - pto_loss.motor_drive_i2r_loss += 3.0; - pto_loss.motor_drive_switching_loss += 4.0; - pto_loss.motor_drive_friction_loss += 5.0; + pto_loss.motor_drive_i2r_loss = + this->dataPtr->functor.MotorDriveISquaredRLoss(this->dataPtr->functor.I_Wind.I); + pto_loss.motor_drive_switching_loss = this->dataPtr->functor.MotorDriveSwitchingLoss(VBus); + pto_loss.motor_drive_friction_loss = this->dataPtr->functor.MotorDriveFrictionLoss(N); pto_loss.battery_i2r_loss = I_Batt * I_Batt * this->dataPtr->Ri; + latent_data.electro_hydraulic.valid = true; + latent_data.electro_hydraulic.inst_power = VBus * (I_Batt + I_Load); + latent_data.electro_hydraulic.rpm = N; + latent_data.electro_hydraulic.motor_drive_i2r_loss = pto_loss.motor_drive_i2r_loss; + latent_data.electro_hydraulic.motor_drive_switching_loss = pto_loss.motor_drive_switching_loss; + latent_data.electro_hydraulic.motor_drive_friction_loss = pto_loss.motor_drive_friction_loss; + latent_data.electro_hydraulic.battery_i2r_loss = pto_loss.battery_i2r_loss; + _ecm.SetComponentData( this->dataPtr->PrismaticJointEntity, battery_state); @@ -425,6 +448,10 @@ void ElectroHydraulicPTO::PreUpdate( this->dataPtr->PrismaticJointEntity, pto_loss); + _ecm.SetComponentData( + this->dataPtr->model.Entity(), + latent_data); + gz::msgs::Double pistonvel; pistonvel.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); pistonvel.set_data(xdot); diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp index 10781255..23c9cc9f 100644 --- a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp @@ -352,6 +352,7 @@ void IncWaveHeight::PreUpdate( auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); latent_data.inc_wave_heights.sec = sec_nsec.first; latent_data.inc_wave_heights.nsec = sec_nsec.second; + latent_data.inc_wave_heights.valid = this->dataPtr->inc_wave_valid_; std::size_t idx = 0U; for (; idx < latent_data.inc_wave_heights.points.size(); ++idx) { diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index f8250cd4..030d11f0 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -52,6 +52,7 @@ struct IncWaveHeightPoint struct IncWaveHeights { int32_t sec{0}; uint32_t nsec{0U}; + bool valid; std::vector points; bool operator==(const IncWaveHeights & that) const @@ -69,18 +70,63 @@ struct IncWaveHeights { equal &= (this->points[idx] == that.points[idx]); } + equal &= this->valid == that.valid; + + return equal; + } +}; + +struct AirSpring +{ + bool valid{false}; + double force{0.0}; + double T{0.0}; + double dQ_dt{0.0}; + double piston_position{0.0}; + double piston_velocity{0.0}; + + bool operator==(const AirSpring & that) const + { + bool equal = (this->valid == that.valid); + equal &= (this->force == that.force); + equal &= (this->T == that.T); + equal &= (this->dQ_dt == that.dQ_dt); + return equal; } }; +struct ElectroHydraulic +{ + bool valid{false}; + double inst_power{0.0}; + double rpm{0.0}; + double motor_drive_i2r_loss{0.0}; + double motor_drive_friction_loss{0.0}; + double motor_drive_switching_loss{0.0}; + double battery_i2r_loss{0.0}; +}; + /// \brief latent data that is modeled but not directly observed for LatentData message in ROS 2 struct LatentData { IncWaveHeights inc_wave_heights; + AirSpring upper_spring; + AirSpring lower_spring; + ElectroHydraulic electro_hydraulic; + + bool valid() const + { + return inc_wave_heights.valid && \ + upper_spring.valid && lower_spring.valid && \ + electro_hydraulic.valid; + } bool operator==(const LatentData & that) const { bool equal = (this->inc_wave_heights == that.inc_wave_heights); + equal &= (this->upper_spring == that.upper_spring); + equal &= (this->lower_spring == that.lower_spring); // equal &= fabs(this-> - that.) < 1e-7F; return equal; } diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 5d98ea1d..fb93a722 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -218,9 +218,9 @@ void LatentDataPublisher::PostUpdate( _ecm.Component(this->dataPtr->entity_); latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); - this->dataPtr->data_valid_ = true; } else { this->dataPtr->data_valid_ = false; + return; } // low prio data access @@ -242,8 +242,38 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.inc_wave_heights[idx]); } + this->dataPtr->latent_data_.upper_spring.force = latent_data.upper_spring.force; + this->dataPtr->latent_data_.upper_spring.temperature = latent_data.upper_spring.T; + this->dataPtr->latent_data_.upper_spring.heat_loss = latent_data.upper_spring.dQ_dt; + this->dataPtr->latent_data_.upper_spring.piston_position = + latent_data.upper_spring.piston_position; + this->dataPtr->latent_data_.upper_spring.piston_velocity = + latent_data.upper_spring.piston_velocity; + this->dataPtr->latent_data_.lower_spring.force = latent_data.lower_spring.force; + this->dataPtr->latent_data_.lower_spring.temperature = latent_data.lower_spring.T; + this->dataPtr->latent_data_.lower_spring.heat_loss = latent_data.lower_spring.dQ_dt; + this->dataPtr->latent_data_.lower_spring.piston_position = + latent_data.lower_spring.piston_position; + this->dataPtr->latent_data_.lower_spring.piston_velocity = + latent_data.lower_spring.piston_velocity; + + this->dataPtr->latent_data_.electro_hydraulic.inst_power = + latent_data.electro_hydraulic.inst_power; + this->dataPtr->latent_data_.electro_hydraulic.rpm = + latent_data.electro_hydraulic.rpm; + this->dataPtr->latent_data_.electro_hydraulic.motor_drive_i2r_loss = + latent_data.electro_hydraulic.motor_drive_i2r_loss; + this->dataPtr->latent_data_.electro_hydraulic.motor_drive_switching_loss = + latent_data.electro_hydraulic.motor_drive_switching_loss; + this->dataPtr->latent_data_.electro_hydraulic.motor_drive_friction_loss = + latent_data.electro_hydraulic.motor_drive_friction_loss; + this->dataPtr->latent_data_.electro_hydraulic.battery_i2r_loss = + latent_data.electro_hydraulic.battery_i2r_loss; + // TODO(andermi) fill in other stuff + this->dataPtr->data_valid_ = latent_data.valid(); + data.unlock(); } } // namespace buoy_gazebo diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt index 660d0a42..59d44d93 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt @@ -6,6 +6,7 @@ gz_add_plugin(PolytropicPneumaticSpring PolytropicPneumaticSpring.cpp INCLUDE_DIRS .. + ../LatentData PUBLIC_LINK_LIBS gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} ) diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index d334549e..995dc5e6 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "PolytropicPneumaticSpring.hpp" + #include #include @@ -35,8 +37,8 @@ #include #include -#include "PolytropicPneumaticSpring.hpp" #include "SpringState.hpp" +#include using namespace std::chrono_literals; @@ -238,8 +240,10 @@ void PolytropicPneumaticSpring::computeLawOfCoolingForce(const double & x, const this->dataPtr->T += dT; // TODO(andermi) find Qdot (rate of heat transfer) from h, A, dT (Qdot = h*A*dT) + // Get chamber surface area from CAD... not true cylinder + // Also, since the chambers wrap around, h (heat transfer constant) is not quite water/steel/gas const double radius = 0.045; - const double A = (2.0 * this->dataPtr->config_->piston_area) * radius * x; + const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0*GZ_PI*radius*x; const double h = 11.3; // (W/(m^2*K)) -- Water<->Mild Steel<->Gas this->dataPtr->Q_rate = h * A * dT; @@ -270,9 +274,9 @@ void PolytropicPneumaticSpring::computePolytropicForce(const double & x, const d // Retrieved from https://ir.library.oregonstate.edu/downloads/ww72bf399 // heat loss rate for polytropic ideal gas: // dQ/dt = (1 - n/gamma)*(c_p/R)*P*A*dx/dt - // TODO(andermi) A != piston_area... it's the chamber surface area + // TODO(andermi) get chamber surface area from CAD... not a true cylinder const double r = 0.045; - const double A = (2.0 * this->dataPtr->config_->piston_area) * r * x; + const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0*GZ_PI*r*x; this->dataPtr->Q_rate = (1.0 - this->dataPtr->n / PolytropicPneumaticSpringConfig::ADIABATIC_INDEX) * cp_R * this->dataPtr->P * A * v; @@ -559,6 +563,7 @@ void PolytropicPneumaticSpring::PreUpdate( if (this->dataPtr->config_->is_upper) { this->dataPtr->F *= -1.0; + this->dataPtr->Q_rate *= -1.0; } auto stampMsg = gz::sim::convert(_info.simTime); @@ -574,6 +579,37 @@ void PolytropicPneumaticSpring::PreUpdate( this->dataPtr->config_->jointEntity, spring_state); + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->config_->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->config_->model.Entity()); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + + if (this->dataPtr->config_->is_upper) { + latent_data.upper_spring.valid = true; + latent_data.upper_spring.force = this->dataPtr->F; + latent_data.upper_spring.T = this->dataPtr->T; + latent_data.upper_spring.dQ_dt = this->dataPtr->Q_rate; + latent_data.upper_spring.piston_position = x; + latent_data.upper_spring.piston_velocity = v; + } else { + latent_data.lower_spring.valid = true; + latent_data.lower_spring.force = this->dataPtr->F; + latent_data.lower_spring.T = this->dataPtr->T; + latent_data.lower_spring.dQ_dt = this->dataPtr->Q_rate; + latent_data.lower_spring.piston_position = this->dataPtr->config_->stroke - x; + latent_data.lower_spring.piston_velocity = -v; + } + + _ecm.SetComponentData( + this->dataPtr->config_->model.Entity(), + latent_data); + gz::msgs::Double force; force.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); force.set_data(this->dataPtr->F); From 39e063ac9367b8640f5bce8b782e367b59f673d2 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 28 Aug 2023 17:56:51 -0700 Subject: [PATCH 10/26] linters Signed-off-by: Michael Anderson --- .../scripts/compute_polytropic.py | 92 ++++++++++--------- .../IncWaveHeight/IncWaveHeight.cpp | 12 ++- .../IncWaveHeight/IncWaveHeight.hpp | 6 +- .../src/LatentData/LatentData/LatentData.hpp | 14 +-- .../LatentData/LatentDataPublisher.cpp | 5 +- 5 files changed, 72 insertions(+), 57 deletions(-) diff --git a/buoy_description/scripts/compute_polytropic.py b/buoy_description/scripts/compute_polytropic.py index 5f31427a..796ef7e2 100755 --- a/buoy_description/scripts/compute_polytropic.py +++ b/buoy_description/scripts/compute_polytropic.py @@ -37,8 +37,8 @@ Ap_l *= 0.0254**2.0 # Ap_u = 0.0127 # Area of piston in upper chamber # Ap_l = 0.0115 # Area of piston in lower -Vd_u = 0.0226 # est from cad 0.0172 # 0.015 # 0.0226 # 0.0266 # Dead volume of upper -Vd_l = 0.0463 # est from cad 0.0546 # 0.025 # 0.0463 # 0.0523 # Dead volume of lower +Vd_u = 0.0226 # est from cad 0.0172 # 0.015 # 0.0226 # 0.0266 # Dead volume of upper +Vd_l = 0.0463 # est from cad 0.0546 # 0.025 # 0.0463 # 0.0523 # Dead volume of lower r = 0.5 # coef of heat transfer (Newton's law of cooling) c_p = 1.04 # for N2 @@ -149,17 +149,18 @@ def set_piston_position(V0_u, # Volume setpoint from upper chamber polytropic P # ---------------- - ---------------- = ---*(rho*V - m) = C # Ap_u*x_eq + Vd_u Ap_l*x_eq + Vd_l R*T - C = lambda g, R_specific, T_ocean, rho, V_hc, m: (g / (R_specific*T_ocean))*(rho*V_hc - m) - x_eq = lambda Ap_u, Ap_l, m_u, m_l, Vd_u, Vd_l, C: -(0.005*( + C = lambda g, R_specific, T_ocean, rho, V_hc, m: \ + (g / (R_specific*T_ocean))*(rho*V_hc - m) # noqa: E731 + x_eq = lambda Ap_u, Ap_l, m_u, m_l, Vd_u, Vd_l, C: -(0.005*( # noqa: E731 math.sqrt( ( - 203*Ap_u*Ap_l*C + 100*Ap_u*C*Vd_l + 100*Ap_u*m_l - \ + 203*Ap_u*Ap_l*C + 100*Ap_u*C*Vd_l + 100*Ap_u*m_l - 100*Ap_l*C*Vd_u + 100*Ap_l*m_u - )**2 + \ + )**2 + 400*Ap_u*Ap_l*C*( 203*Ap_l*C*Vd_u - 203*Ap_l*m_u + 100*C*Vd_u*Vd_l + 100*m_l*Vd_u - 100*m_u*Vd_l ) - ) - \ + ) - 203*Ap_u*Ap_l*C - 100*Ap_u*C*Vd_l - 100*Ap_u*m_l + 100*Ap_l*C*Vd_u - 100*Ap_l*m_u ) ) / (Ap_u*Ap_l*C) @@ -228,7 +229,8 @@ def computePolytropicForce(V_, P, V, n, c, v, is_upper): Q_rate = 0.0 is_adiabatic = False if !is_adiabatic: - # Rodrigues, M. J. (June 5, 2014). Heat Transfer During the Piston-Cylinder Expansion of a Gas + # Rodrigues, M. J. (June 5, 2014). Heat Transfer During the + # Piston-Cylinder Expansion of a Gas # (Master's thesis, Oregon State University). # Retrieved from https://ir.library.oregonstate.edu/downloads/ww72bf399 # heat loss rate for polytropic ideal gas: @@ -253,7 +255,7 @@ def model_thermal(PV, p, dt, ax2=None): n = n1 c = P0 * V0 / T0 # m*R_specific - mass = c / R_specific + # mass = c / R_specific V = V0 P = P0 @@ -272,19 +274,19 @@ def model_thermal(PV, p, dt, v *= -1.0 if v_ >= vel_dz_u: - #print(f'vel above: {v_}') + # print(f'vel above: {v_}') n = n1 F, P, V, T = computePolytropicForce(V_, P, V, n, c, v_, is_upper) elif v_ <= vel_dz_l: - #print(f'vel below: {v_}') + # print(f'vel below: {v_}') n = n2 F, P, V, T = computePolytropicForce(V_, P, V, n, c, v_, is_upper) else: - #print(f'cooling: {v_}') + # print(f'cooling: {v_}') F, P, V, T = computeLawOfCoolingForce(V_, T, c, dt, is_upper) - #if is_upper: - # F *= -1.0 + # if is_upper: + # F *= -1.0 P_data.append(P) V_data.append(V) T_data.append(T) @@ -322,19 +324,27 @@ def main(): parser.add_argument('atsea_logs', nargs='+') parser.add_argument('--plot', action='store_true') parser.add_argument('--override', action='store_true') - parser.add_argument('--polytropic_u', nargs=6, type=float) # n1_u, n2_u, - # P0_u, V0_u, - # C1_u, C2_u - parser.add_argument('--polytropic_l', nargs=6, type=float) # n1_l, n2_l - # P0_l, V0_l, - # C1_l, C2_l + + # n1_u, n2_u, + # P0_u, V0_u, + # C1_u, C2_u + parser.add_argument('--polytropic_u', nargs=6, type=float) + + # n1_l, n2_l + # P0_l, V0_l, + # C1_l, C2_l + parser.add_argument('--polytropic_l', nargs=6, type=float) parser.add_argument('--model_thermal', action='store_true') - parser.add_argument('--thermal_params_u', nargs=3, type=float) # T0 - # velocity_deadzone_upper - # velocity_deadzone_lower - parser.add_argument('--thermal_params_l', nargs=3, type=float) # T0 - # velocity_deadzone_upper - # velocity_deadzone_lower + + # T0 + # velocity_deadzone_upper + # velocity_deadzone_lower + parser.add_argument('--thermal_params_u', nargs=3, type=float) + + # T0 + # velocity_deadzone_upper + # velocity_deadzone_lower + parser.add_argument('--thermal_params_l', nargs=3, type=float) parser.add_argument('--set_piston_pos', action='store_true') # parser.add_argument('--piston_pos', nargs=2, type=float) # initial_piston_position # # x_mean_pos @@ -396,15 +406,15 @@ def main(): fig2, ax2 = plt.subplots(2, 1) dt = np.mean(np.diff(spring_data.t)) F_u = model_thermal(spring_data.loc[(1, ('uv', 'up'))].to_numpy(), - spring_data.p, dt, - P0_u, V0_u, n1_u, n2_u, - *args.thermal_params_u, - is_upper=True, ax1=ax1, ax2=ax2) + spring_data.p, dt, + P0_u, V0_u, n1_u, n2_u, + *args.thermal_params_u, + is_upper=True, ax1=ax1, ax2=ax2) F_l = model_thermal(spring_data.loc[(1, ('lv', 'lp'))].to_numpy(), - spring_data.p, dt, - P0_l, V0_l, n1_l, n2_l, - *args.thermal_params_l, - is_upper=False, ax1=ax1, ax2=ax2) + spring_data.p, dt, + P0_l, V0_l, n1_l, n2_l, + *args.thermal_params_l, + is_upper=False, ax1=ax1, ax2=ax2) fig3, ax3 = plt.subplots(1, 1) ax3.plot(spring_data.p/0.0254, @@ -421,7 +431,7 @@ def main(): ax3.set_title(f'Stiffness: AtSea={atsea_stiffness:.2f} Sim={sim_stiffness:.2f}') if ax1 is not None: - import os + # import os for ax1_ in ax1: ax1_.legend() ax1_.set_xlabel('Volume (m^3)') @@ -437,21 +447,21 @@ def main(): f' vel_dz_u={args.thermal_params_l[1]}' f' vel_dz_l={args.thermal_params_l[2]}') fig1.tight_layout() - #fig1.savefig(os.path.basename(args.atsea_logs[0])+'.PVPolytropicFit.png', - # dpi=fig1.dpi, bbox_inches='tight') + # fig1.savefig(os.path.basename(args.atsea_logs[0])+'.PVPolytropicFit.png', + # dpi=fig1.dpi, bbox_inches='tight') for ax2_ in ax2: ax2_.legend() ax2_.set_xlabel('Time from start (sec)') ax2_.set_ylabel('Pressure (Pascals)') fig2.tight_layout() - #fig2.savefig(os.path.basename(args.atsea_logs[0])+'.PressureDiff.png', - # dpi=fig2.dpi, bbox_inches='tight') + # fig2.savefig(os.path.basename(args.atsea_logs[0])+'.PressureDiff.png', + # dpi=fig2.dpi, bbox_inches='tight') ax3.set_xlabel('Piston Position (in)') ax3.set_ylabel('Force (pound-force)') ax3.legend() fig3.tight_layout() - #fig3.savefig(os.path.basename(args.atsea_logs[0])+'.Stiffness.png', - # dpi=fig3.dpi, bbox_inches='tight') + # fig3.savefig(os.path.basename(args.atsea_logs[0])+'.Stiffness.png', + # dpi=fig3.dpi, bbox_inches='tight') plt.show() diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp index 10781255..574314c2 100644 --- a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp @@ -15,6 +15,7 @@ #include "IncWaveHeight.hpp" #include +#include #include #include @@ -100,10 +101,11 @@ struct IncWaveHeightPrivate thread_executor_spin_ = std::thread(spin); } - std::tuple > compute_eta(const double & _x, - const double & _y, - const double & SimTime, - const bool use_buoy_origin) + std::tuple> compute_eta( + const double & _x, + const double & _y, + const double & SimTime, + const bool use_buoy_origin) { double x = _x; double y = _y; @@ -253,7 +255,7 @@ void IncWaveHeight::Configure( this->dataPtr->inc_wave_heights.points.clear(); bool first = true; sdf::ElementPtr e{nullptr}; - for(;;) { + for (;; ) { if (first) { e = points->GetElementImpl("xy"); first = false; diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.hpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.hpp index e2d8f711..c540f9af 100644 --- a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.hpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LATENT_DATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ -#define LATENT_DATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ +#ifndef LATENTDATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ +#define LATENTDATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ #include @@ -68,4 +68,4 @@ class IncWaveHeight }; } // namespace buoy_gazebo -#endif // LATENT_DATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ +#endif // LATENTDATA__INCWAVEHEIGHT__INCWAVEHEIGHT_HPP_ diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index f8250cd4..095a57ee 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LATENTDATA__LATENTDATA_HPP_ -#define LATENTDATA__LATENTDATA_HPP_ +#ifndef LATENTDATA__LATENTDATA__LATENTDATA_HPP_ +#define LATENTDATA__LATENTDATA__LATENTDATA_HPP_ + +#include #include #include @@ -49,7 +51,8 @@ struct IncWaveHeightPoint } }; -struct IncWaveHeights { +struct IncWaveHeights +{ int32_t sec{0}; uint32_t nsec{0U}; std::vector points; @@ -64,8 +67,7 @@ struct IncWaveHeights { // since sizes are equal, iterate over all points (or none) std::size_t idx = 0U; - for (; idx < this->points.size(); ++idx) - { + for (; idx < this->points.size(); ++idx) { equal &= (this->points[idx] == that.points[idx]); } @@ -100,4 +102,4 @@ GZ_SIM_REGISTER_COMPONENT( } // namespace buoy_gazebo -#endif // LATENTDATA__LATENTDATA_HPP_ +#endif // LATENTDATA__LATENTDATA__LATENTDATA_HPP_ diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 5d98ea1d..73738562 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -56,8 +56,9 @@ struct buoy_gazebo::LatentDataPublisherPrivate std::thread thread_executor_spin_, thread_publish_; std::atomic stop_{false}, paused_{true}; - void copy_inc_wave_height(const buoy_gazebo::IncWaveHeightPoint & in, - buoy_interfaces::msg::IncWaveHeight & out) + void copy_inc_wave_height( + const buoy_gazebo::IncWaveHeightPoint & in, + buoy_interfaces::msg::IncWaveHeight & out) { out.use_buoy_origin = in.use_buoy_origin; out.pose.position.x = in.x; From 9c47ee069ae0fe6d6f8e802db8294fdf0de2ffba Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 28 Aug 2023 17:59:52 -0700 Subject: [PATCH 11/26] linters Signed-off-by: Michael Anderson --- buoy_gazebo/src/LatentData/LatentData/LatentData.hpp | 4 ++-- .../PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 117eb1d8..5c88f921 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -120,8 +120,8 @@ struct LatentData bool valid() const { return inc_wave_heights.valid && \ - upper_spring.valid && lower_spring.valid && \ - electro_hydraulic.valid; + upper_spring.valid && lower_spring.valid && \ + electro_hydraulic.valid; } bool operator==(const LatentData & that) const diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 995dc5e6..5f392ffe 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -243,7 +243,7 @@ void PolytropicPneumaticSpring::computeLawOfCoolingForce(const double & x, const // Get chamber surface area from CAD... not true cylinder // Also, since the chambers wrap around, h (heat transfer constant) is not quite water/steel/gas const double radius = 0.045; - const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0*GZ_PI*radius*x; + const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0 * GZ_PI * radius * x; const double h = 11.3; // (W/(m^2*K)) -- Water<->Mild Steel<->Gas this->dataPtr->Q_rate = h * A * dT; @@ -276,7 +276,7 @@ void PolytropicPneumaticSpring::computePolytropicForce(const double & x, const d // dQ/dt = (1 - n/gamma)*(c_p/R)*P*A*dx/dt // TODO(andermi) get chamber surface area from CAD... not a true cylinder const double r = 0.045; - const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0*GZ_PI*r*x; + const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0 * GZ_PI * r * x; this->dataPtr->Q_rate = (1.0 - this->dataPtr->n / PolytropicPneumaticSpringConfig::ADIABATIC_INDEX) * cp_R * this->dataPtr->P * A * v; From 5251ce3f733a34ebe6dd2ba0d137ea843e8192c3 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Fri, 1 Sep 2023 09:51:12 -0700 Subject: [PATCH 12/26] add ehpto force Signed-off-by: Michael Anderson --- .../ElectroHydraulicPTO/ElectroHydraulicPTO.cpp | 4 +++- .../src/LatentData/LatentData/LatentData.hpp | 14 ++++++++------ .../LatentData/LatentData/LatentDataPublisher.cpp | 2 ++ 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 53faa358..8bc352ee 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -426,9 +426,12 @@ void ElectroHydraulicPTO::PreUpdate( pto_loss.motor_drive_friction_loss = this->dataPtr->functor.MotorDriveFrictionLoss(N); pto_loss.battery_i2r_loss = I_Batt * I_Batt * this->dataPtr->Ri; + double piston_force = -deltaP * this->dataPtr->PistonArea * buoy_utils::NEWTONS_PER_LB; + latent_data.electro_hydraulic.valid = true; latent_data.electro_hydraulic.inst_power = VBus * (I_Batt + I_Load); latent_data.electro_hydraulic.rpm = N; + latent_data.electro_hydraulic.force = piston_force; latent_data.electro_hydraulic.motor_drive_i2r_loss = pto_loss.motor_drive_i2r_loss; latent_data.electro_hydraulic.motor_drive_switching_loss = pto_loss.motor_drive_switching_loss; latent_data.electro_hydraulic.motor_drive_friction_loss = pto_loss.motor_drive_friction_loss; @@ -464,7 +467,6 @@ void ElectroHydraulicPTO::PreUpdate( // Apply force if not in Velocity Mode, in which case a joint velocity is applied elsewhere // (likely by a test Fixture) if (!this->dataPtr->VelMode) { - double piston_force = -deltaP * this->dataPtr->PistonArea * buoy_utils::NEWTONS_PER_LB; // Create new component for this entitiy in ECM (if it doesn't already exist) auto forceComp = _ecm.Component( this->dataPtr->PrismaticJointEntity); diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 5c88f921..40af76a5 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -36,6 +36,7 @@ struct IncWaveHeightPoint double eta{0.0}; // output // ==== orientation ==== + // normal vector of deta/dx, deta/dy (slope of wave) double qx{0.0}, qy{0.0}, qz{0.0}, qw{0.0}; // output bool operator==(const IncWaveHeightPoint & that) const @@ -81,11 +82,11 @@ struct IncWaveHeights struct AirSpring { bool valid{false}; - double force{0.0}; - double T{0.0}; - double dQ_dt{0.0}; - double piston_position{0.0}; - double piston_velocity{0.0}; + double force{0.0}; // Newtons + double T{0.0}; // temp in K + double dQ_dt{0.0}; // heat loss rate + double piston_position{0.0}; // meters + double piston_velocity{0.0}; // m/s bool operator==(const AirSpring & that) const { @@ -101,8 +102,9 @@ struct AirSpring struct ElectroHydraulic { bool valid{false}; - double inst_power{0.0}; + double inst_power{0.0}; // Watts double rpm{0.0}; + double force{0.0}; // Newtons double motor_drive_i2r_loss{0.0}; double motor_drive_friction_loss{0.0}; double motor_drive_switching_loss{0.0}; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index e62c5810..b55b7524 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -262,6 +262,8 @@ void LatentDataPublisher::PostUpdate( latent_data.electro_hydraulic.inst_power; this->dataPtr->latent_data_.electro_hydraulic.rpm = latent_data.electro_hydraulic.rpm; + this->dataPtr->latent_data_.electro_hydraulic.force = + latent_data.electro_hydraulic.force; this->dataPtr->latent_data_.electro_hydraulic.motor_drive_i2r_loss = latent_data.electro_hydraulic.motor_drive_i2r_loss; this->dataPtr->latent_data_.electro_hydraulic.motor_drive_switching_loss = From 85af08860725fd41bd457190978692c5a0d4137c Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 5 Sep 2023 12:50:09 -0700 Subject: [PATCH 13/26] added forces from piston friction and wavebodyinteraction Signed-off-by: Michael Anderson --- .../src/LatentData/LatentData/LatentData.hpp | 60 ++++++++++++++++--- .../LatentData/LatentDataPublisher.cpp | 40 +++++++++++++ buoy_gazebo/src/PTOFriction/CMakeLists.txt | 3 +- buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 22 ++++++- .../src/WaveBodyInteractions/CMakeLists.txt | 1 + .../WaveBodyInteractions.cpp | 30 +++++++++- 6 files changed, 145 insertions(+), 11 deletions(-) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 40af76a5..ce41df3a 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -17,6 +17,8 @@ #include +#include + #include #include #include @@ -63,6 +65,7 @@ struct IncWaveHeights { // shortcut different sizes as not equal bool equal = (this->points.size() == that.points.size()); + equal &= this->valid == that.valid; if (!equal) { return false; } @@ -73,8 +76,6 @@ struct IncWaveHeights equal &= (this->points[idx] == that.points[idx]); } - equal &= this->valid == that.valid; - return equal; } }; @@ -91,9 +92,9 @@ struct AirSpring bool operator==(const AirSpring & that) const { bool equal = (this->valid == that.valid); - equal &= (this->force == that.force); - equal &= (this->T == that.T); - equal &= (this->dQ_dt == that.dQ_dt); + equal &= fabs(this->force - that.force) < 1e-7F; + equal &= fabs(this->T - that.T) < 1e-7F; + equal &= fabs(this->dQ_dt - that.dQ_dt) < 1e-7F; return equal; } @@ -109,6 +110,44 @@ struct ElectroHydraulic double motor_drive_friction_loss{0.0}; double motor_drive_switching_loss{0.0}; double battery_i2r_loss{0.0}; + + bool operator==(const ElectroHydraulic & that) const + { + bool equal = (this->valid == that.valid); + equal &= fabs(this->inst_power - that.inst_power) < 1e-7F; + equal &= fabs(this->force - that.force) < 1e-7F; + equal &= fabs(this->motor_drive_i2r_loss - that.motor_drive_i2r_loss) < 1e-7F; + equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; + equal &= fabs(this->motor_drive_switching_loss - that.motor_drive_switching_loss) < 1e-7F; + equal &= fabs(this->battery_i2r_loss - that.battery_i2r_loss) < 1e-7F; + + return equal; + } +}; + +struct WaveBody +{ + bool valid{false}; + + gz::math::Vector3d buoyant_force{0.0, 0.0, 0.0}; + gz::math::Vector3d buoyant_moment{0.0, 0.0, 0.0}; + gz::math::Vector3d radiation_force{0.0, 0.0, 0.0}; + gz::math::Vector3d radiation_moment{0.0, 0.0, 0.0}; + gz::math::Vector3d exciting_force{0.0, 0.0, 0.0}; + gz::math::Vector3d exciting_moment{0.0, 0.0, 0.0}; + + bool operator==(const WaveBody & that) const + { + bool equal = (this->valid == that.valid); + equal &= (this->buoyant_force == that.buoyant_force); + equal &= (this->buoyant_moment == that.buoyant_moment); + equal &= (this->radiation_force == that.radiation_force); + equal &= (this->radiation_moment == that.radiation_moment); + equal &= (this->exciting_force == that.exciting_force); + equal &= (this->exciting_moment == that.exciting_moment); + + return equal; + } }; /// \brief latent data that is modeled but not directly observed for LatentData message in ROS 2 @@ -118,12 +157,17 @@ struct LatentData AirSpring upper_spring; AirSpring lower_spring; ElectroHydraulic electro_hydraulic; + WaveBody wave_body; + + double piston_friction_force_valid; + double piston_friction_force; // Newtons bool valid() const { return inc_wave_heights.valid && \ upper_spring.valid && lower_spring.valid && \ - electro_hydraulic.valid; + electro_hydraulic.valid && wave_body.valid && \ + piston_friction_force_valid; } bool operator==(const LatentData & that) const @@ -131,7 +175,9 @@ struct LatentData bool equal = (this->inc_wave_heights == that.inc_wave_heights); equal &= (this->upper_spring == that.upper_spring); equal &= (this->lower_spring == that.lower_spring); - // equal &= fabs(this-> - that.) < 1e-7F; + equal &= (this->electro_hydraulic == that.electro_hydraulic); + equal &= (this->wave_body == that.wave_body); + equal &= fabs(this->piston_friction_force - that.piston_friction_force) < 1e-7F; return equal; } }; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index b55b7524..1210da57 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -273,6 +273,46 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.electro_hydraulic.battery_i2r_loss = latent_data.electro_hydraulic.battery_i2r_loss; + this->dataPtr->latent_data_.wave_body.buoyancy.force.x = + latent_data.wave_body.buoyant_force.X(); + this->dataPtr->latent_data_.wave_body.buoyancy.force.y = + latent_data.wave_body.buoyant_force.Y(); + this->dataPtr->latent_data_.wave_body.buoyancy.force.z = + latent_data.wave_body.buoyant_force.Z(); + this->dataPtr->latent_data_.wave_body.buoyancy.torque.x = + latent_data.wave_body.buoyant_moment.X(); + this->dataPtr->latent_data_.wave_body.buoyancy.torque.y = + latent_data.wave_body.buoyant_moment.Y(); + this->dataPtr->latent_data_.wave_body.buoyancy.torque.z = + latent_data.wave_body.buoyant_moment.Z(); + this->dataPtr->latent_data_.wave_body.radiation.force.x = + latent_data.wave_body.radiation_force.X(); + this->dataPtr->latent_data_.wave_body.radiation.force.y = + latent_data.wave_body.radiation_force.Y(); + this->dataPtr->latent_data_.wave_body.radiation.force.z = + latent_data.wave_body.radiation_force.Z(); + this->dataPtr->latent_data_.wave_body.radiation.torque.x = + latent_data.wave_body.radiation_moment.X(); + this->dataPtr->latent_data_.wave_body.radiation.torque.y = + latent_data.wave_body.radiation_moment.Y(); + this->dataPtr->latent_data_.wave_body.radiation.torque.z = + latent_data.wave_body.radiation_moment.Z(); + this->dataPtr->latent_data_.wave_body.excitation.force.x = + latent_data.wave_body.exciting_force.X(); + this->dataPtr->latent_data_.wave_body.excitation.force.y = + latent_data.wave_body.exciting_force.Y(); + this->dataPtr->latent_data_.wave_body.excitation.force.z = + latent_data.wave_body.exciting_force.Z(); + this->dataPtr->latent_data_.wave_body.excitation.torque.x = + latent_data.wave_body.exciting_moment.X(); + this->dataPtr->latent_data_.wave_body.excitation.torque.y = + latent_data.wave_body.exciting_moment.Y(); + this->dataPtr->latent_data_.wave_body.excitation.torque.z = + latent_data.wave_body.exciting_moment.Z(); + + this->dataPtr->latent_data_.piston_friction_force = + latent_data.piston_friction_force; + // TODO(andermi) fill in other stuff this->dataPtr->data_valid_ = latent_data.valid(); diff --git a/buoy_gazebo/src/PTOFriction/CMakeLists.txt b/buoy_gazebo/src/PTOFriction/CMakeLists.txt index 356dc639..4eefe1b2 100644 --- a/buoy_gazebo/src/PTOFriction/CMakeLists.txt +++ b/buoy_gazebo/src/PTOFriction/CMakeLists.txt @@ -3,6 +3,7 @@ gz_add_plugin(PTOFriction PTOFriction.cpp INCLUDE_DIRS .. + ../LatentData EXTRA_ROS_PKGS simple_interp -) \ No newline at end of file +) diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index 2327b6d0..f63c4064 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -30,6 +30,8 @@ #include +#include + namespace buoy_gazebo { @@ -48,7 +50,7 @@ class PTOFrictionPrivate /// \brief Piston mean friction force const std::vector meanFriction; // N - /// \brief Construct and approximation of friction model using linear spline + /// \brief Construct and approximation of friction model using linear interpolation simple_interp::Interp1d pto_friction_model; PTOFrictionPrivate() @@ -147,6 +149,24 @@ void PTOFriction::PreUpdate( } else { forceComp->Data()[0] += friction_force; // Add friction to existing forces } + + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->model.Entity()); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + + latent_data.piston_friction_force_valid = true; + latent_data.piston_friction_force = friction_force; + + _ecm.SetComponentData( + this->dataPtr->model.Entity(), + latent_data); } } // namespace buoy_gazebo diff --git a/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt b/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt index 4c03dfc1..d2b9454d 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt +++ b/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt @@ -14,6 +14,7 @@ gz_add_plugin(WaveBodyInteractions INCLUDE_DIRS .. ../.. + ../LatentData EXTRA_ROS_PKGS simple_interp ament_index_cpp diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index 9ffa22b1..154f9239 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include #include @@ -43,13 +43,16 @@ #include #include -#include "IncidentWaves/IncWaveState.hpp" +#include +#include #include #include #include + + namespace buoy_gazebo { class WaveBodyInteractionsPrivate @@ -340,6 +343,29 @@ void WaveBodyInteractions::PreUpdate( _ecm.SetComponentData( this->dataPtr->IncWaveEntity, inc_wave_state); } + + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->model.Entity()); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + + latent_data.wave_body.valid = true; + latent_data.wave_body.buoyant_force = w_FBp; + latent_data.wave_body.buoyant_moment = w_MBp; + latent_data.wave_body.radiation_force = w_FRp; + latent_data.wave_body.radiation_moment = w_MRp; + latent_data.wave_body.exciting_force = w_FEp; + latent_data.wave_body.exciting_moment = w_MEp; + + _ecm.SetComponentData( + this->dataPtr->model.Entity(), + latent_data); } ////////////////////////////////////////////////// From fd00588bee9ce2a780de7adda74c78665bbb226d Mon Sep 17 00:00:00 2001 From: andermi Date: Tue, 5 Sep 2023 21:24:19 -0700 Subject: [PATCH 14/26] Debug flag for WaveBodyInteractions plugin (#160) --- .../models/mbari_wec/model.sdf.em | 3 +- .../ElectroHydraulicPTO.cpp | 4 +- .../WaveBodyInteractions.cpp | 67 ++++++++++--------- 3 files changed, 41 insertions(+), 33 deletions(-) diff --git a/buoy_description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf.em index 12b07c27..862fab1d 100644 --- a/buoy_description/models/mbari_wec/model.sdf.em +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -265,8 +265,9 @@ if not ignore_piston_mean_pos: @(print(f'{P0_l:.00f}', end='')) - + Buoy + false 0 0 2.46 diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 8bc352ee..590c1346 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -360,7 +360,7 @@ void ElectroHydraulicPTO::PreUpdate( if (i_try > 0) { std::stringstream warning; warning << "Warning: Reduced piston to achieve convergence" << std::endl; - igndbg << warning.str(); + gzdbg << warning.str(); } if (solver_info != 1) { @@ -369,7 +369,7 @@ void ElectroHydraulicPTO::PreUpdate( warning << "Warning: Numericals solver in ElectroHydraulicPTO did not converge" << std::endl; warning << "solver info: [" << solver_info << "]" << std::endl; warning << "=================================" << std::endl; - igndbg << warning.str(); + gzdbg << warning.str(); } // Solve Electrical diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index 154f9239..c86986b1 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -55,6 +55,11 @@ namespace buoy_gazebo { + +static bool WBI_DBG_FLAG{false}; +#define wbidbg if (WBI_DBG_FLAG) gzdbg + + class WaveBodyInteractionsPrivate { public: @@ -101,7 +106,7 @@ void WaveBodyInteractions::Configure( { this->dataPtr->model = gz::sim::Model(_entity); if (!this->dataPtr->model.Valid(_ecm)) { - ignerr + gzerr << "WaveBodyInteractions plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; @@ -109,16 +114,18 @@ void WaveBodyInteractions::Configure( // Get params from SDF. if (!_sdf->HasElement("LinkName")) { - ignerr << "You musk specify a for the wavebodyinteraction " + gzerr << "You musk specify a for the wavebodyinteraction " "plugin to act upon" - << "Failed to initialize." << std::endl; + << "Failed to initialize." << std::endl; return; } auto linkName = _sdf->Get("LinkName"); + WBI_DBG_FLAG = _sdf->Get("debug", false).first; + this->dataPtr->linkEntity = this->dataPtr->model.LinkByName(_ecm, linkName); if (!_ecm.HasEntity(this->dataPtr->linkEntity)) { - ignerr << "Link name" << linkName << "does not exist"; + gzerr << "Link name" << linkName << "does not exist"; return; } @@ -166,7 +173,7 @@ void WaveBodyInteractions::Configure( ament_index_cpp::get_package_share_directory(package_share_dir) + base_filenm; } else { - ignerr << "Only package: URI scheme has been implemented" << std::endl; + gzerr << "Only package: URI scheme has been implemented" << std::endl; return; } @@ -189,12 +196,12 @@ void WaveBodyInteractions::PreUpdate( if (_info.iterations == 1) { // First iteration, set timestep size. double dt = std::chrono::duration(_info.dt).count(); dataPtr->FloatingBody.SetTimestepSize(dt); - gzdbg << " Set Wave Forcing timestep size: dt = " << dt << std::endl; + wbidbg << " Set Wave Forcing timestep size: dt = " << dt << std::endl; } // \TODO(anyone): Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { - ignwarn + gzwarn << "Detected jump back in time [" << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; @@ -219,7 +226,7 @@ void WaveBodyInteractions::PreUpdate( gz::sim::Link baseLink(this->dataPtr->linkEntity); - gzdbg << "baseLink.Name = " << baseLink.Name(_ecm).value() << std::endl; + wbidbg << "baseLink.Name = " << baseLink.Name(_ecm).value() << std::endl; auto w_xddot_b = baseLink.WorldLinearAcceleration(_ecm).value(); auto w_omegadot_b = baseLink.WorldAngularAcceleration(_ecm).value(); @@ -228,8 +235,8 @@ void WaveBodyInteractions::PreUpdate( auto w_Pose_b = gz::sim::worldPose(this->dataPtr->linkEntity, _ecm); auto w_Pose_p = w_Pose_b * this->dataPtr->b_Pose_p; - gzdbg << "w_Pose_b = " << w_Pose_b << std::endl; - gzdbg << "w_Pose_p = " << w_Pose_p << std::endl; + wbidbg << "w_Pose_b = " << w_Pose_b << std::endl; + wbidbg << "w_Pose_p = " << w_Pose_p << std::endl; // gz::math::Vector3 p_xdot = w_Pose_p.Rot().Inverse() * *w_xdot; gz::math::Vector3 w_xdot_p = @@ -237,10 +244,10 @@ void WaveBodyInteractions::PreUpdate( w_omega_b.Cross(w_Pose_b.Rot() * this->dataPtr->b_Pose_p.Pos()); gz::math::Vector3 w_omega_p = w_omega_b; // Waterplane Coord sys is parallel to body C.S. - gzdbg << "w_xdot_b = " << w_xdot_b << std::endl; - gzdbg << "w_xdot_p = " << w_xdot_p << std::endl; - gzdbg << "w_omega_b = " << w_omega_b << std::endl; - gzdbg << "w_omega_p = " << w_omega_p << std::endl; + wbidbg << "w_xdot_b = " << w_xdot_b << std::endl; + wbidbg << "w_xdot_p = " << w_xdot_p << std::endl; + wbidbg << "w_omega_b = " << w_omega_b << std::endl; + wbidbg << "w_omega_p = " << w_omega_p << std::endl; gz::math::Vector3 w_xddot_p = w_xddot_b + @@ -249,10 +256,10 @@ void WaveBodyInteractions::PreUpdate( w_omega_b.Cross(w_Pose_b.Rot() * this->dataPtr->b_Pose_p.Pos())); gz::math::Vector3 w_omegadot_p = w_omegadot_b; // Waterplane Coord sys is parallel to body C.S. - gzdbg << "w_xddot_b = " << w_xddot_b << std::endl; - gzdbg << "w_xddot_p = " << w_xddot_p << std::endl; - gzdbg << "w_omegadot_b = " << w_omegadot_b << std::endl; - gzdbg << "w_omegadot_p = " << w_omegadot_p << std::endl; + wbidbg << "w_xddot_b = " << w_xddot_b << std::endl; + wbidbg << "w_xddot_p = " << w_xddot_p << std::endl; + wbidbg << "w_omegadot_b = " << w_omegadot_b << std::endl; + wbidbg << "w_omegadot_p = " << w_omegadot_p << std::endl; gz::math::Vector3 b_xddot_p = w_Pose_p.Rot().Inverse() * w_xddot_p; gz::math::Vector3 b_omegadot_p = w_Pose_p.Rot().Inverse() * w_omegadot_p; @@ -263,11 +270,11 @@ void WaveBodyInteractions::PreUpdate( Eigen::VectorXd x(6); x << w_Pose_p.X(), w_Pose_p.Y(), w_Pose_p.Z(), w_Pose_p.Roll(), w_Pose_p.Pitch(), w_Pose_p.Yaw(); - gzdbg << "x(6) = " << x.transpose() << std::endl; + wbidbg << "x(6) = " << x.transpose() << std::endl; Eigen::VectorXd BuoyancyForce(6); BuoyancyForce = this->dataPtr->FloatingBody.BuoyancyForce(x); - gzdbg << "Buoyancy Force at waterplane = " << BuoyancyForce.transpose() - << std::endl; + wbidbg << "Buoyancy Force at waterplane = " << BuoyancyForce.transpose() + << std::endl; // Compute Buoyancy Force gz::math::Vector3d w_FBp(BuoyancyForce(0), BuoyancyForce(1), @@ -280,7 +287,7 @@ void WaveBodyInteractions::PreUpdate( // Add contribution due to force offset from origin w_MBp += (w_Pose_b.Rot().RotateVector(this->dataPtr->b_Pose_p.Pos())).Cross(w_FBp); - gzdbg << "Buoyancy: applied moment = " << w_MBp << std::endl; + wbidbg << "Buoyancy: applied moment = " << w_MBp << std::endl; // Compute Memory part of Radiation Force based on accelerations in the body // frame, result is in body frame @@ -295,14 +302,14 @@ void WaveBodyInteractions::PreUpdate( // xddot << 0.0, 0.0, 0.0, b_omegadot_p.X(), b_omegadot_p.Y(), // b_omegadot_p.Z(); - gzdbg << "xddot = " << xddot.transpose() << std::endl; - gzdbg << "xddot = " << xddot.transpose() << std::endl; + wbidbg << "xddot = " << xddot.transpose() << std::endl; + wbidbg << "xddot = " << xddot.transpose() << std::endl; Eigen::VectorXd MemForce(6); // Note negative sign, FS_Hydrodynamics returns force required to move body in // prescribed way, // force of water on body is opposite. MemForce = -this->dataPtr->FloatingBody.RadiationForce(xddot); - gzdbg << " MemForce at Waterplane = " << MemForce.transpose() << std::endl; + wbidbg << " MemForce at Waterplane = " << MemForce.transpose() << std::endl; gz::math::Vector3d w_FRp(MemForce(0), MemForce(1), MemForce(2)); // Needs to be adjusted for yaw only becaues of small pitch/roll assumption gz::math::Vector3d w_MRp( @@ -313,13 +320,13 @@ void WaveBodyInteractions::PreUpdate( // Add contribution due to force offset from origin w_MRp += (w_Pose_b.Rot().RotateVector(this->dataPtr->b_Pose_p.Pos())).Cross(w_FRp); - gzdbg << "Radiation: applied moment = " << w_MRp << std::endl; + wbidbg << "Radiation: applied moment = " << w_MRp << std::endl; - gzdbg << std::endl; + wbidbg << std::endl; // Compute Wave Exciting Force Eigen::VectorXd ExtForce(6); ExtForce = this->dataPtr->FloatingBody.ExcitingForce(); - gzdbg << "Exciting Force = " << ExtForce.transpose() << std::endl; + wbidbg << "Exciting Force = " << ExtForce.transpose() << std::endl; gz::math::Vector3d w_FEp(ExtForce(0), ExtForce(1), ExtForce(2)); // Needs to be adjusted for yaw only gz::math::Vector3d w_MEp( @@ -327,8 +334,8 @@ void WaveBodyInteractions::PreUpdate( 1 * (sin(x(5)) * ExtForce(3) + cos(x(5)) * ExtForce(4)), ExtForce(5)); // Needs to be adjusted for yaw only - gzdbg << "Exciting: applied force = " << w_FEp << std::endl; - gzdbg << "Exciting: applied moment = " << w_MEp << std::endl; + wbidbg << "Exciting: applied force = " << w_FEp << std::endl; + wbidbg << "Exciting: applied moment = " << w_MEp << std::endl; // Add contribution due to force offset from origin w_MEp += From a4300d32dffd9fee657c51a77ab785989a36bebc Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 6 Sep 2023 23:31:58 -0700 Subject: [PATCH 15/26] temporarily checkout andermi/latent_data in mbari_wec_utils for CI Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 9d10afc4..96effdee 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -34,6 +34,10 @@ cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC wget https://raw.githubusercontent.com/osrf/mbari_wec/main/mbari_wec_all.yaml vcs import --skip-existing < mbari_wec_all.yaml +cd mbari_wec_utils +git checkout andermi/latent_data +cd .. + rosdep init rosdep update rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO From f85f14b36bdbd877b50c9ec53b61d402465c635e Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 6 Sep 2023 23:32:15 -0700 Subject: [PATCH 16/26] linters Signed-off-by: Michael Anderson --- buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index c86986b1..a9b6eb0b 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -52,7 +52,6 @@ #include - namespace buoy_gazebo { From dadcc18ab9151d2aa593c10b9ad1a2dc84a64e21 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Sun, 10 Sep 2023 12:51:33 -0700 Subject: [PATCH 17/26] add gas mass and eff_v/m; comment units; use buoy_utils/constants.h in spring Signed-off-by: Michael Anderson --- .../src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp | 5 +++++ buoy_gazebo/src/LatentData/LatentData/LatentData.hpp | 10 ++++++---- .../src/LatentData/LatentData/LatentDataPublisher.cpp | 6 ++++++ .../PolytropicPneumaticSpring.cpp | 8 ++++++-- buoy_gazebo/src/buoy_utils/Constants.hpp | 1 + 5 files changed, 24 insertions(+), 6 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 590c1346..97bb8bc6 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -376,6 +376,9 @@ void ElectroHydraulicPTO::PreUpdate( const double N = this->dataPtr->x[0U]; double deltaP = this->dataPtr->x[1U]; double VBus = this->dataPtr->x[2U]; + const double eff_m = this->hyd_eff_m.eval(fabs(N)); + const double eff_v = this->hyd_eff_v.eval(fabs(deltaP)); + VBus = std::min(VBus, this->dataPtr->MaxTargetVoltage); double BusPower = this->dataPtr->functor.BusPower; @@ -436,6 +439,8 @@ void ElectroHydraulicPTO::PreUpdate( latent_data.electro_hydraulic.motor_drive_switching_loss = pto_loss.motor_drive_switching_loss; latent_data.electro_hydraulic.motor_drive_friction_loss = pto_loss.motor_drive_friction_loss; latent_data.electro_hydraulic.battery_i2r_loss = pto_loss.battery_i2r_loss; + latent_data.electro_hydraulic.eff_m = eff_m; + latent_data.electro_hydraulic.eff_v = eff_v; _ecm.SetComponentData( this->dataPtr->PrismaticJointEntity, diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index f8fd3cec..0a579158 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -106,10 +106,12 @@ struct ElectroHydraulic double inst_power{0.0}; // Watts double rpm{0.0}; double force{0.0}; // Newtons - double motor_drive_i2r_loss{0.0}; - double motor_drive_friction_loss{0.0}; - double motor_drive_switching_loss{0.0}; - double battery_i2r_loss{0.0}; + double motor_drive_i2r_loss{0.0}; // Watts + double motor_drive_friction_loss{0.0}; // Watts + double motor_drive_switching_loss{0.0}; // Watts + double battery_i2r_loss{0.0}; // Watts + double eff_m{0.0}; // mechanical efficiency + double eff_v{0.0}; // volumetric efficiency bool operator==(const ElectroHydraulic & that) const { diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 49705430..64984200 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -279,6 +279,12 @@ void LatentDataPublisher::PostUpdate( latent_data.electro_hydraulic.motor_drive_friction_loss; this->dataPtr->latent_data_.electro_hydraulic.battery_i2r_loss = latent_data.electro_hydraulic.battery_i2r_loss; + this->dataPtr->latent_data_.electro_hydraulic.eff_m = + latent_data.electro_hydraulic.eff_m; + this->dataPtr->latent_data_.electro_hydraulic.eff_v = + latent_data.electro_hydraulic.eff_v; + + this->dataPtr->latent_data_.wave_body.buoyancy.force.x = latent_data.wave_body.buoyant_force.X(); diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 5f392ffe..0f995e86 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -40,6 +40,8 @@ #include "SpringState.hpp" #include +#include + using namespace std::chrono_literals; @@ -528,8 +530,8 @@ void PolytropicPneumaticSpring::PreUpdate( const int dt_nano = static_cast(std::chrono::duration_cast(_info.dt).count()); - static const double PASCAL_TO_PSI = 1.450377e-4; // PSI/Pascal - const double pressure_diff = (spring_state.lower_psi - spring_state.upper_psi) / PASCAL_TO_PSI; + const double pressure_diff = + (spring_state.lower_psi - spring_state.upper_psi) * buoy_utils::PASCAL_PER_PSI; if (spring_state.valve_command) { openValve( @@ -597,6 +599,7 @@ void PolytropicPneumaticSpring::PreUpdate( latent_data.upper_spring.dQ_dt = this->dataPtr->Q_rate; latent_data.upper_spring.piston_position = x; latent_data.upper_spring.piston_velocity = v; + latent_data.upper_spring.mass = this->dataPtr->mass; } else { latent_data.lower_spring.valid = true; latent_data.lower_spring.force = this->dataPtr->F; @@ -604,6 +607,7 @@ void PolytropicPneumaticSpring::PreUpdate( latent_data.lower_spring.dQ_dt = this->dataPtr->Q_rate; latent_data.lower_spring.piston_position = this->dataPtr->config_->stroke - x; latent_data.lower_spring.piston_velocity = -v; + latent_data.lower_spring.mass = this->dataPtr->mass; } _ecm.SetComponentData( diff --git a/buoy_gazebo/src/buoy_utils/Constants.hpp b/buoy_gazebo/src/buoy_utils/Constants.hpp index 68810e92..49d5e21d 100644 --- a/buoy_gazebo/src/buoy_utils/Constants.hpp +++ b/buoy_gazebo/src/buoy_utils/Constants.hpp @@ -27,6 +27,7 @@ static constexpr double NM_PER_INLB{0.112984829}; static constexpr double INLB_PER_NM{8.851}; static constexpr double INCHES_PER_METER{39.4}; static constexpr double NEWTONS_PER_LB{4.4482}; +static constexpr double PASCAL_PER_PSI = 6894.757; // Pascal/PSI } // namespace buoy_utils From 2f0f097e97480debed1a18a688872c5cb24fe408 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Sun, 10 Sep 2023 13:27:21 -0700 Subject: [PATCH 18/26] fix last push Signed-off-by: Michael Anderson --- buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp | 4 ++-- buoy_gazebo/src/LatentData/LatentData/LatentData.hpp | 1 + .../PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 97bb8bc6..7fcdc6ca 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -376,8 +376,8 @@ void ElectroHydraulicPTO::PreUpdate( const double N = this->dataPtr->x[0U]; double deltaP = this->dataPtr->x[1U]; double VBus = this->dataPtr->x[2U]; - const double eff_m = this->hyd_eff_m.eval(fabs(N)); - const double eff_v = this->hyd_eff_v.eval(fabs(deltaP)); + const double eff_m = this->dataPtr->functor.hyd_eff_m.eval(fabs(N)); + const double eff_v = this->dataPtr->functor.hyd_eff_v.eval(fabs(deltaP)); VBus = std::min(VBus, this->dataPtr->MaxTargetVoltage); double BusPower = this->dataPtr->functor.BusPower; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 0a579158..5cb93f5f 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -88,6 +88,7 @@ struct AirSpring double dQ_dt{0.0}; // heat loss rate double piston_position{0.0}; // meters double piston_velocity{0.0}; // m/s + double mass{0.0}; // kg bool operator==(const AirSpring & that) const { diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 0f995e86..bdbb5e5f 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -572,9 +572,9 @@ void PolytropicPneumaticSpring::PreUpdate( if (this->dataPtr->config_->is_upper) { spring_state.range_finder = x; - spring_state.upper_psi = PASCAL_TO_PSI * this->dataPtr->P; + spring_state.upper_psi = this->dataPtr->P / buoy_utils::PASCAL_PER_PSI; } else { - spring_state.lower_psi = PASCAL_TO_PSI * this->dataPtr->P; + spring_state.lower_psi = this->dataPtr->P / buoy_utils::PASCAL_PER_PSI; } _ecm.SetComponentData( From cfd1e1af9c555f459c104883705134ce2d0efc1c Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Sun, 10 Sep 2023 14:03:48 -0700 Subject: [PATCH 19/26] fix latent_data pub Signed-off-by: Michael Anderson --- .../src/LatentData/IncWaveHeight/IncWaveHeight.cpp | 2 ++ buoy_gazebo/src/LatentData/LatentData/LatentData.hpp | 11 +++++++++++ .../src/LatentData/LatentData/LatentDataPublisher.cpp | 2 -- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp index 97751fd7..db03b76e 100644 --- a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp @@ -334,6 +334,8 @@ void IncWaveHeight::PreUpdate( latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); } + latent_data.inc_wave_heights.valid = this->dataPtr->inc_wave_valid_; + double SimTime = std::chrono::duration(this->dataPtr->current_time_).count(); auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); // all fixed points from SDF computed at SimTime (relative_time = 0.0) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 5cb93f5f..b4ee0502 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -96,6 +96,9 @@ struct AirSpring equal &= fabs(this->force - that.force) < 1e-7F; equal &= fabs(this->T - that.T) < 1e-7F; equal &= fabs(this->dQ_dt - that.dQ_dt) < 1e-7F; + equal &= fabs(this->piston_position - that.piston_position) < 1e-7F; + equal &= fabs(this->piston_velocity - that.piston_velocity) < 1e-7F; + equal &= fabs(this->mass - that.mass) < 1e-7F; return equal; } @@ -123,6 +126,8 @@ struct ElectroHydraulic equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; equal &= fabs(this->motor_drive_switching_loss - that.motor_drive_switching_loss) < 1e-7F; equal &= fabs(this->battery_i2r_loss - that.battery_i2r_loss) < 1e-7F; + equal &= fabs(this->eff_m - that.eff_m) < 1e-7F; + equal &= fabs(this->eff_v - that.eff_v) < 1e-7F; return equal; } @@ -167,6 +172,11 @@ struct LatentData bool valid() const { + /* std::cout << "iwh[" << inc_wave_heights.valid << "] :: " + << "us[" << upper_spring.valid << "] :: " + << "ls[" << lower_spring.valid << "] :: " + << "wb[" << wave_body.valid << "] :: " + << "friction[" << piston_friction_force_valid << "]" << std::endl; */ return inc_wave_heights.valid && \ upper_spring.valid && lower_spring.valid && \ electro_hydraulic.valid && wave_body.valid && \ @@ -181,6 +191,7 @@ struct LatentData equal &= (this->electro_hydraulic == that.electro_hydraulic); equal &= (this->wave_body == that.wave_body); equal &= fabs(this->piston_friction_force - that.piston_friction_force) < 1e-7F; + equal &= (this->piston_friction_force_valid == that.piston_friction_force_valid); return equal; } }; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 64984200..6cea5a58 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -284,8 +284,6 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.electro_hydraulic.eff_v = latent_data.electro_hydraulic.eff_v; - - this->dataPtr->latent_data_.wave_body.buoyancy.force.x = latent_data.wave_body.buoyant_force.X(); this->dataPtr->latent_data_.wave_body.buoyancy.force.y = From 1acea31ca8bfe06ce4df2a90eebe5c144a1cb0b0 Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Wed, 13 Sep 2023 08:37:45 -0700 Subject: [PATCH 20/26] Added Shaft Mechanical Power to Latent Data --- .../ElectroHydraulicLoss.hpp | 63 ------------------- .../ElectroHydraulicPTO.cpp | 39 +++--------- .../ElectroHydraulicSoln.hpp | 3 +- .../src/LatentData/LatentData/LatentData.hpp | 2 + .../LatentData/LatentDataPublisher.cpp | 2 + 5 files changed, 16 insertions(+), 93 deletions(-) delete mode 100644 buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicLoss.hpp diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicLoss.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicLoss.hpp deleted file mode 100644 index 1dffc02d..00000000 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicLoss.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ELECTROHYDRAULICPTO__ELECTROHYDRAULICLOSS_HPP_ -#define ELECTROHYDRAULICPTO__ELECTROHYDRAULICLOSS_HPP_ - - -#include -#include -#include -#include - - -namespace buoy_gazebo -{ - -/// \brief State data for power commands and feedback from sensors for PCRecord message in ROS2 -struct ElectroHydraulicLoss -{ - float hydraulic_motor_loss{0.0F}; - float relief_valve_loss{0.0F}; - float motor_drive_i2r_loss{0.0F}; - float motor_drive_switching_loss{0.0F}; - float motor_drive_friction_loss{0.0F}; - float battery_i2r_loss{0.0F}; - - bool operator==(const ElectroHydraulicLoss & that) const - { - bool equal = fabs(this->hydraulic_motor_loss - that.hydraulic_motor_loss) < 1e-7F; - equal &= fabs(this->relief_valve_loss - that.relief_valve_loss) < 1e-7F; - equal &= fabs(this->motor_drive_i2r_loss - that.motor_drive_i2r_loss) < 1e-7F; - equal &= fabs(this->motor_drive_switching_loss - that.motor_drive_switching_loss) < 1e-7F; - equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; - equal &= fabs(this->battery_i2r_loss - that.battery_i2r_loss) < 1e-7F; - return equal; - } -}; - -namespace components -{ -/// \brief State data as component for reporting via ROS2 -using ElectroHydraulicLoss = - gz::sim::components::Component; -GZ_SIM_REGISTER_COMPONENT( - "buoy_gazebo.components.ElectroHydraulicLoss", - ElectroHydraulicLoss) -} // namespace components - -} // namespace buoy_gazebo - -#endif // ELECTROHYDRAULICPTO__ELECTROHYDRAULICLOSS_HPP_ diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 7fcdc6ca..a63169c9 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -44,7 +44,6 @@ #include "ElectroHydraulicSoln.hpp" #include "ElectroHydraulicState.hpp" -#include "ElectroHydraulicLoss.hpp" #include "BatteryState.hpp" #include @@ -265,18 +264,6 @@ void ElectroHydraulicPTO::PreUpdate( pto_state = buoy_gazebo::ElectroHydraulicState(pto_state_comp->Data()); } - buoy_gazebo::ElectroHydraulicLoss pto_loss; - if (_ecm.EntityHasComponentType( - this->dataPtr->PrismaticJointEntity, - buoy_gazebo::components::ElectroHydraulicLoss().TypeId())) - { - auto pto_loss_comp = - _ecm.Component( - this->dataPtr->PrismaticJointEntity); - - pto_loss = buoy_gazebo::ElectroHydraulicLoss(pto_loss_comp->Data()); - } - buoy_gazebo::LatentData latent_data; if (_ecm.EntityHasComponentType( this->dataPtr->model.Entity(), @@ -421,24 +408,22 @@ void ElectroHydraulicPTO::PreUpdate( pto_state.target_a = this->dataPtr->functor.I_Wind.I; - pto_loss.hydraulic_motor_loss += 1.0; - pto_loss.relief_valve_loss += 2.0; - pto_loss.motor_drive_i2r_loss = - this->dataPtr->functor.MotorDriveISquaredRLoss(this->dataPtr->functor.I_Wind.I); - pto_loss.motor_drive_switching_loss = this->dataPtr->functor.MotorDriveSwitchingLoss(VBus); - pto_loss.motor_drive_friction_loss = this->dataPtr->functor.MotorDriveFrictionLoss(N); - pto_loss.battery_i2r_loss = I_Batt * I_Batt * this->dataPtr->Ri; - double piston_force = -deltaP * this->dataPtr->PistonArea * buoy_utils::NEWTONS_PER_LB; latent_data.electro_hydraulic.valid = true; latent_data.electro_hydraulic.inst_power = VBus * (I_Batt + I_Load); + latent_data.electro_hydraulic.shaft_mech_power = + this->dataPtr->functor.ShaftMechPower; latent_data.electro_hydraulic.rpm = N; latent_data.electro_hydraulic.force = piston_force; - latent_data.electro_hydraulic.motor_drive_i2r_loss = pto_loss.motor_drive_i2r_loss; - latent_data.electro_hydraulic.motor_drive_switching_loss = pto_loss.motor_drive_switching_loss; - latent_data.electro_hydraulic.motor_drive_friction_loss = pto_loss.motor_drive_friction_loss; - latent_data.electro_hydraulic.battery_i2r_loss = pto_loss.battery_i2r_loss; + latent_data.electro_hydraulic.motor_drive_i2r_loss = + this->dataPtr->functor.MotorDriveISquaredRLoss(this->dataPtr->functor.I_Wind.I); + latent_data.electro_hydraulic.motor_drive_switching_loss = + this->dataPtr->functor.MotorDriveSwitchingLoss(VBus); + latent_data.electro_hydraulic.motor_drive_friction_loss = + this->dataPtr->functor.MotorDriveFrictionLoss(N); + latent_data.electro_hydraulic.battery_i2r_loss = + I_Batt * I_Batt * this->dataPtr->Ri; latent_data.electro_hydraulic.eff_m = eff_m; latent_data.electro_hydraulic.eff_v = eff_v; @@ -452,10 +437,6 @@ void ElectroHydraulicPTO::PreUpdate( auto stampMsg = gz::sim::convert(_info.simTime); - _ecm.SetComponentData( - this->dataPtr->PrismaticJointEntity, - pto_loss); - _ecm.SetComponentData( this->dataPtr->model.Entity(), latent_data); diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index dde5ef4d..063e32fc 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -110,6 +110,7 @@ struct ElectroHydraulicSoln : Functor mutable double BusPower; double Q; + mutable double ShaftMechPower; private: static const std::vector Peff; // psi static const std::vector Neff; // rpm @@ -170,7 +171,7 @@ struct ElectroHydraulicSoln : Functor const double T_applied = 1.375 * this->I_Wind.TorqueConstantInLbPerAmp * WindCurr; - double ShaftMechPower = T_applied * buoy_utils::NM_PER_INLB * + ShaftMechPower = T_applied * buoy_utils::NM_PER_INLB * x[0U] * buoy_utils::RPM_TO_RAD_PER_SEC; BusPower = -ShaftMechPower - ( MotorDriveFrictionLoss(x[0U]) + diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index b4ee0502..6c344bd9 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -107,6 +107,7 @@ struct AirSpring struct ElectroHydraulic { bool valid{false}; + double shaft_mech_power{0.0}; // Watts double inst_power{0.0}; // Watts double rpm{0.0}; double force{0.0}; // Newtons @@ -120,6 +121,7 @@ struct ElectroHydraulic bool operator==(const ElectroHydraulic & that) const { bool equal = (this->valid == that.valid); + equal &= fabs(this->shaft_mech_power - that.shaft_mech_power) < 1e-7F; equal &= fabs(this->inst_power - that.inst_power) < 1e-7F; equal &= fabs(this->force - that.force) < 1e-7F; equal &= fabs(this->motor_drive_i2r_loss - that.motor_drive_i2r_loss) < 1e-7F; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 6cea5a58..6e38c075 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -265,6 +265,8 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.lower_spring.piston_velocity = latent_data.lower_spring.piston_velocity; + this->dataPtr->latent_data_.electro_hydraulic.shaft_mech_power = + latent_data.electro_hydraulic.shaft_mech_power; this->dataPtr->latent_data_.electro_hydraulic.inst_power = latent_data.electro_hydraulic.inst_power; this->dataPtr->latent_data_.electro_hydraulic.rpm = From 721ce7e0c3539dca28957794565bfd4d45cc003f Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Wed, 13 Sep 2023 10:02:33 -0700 Subject: [PATCH 21/26] Added additional loss terms to electrohydraulic latent data --- .../ElectroHydraulicPTO.cpp | 10 +++++++--- .../ElectroHydraulicSoln.hpp | 20 ++++++++++++++----- .../src/LatentData/LatentData/LatentData.hpp | 4 ++++ .../LatentData/LatentDataPublisher.cpp | 4 ++++ 4 files changed, 30 insertions(+), 8 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index a63169c9..80865d65 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -417,11 +417,15 @@ void ElectroHydraulicPTO::PreUpdate( latent_data.electro_hydraulic.rpm = N; latent_data.electro_hydraulic.force = piston_force; latent_data.electro_hydraulic.motor_drive_i2r_loss = - this->dataPtr->functor.MotorDriveISquaredRLoss(this->dataPtr->functor.I_Wind.I); + this->dataPtr->functor.I2RLoss; latent_data.electro_hydraulic.motor_drive_switching_loss = - this->dataPtr->functor.MotorDriveSwitchingLoss(VBus); + this->dataPtr->functor.SwitchingLoss; latent_data.electro_hydraulic.motor_drive_friction_loss = - this->dataPtr->functor.MotorDriveFrictionLoss(N); + this->dataPtr->functor.FrictionLoss; + latent_data.electro_hydraulic.relief_valve_loss = + this->dataPtr->functor.ReliefValveLoss; + latent_data.electro_hydraulic.hydraulic_motor_loss = + this->dataPtr->functor.HydraulicMotorLoss; latent_data.electro_hydraulic.battery_i2r_loss = I_Batt * I_Batt * this->dataPtr->Ri; latent_data.electro_hydraulic.eff_m = eff_m; diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index 063e32fc..28d5f0ff 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -111,6 +111,12 @@ struct ElectroHydraulicSoln : Functor double Q; mutable double ShaftMechPower; + mutable double FrictionLoss; + mutable double SwitchingLoss; + mutable double I2RLoss; + mutable double ReliefValveLoss; + mutable double HydraulicMotorLoss; + private: static const std::vector Peff; // psi static const std::vector Neff; // rpm @@ -173,10 +179,10 @@ struct ElectroHydraulicSoln : Functor ShaftMechPower = T_applied * buoy_utils::NM_PER_INLB * x[0U] * buoy_utils::RPM_TO_RAD_PER_SEC; - BusPower = -ShaftMechPower - ( - MotorDriveFrictionLoss(x[0U]) + - MotorDriveSwitchingLoss(x[2U]) + - MotorDriveISquaredRLoss(WindCurr)); + FrictionLoss = MotorDriveFrictionLoss(x[0U]); + SwitchingLoss = MotorDriveSwitchingLoss(x[2U]); + I2RLoss = MotorDriveISquaredRLoss(WindCurr); + BusPower = -ShaftMechPower - (FrictionLoss+SwitchingLoss+I2RLoss); double Q_Relief = 0; if (x[1U] < -PressReliefSetPoint) { // Pressure relief is a one wave valve, // relieves when lower pressure is higher @@ -184,7 +190,8 @@ struct ElectroHydraulicSoln : Functor Q_Relief = (x[1U] + PressReliefSetPoint) * ReliefValveFlowPerPSI * buoy_utils::CubicInchesPerGallon / buoy_utils::SecondsPerMinute; } -// Q_Relief = 0; + ReliefValveLoss = Q_Relief*x[1U]*buoy_utils::INLB_PER_NM; // Result is Watts + double Q_Motor = this->Q - Q_Relief; double Q = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute; double Q_Leak = (1.0 - eff_v) * std::max(fabs(Q_Motor), fabs(Q)) * sgn(x[1]); @@ -192,6 +199,9 @@ struct ElectroHydraulicSoln : Functor double T_Fluid = x[1U] * this->HydMotorDisp / (2.0 * M_PI); double T_Friction = -(1.0 - eff_m) * std::max(fabs(T_applied), fabs(T_Fluid)) * sgn(x[0]); + HydraulicMotorLoss = Q_Leak*x[1U]/buoy_utils::INLB_PER_NM + //Result is Watts + T_Friction*x[0U]/(buoy_utils::INLB_PER_NM*buoy_utils::SecondsPerMinute); + fvec[0U] = Q_Motor - Q_Leak - Q; fvec[1U] = T_applied + T_Friction + T_Fluid; fvec[2U] = BusPower - (x[2U] - VBattEMF) * x[2U] / this->Ri; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 6c344bd9..957f5af7 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -115,6 +115,8 @@ struct ElectroHydraulic double motor_drive_friction_loss{0.0}; // Watts double motor_drive_switching_loss{0.0}; // Watts double battery_i2r_loss{0.0}; // Watts + double relief_valve_loss{0.0}; // Watts + double hydraulic_motor_loss{0.0}; // Watts double eff_m{0.0}; // mechanical efficiency double eff_v{0.0}; // volumetric efficiency @@ -128,6 +130,8 @@ struct ElectroHydraulic equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; equal &= fabs(this->motor_drive_switching_loss - that.motor_drive_switching_loss) < 1e-7F; equal &= fabs(this->battery_i2r_loss - that.battery_i2r_loss) < 1e-7F; + equal &= fabs(this->relief_valve_loss - that.relief_valve_loss) < 1e-7F; + equal &= fabs(this->hydraulic_motor_loss - that.hydraulic_motor_loss) < 1e-7F; equal &= fabs(this->eff_m - that.eff_m) < 1e-7F; equal &= fabs(this->eff_v - that.eff_v) < 1e-7F; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 6e38c075..bfe7d7b8 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -281,6 +281,10 @@ void LatentDataPublisher::PostUpdate( latent_data.electro_hydraulic.motor_drive_friction_loss; this->dataPtr->latent_data_.electro_hydraulic.battery_i2r_loss = latent_data.electro_hydraulic.battery_i2r_loss; + this->dataPtr->latent_data_.electro_hydraulic.relief_valve_loss = + latent_data.electro_hydraulic.relief_valve_loss; + this->dataPtr->latent_data_.electro_hydraulic.hydraulic_motor_loss = + latent_data.electro_hydraulic.hydraulic_motor_loss; this->dataPtr->latent_data_.electro_hydraulic.eff_m = latent_data.electro_hydraulic.eff_m; this->dataPtr->latent_data_.electro_hydraulic.eff_v = From 0f24cce66d7dada26c81ce55909dc56330e468ff Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Wed, 13 Sep 2023 10:51:46 -0700 Subject: [PATCH 22/26] Resolved signs with supplied hydraulic power --- .../ElectroHydraulicPTO/ElectroHydraulicPTO.cpp | 2 ++ .../ElectroHydraulicPTO/ElectroHydraulicSoln.hpp | 14 +++++++------- .../src/LatentData/LatentData/LatentData.hpp | 2 ++ .../LatentData/LatentData/LatentDataPublisher.cpp | 2 ++ 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 80865d65..283de0b6 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -412,6 +412,8 @@ void ElectroHydraulicPTO::PreUpdate( latent_data.electro_hydraulic.valid = true; latent_data.electro_hydraulic.inst_power = VBus * (I_Batt + I_Load); + latent_data.electro_hydraulic.supplied_hydraulic_power = + -deltaP*this->dataPtr->functor.Q/buoy_utils::INLB_PER_NM; latent_data.electro_hydraulic.shaft_mech_power = this->dataPtr->functor.ShaftMechPower; latent_data.electro_hydraulic.rpm = N; diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index 28d5f0ff..0c6c62ec 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -177,12 +177,12 @@ struct ElectroHydraulicSoln : Functor const double T_applied = 1.375 * this->I_Wind.TorqueConstantInLbPerAmp * WindCurr; - ShaftMechPower = T_applied * buoy_utils::NM_PER_INLB * + ShaftMechPower = -T_applied * buoy_utils::NM_PER_INLB * x[0U] * buoy_utils::RPM_TO_RAD_PER_SEC; FrictionLoss = MotorDriveFrictionLoss(x[0U]); SwitchingLoss = MotorDriveSwitchingLoss(x[2U]); I2RLoss = MotorDriveISquaredRLoss(WindCurr); - BusPower = -ShaftMechPower - (FrictionLoss+SwitchingLoss+I2RLoss); + BusPower = ShaftMechPower - (FrictionLoss+SwitchingLoss+I2RLoss); double Q_Relief = 0; if (x[1U] < -PressReliefSetPoint) { // Pressure relief is a one wave valve, // relieves when lower pressure is higher @@ -193,16 +193,16 @@ struct ElectroHydraulicSoln : Functor ReliefValveLoss = Q_Relief*x[1U]*buoy_utils::INLB_PER_NM; // Result is Watts double Q_Motor = this->Q - Q_Relief; - double Q = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute; - double Q_Leak = (1.0 - eff_v) * std::max(fabs(Q_Motor), fabs(Q)) * sgn(x[1]); + double Q_Ideal = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute; + double Q_Leak = (1.0 - eff_v) * std::max(fabs(Q_Motor), fabs(Q_Ideal)) * sgn(x[1]); double T_Fluid = x[1U] * this->HydMotorDisp / (2.0 * M_PI); double T_Friction = -(1.0 - eff_m) * std::max(fabs(T_applied), fabs(T_Fluid)) * sgn(x[0]); - HydraulicMotorLoss = Q_Leak*x[1U]/buoy_utils::INLB_PER_NM + //Result is Watts - T_Friction*x[0U]/(buoy_utils::INLB_PER_NM*buoy_utils::SecondsPerMinute); + HydraulicMotorLoss = Q_Leak*x[1U]/buoy_utils::INLB_PER_NM - //Result is Watts + T_Friction*x[0U]*2.0 * M_PI/(buoy_utils::INLB_PER_NM*buoy_utils::SecondsPerMinute); - fvec[0U] = Q_Motor - Q_Leak - Q; + fvec[0U] = Q_Motor - Q_Leak - Q_Ideal; fvec[1U] = T_applied + T_Friction + T_Fluid; fvec[2U] = BusPower - (x[2U] - VBattEMF) * x[2U] / this->Ri; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 957f5af7..1a5e6bcb 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -107,6 +107,7 @@ struct AirSpring struct ElectroHydraulic { bool valid{false}; + double supplied_hydraulic_power{0.0}; // Watts double shaft_mech_power{0.0}; // Watts double inst_power{0.0}; // Watts double rpm{0.0}; @@ -123,6 +124,7 @@ struct ElectroHydraulic bool operator==(const ElectroHydraulic & that) const { bool equal = (this->valid == that.valid); + equal &= fabs(this->supplied_hydraulic_power - that.supplied_hydraulic_power) < 1e-7F; equal &= fabs(this->shaft_mech_power - that.shaft_mech_power) < 1e-7F; equal &= fabs(this->inst_power - that.inst_power) < 1e-7F; equal &= fabs(this->force - that.force) < 1e-7F; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index bfe7d7b8..8bdb6c73 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -265,6 +265,8 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.lower_spring.piston_velocity = latent_data.lower_spring.piston_velocity; + this->dataPtr->latent_data_.electro_hydraulic.supplied_hydraulic_power = + latent_data.electro_hydraulic.supplied_hydraulic_power; this->dataPtr->latent_data_.electro_hydraulic.shaft_mech_power = latent_data.electro_hydraulic.shaft_mech_power; this->dataPtr->latent_data_.electro_hydraulic.inst_power = From 64a5198eaf45c28e707c1f1fe56acc57bf24bde6 Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Wed, 13 Sep 2023 11:10:15 -0700 Subject: [PATCH 23/26] Fixed bug in relief valve loss computation --- buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index 0c6c62ec..91a49220 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -190,7 +190,7 @@ struct ElectroHydraulicSoln : Functor Q_Relief = (x[1U] + PressReliefSetPoint) * ReliefValveFlowPerPSI * buoy_utils::CubicInchesPerGallon / buoy_utils::SecondsPerMinute; } - ReliefValveLoss = Q_Relief*x[1U]*buoy_utils::INLB_PER_NM; // Result is Watts + ReliefValveLoss = Q_Relief*x[1U]/buoy_utils::INLB_PER_NM; // Result is Watts double Q_Motor = this->Q - Q_Relief; double Q_Ideal = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute; From 4cfbdb400f3633d7370dd5cdd955f64539d71450 Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Thu, 14 Sep 2023 10:00:09 -0700 Subject: [PATCH 24/26] Adjusted fields --- .../ElectroHydraulicPTO.cpp | 23 ++++++++----- .../src/LatentData/LatentData/LatentData.hpp | 33 ++++++++++--------- .../LatentData/LatentDataPublisher.cpp | 30 +++++++++-------- 3 files changed, 48 insertions(+), 38 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 283de0b6..01ce9276 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -411,27 +411,32 @@ void ElectroHydraulicPTO::PreUpdate( double piston_force = -deltaP * this->dataPtr->PistonArea * buoy_utils::NEWTONS_PER_LB; latent_data.electro_hydraulic.valid = true; - latent_data.electro_hydraulic.inst_power = VBus * (I_Batt + I_Load); + latent_data.electro_hydraulic.rpm = N; + latent_data.electro_hydraulic.upper_hydraulic_pressure = + pto_state.upper_hyd_press * buoy_utils::PASCAL_PER_PSI; + latent_data.electro_hydraulic.lower_hydraulic_pressure = + pto_state.lower_hyd_press * buoy_utils::PASCAL_PER_PSI; + latent_data.electro_hydraulic.force = piston_force; latent_data.electro_hydraulic.supplied_hydraulic_power = -deltaP*this->dataPtr->functor.Q/buoy_utils::INLB_PER_NM; + latent_data.electro_hydraulic.hydraulic_motor_loss = + this->dataPtr->functor.HydraulicMotorLoss; + latent_data.electro_hydraulic.relief_valve_loss = + this->dataPtr->functor.ReliefValveLoss; latent_data.electro_hydraulic.shaft_mech_power = this->dataPtr->functor.ShaftMechPower; - latent_data.electro_hydraulic.rpm = N; - latent_data.electro_hydraulic.force = piston_force; latent_data.electro_hydraulic.motor_drive_i2r_loss = this->dataPtr->functor.I2RLoss; latent_data.electro_hydraulic.motor_drive_switching_loss = this->dataPtr->functor.SwitchingLoss; latent_data.electro_hydraulic.motor_drive_friction_loss = this->dataPtr->functor.FrictionLoss; - latent_data.electro_hydraulic.relief_valve_loss = - this->dataPtr->functor.ReliefValveLoss; - latent_data.electro_hydraulic.hydraulic_motor_loss = - this->dataPtr->functor.HydraulicMotorLoss; + latent_data.electro_hydraulic.load_dump_power = + I_Load * VBus; latent_data.electro_hydraulic.battery_i2r_loss = I_Batt * I_Batt * this->dataPtr->Ri; - latent_data.electro_hydraulic.eff_m = eff_m; - latent_data.electro_hydraulic.eff_v = eff_v; + latent_data.electro_hydraulic.battery_storage_power = + I_Batt * VBus - latent_data.electro_hydraulic.battery_i2r_loss; _ecm.SetComponentData( this->dataPtr->PrismaticJointEntity, diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 1a5e6bcb..5733613c 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -107,35 +107,38 @@ struct AirSpring struct ElectroHydraulic { bool valid{false}; - double supplied_hydraulic_power{0.0}; // Watts - double shaft_mech_power{0.0}; // Watts - double inst_power{0.0}; // Watts double rpm{0.0}; + double upper_hydraulic_pressure{0.0}; // Pa + double lower_hydraulic_pressure{0.0}; // Pa double force{0.0}; // Newtons + double supplied_hydraulic_power{0.0}; // Watts + double hydraulic_motor_loss{0.0}; // Watts + double relief_valve_loss{0.0}; // Watts + double shaft_mech_power{0.0}; // Watts double motor_drive_i2r_loss{0.0}; // Watts - double motor_drive_friction_loss{0.0}; // Watts double motor_drive_switching_loss{0.0}; // Watts + double motor_drive_friction_loss{0.0}; // Watts + double load_dump_power{0.0}; // Watts double battery_i2r_loss{0.0}; // Watts - double relief_valve_loss{0.0}; // Watts - double hydraulic_motor_loss{0.0}; // Watts - double eff_m{0.0}; // mechanical efficiency - double eff_v{0.0}; // volumetric efficiency + double battery_storage_power{0.0}; // Watts bool operator==(const ElectroHydraulic & that) const { bool equal = (this->valid == that.valid); + equal &= fabs(this->rpm - that.rpm) < 1e-7F; + equal &= fabs(this->upper_hydraulic_pressure - that.upper_hydraulic_pressure) < 1e-7F; + equal &= fabs(this->lower_hydraulic_pressure - that.lower_hydraulic_pressure) < 1e-7F; + equal &= fabs(this->force - that.force) < 1e-7F; equal &= fabs(this->supplied_hydraulic_power - that.supplied_hydraulic_power) < 1e-7F; + equal &= fabs(this->hydraulic_motor_loss - that.hydraulic_motor_loss) < 1e-7F; + equal &= fabs(this->relief_valve_loss - that.relief_valve_loss) < 1e-7F; equal &= fabs(this->shaft_mech_power - that.shaft_mech_power) < 1e-7F; - equal &= fabs(this->inst_power - that.inst_power) < 1e-7F; - equal &= fabs(this->force - that.force) < 1e-7F; equal &= fabs(this->motor_drive_i2r_loss - that.motor_drive_i2r_loss) < 1e-7F; - equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; equal &= fabs(this->motor_drive_switching_loss - that.motor_drive_switching_loss) < 1e-7F; + equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; + equal &= fabs(this->load_dump_power - that.load_dump_power) < 1e-7F; equal &= fabs(this->battery_i2r_loss - that.battery_i2r_loss) < 1e-7F; - equal &= fabs(this->relief_valve_loss - that.relief_valve_loss) < 1e-7F; - equal &= fabs(this->hydraulic_motor_loss - that.hydraulic_motor_loss) < 1e-7F; - equal &= fabs(this->eff_m - that.eff_m) < 1e-7F; - equal &= fabs(this->eff_v - that.eff_v) < 1e-7F; + equal &= fabs(this->battery_storage_power - that.battery_storage_power) < 1e-7F; return equal; } diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 8bdb6c73..e313a839 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -265,32 +265,34 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.lower_spring.piston_velocity = latent_data.lower_spring.piston_velocity; - this->dataPtr->latent_data_.electro_hydraulic.supplied_hydraulic_power = - latent_data.electro_hydraulic.supplied_hydraulic_power; - this->dataPtr->latent_data_.electro_hydraulic.shaft_mech_power = - latent_data.electro_hydraulic.shaft_mech_power; - this->dataPtr->latent_data_.electro_hydraulic.inst_power = - latent_data.electro_hydraulic.inst_power; this->dataPtr->latent_data_.electro_hydraulic.rpm = latent_data.electro_hydraulic.rpm; + this->dataPtr->latent_data_.electro_hydraulic.upper_hydraulic_pressure = + latent_data.electro_hydraulic.upper_hydraulic_pressure; + this->dataPtr->latent_data_.electro_hydraulic.lower_hydraulic_pressure = + latent_data.electro_hydraulic.lower_hydraulic_pressure; this->dataPtr->latent_data_.electro_hydraulic.force = latent_data.electro_hydraulic.force; + this->dataPtr->latent_data_.electro_hydraulic.supplied_hydraulic_power = + latent_data.electro_hydraulic.supplied_hydraulic_power; + this->dataPtr->latent_data_.electro_hydraulic.hydraulic_motor_loss = + latent_data.electro_hydraulic.hydraulic_motor_loss; + this->dataPtr->latent_data_.electro_hydraulic.relief_valve_loss = + latent_data.electro_hydraulic.relief_valve_loss; + this->dataPtr->latent_data_.electro_hydraulic.shaft_mech_power = + latent_data.electro_hydraulic.shaft_mech_power; this->dataPtr->latent_data_.electro_hydraulic.motor_drive_i2r_loss = latent_data.electro_hydraulic.motor_drive_i2r_loss; this->dataPtr->latent_data_.electro_hydraulic.motor_drive_switching_loss = latent_data.electro_hydraulic.motor_drive_switching_loss; this->dataPtr->latent_data_.electro_hydraulic.motor_drive_friction_loss = latent_data.electro_hydraulic.motor_drive_friction_loss; + this->dataPtr->latent_data_.electro_hydraulic.load_dump_power = + latent_data.electro_hydraulic.load_dump_power; this->dataPtr->latent_data_.electro_hydraulic.battery_i2r_loss = latent_data.electro_hydraulic.battery_i2r_loss; - this->dataPtr->latent_data_.electro_hydraulic.relief_valve_loss = - latent_data.electro_hydraulic.relief_valve_loss; - this->dataPtr->latent_data_.electro_hydraulic.hydraulic_motor_loss = - latent_data.electro_hydraulic.hydraulic_motor_loss; - this->dataPtr->latent_data_.electro_hydraulic.eff_m = - latent_data.electro_hydraulic.eff_m; - this->dataPtr->latent_data_.electro_hydraulic.eff_v = - latent_data.electro_hydraulic.eff_v; + this->dataPtr->latent_data_.electro_hydraulic.battery_storage_power = + latent_data.electro_hydraulic.battery_storage_power; this->dataPtr->latent_data_.wave_body.buoyancy.force.x = latent_data.wave_body.buoyant_force.X(); From da314862743cdc06c9e306fd29c8ce785b980905 Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Thu, 14 Sep 2023 11:10:35 -0700 Subject: [PATCH 25/26] Fixed formating for linters --- .../ElectroHydraulicPTO.cpp | 38 +++++++++---------- .../ElectroHydraulicSoln.hpp | 10 ++--- .../src/LatentData/LatentData/LatentData.hpp | 4 +- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 01ce9276..fa927ca9 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -412,31 +412,31 @@ void ElectroHydraulicPTO::PreUpdate( latent_data.electro_hydraulic.valid = true; latent_data.electro_hydraulic.rpm = N; - latent_data.electro_hydraulic.upper_hydraulic_pressure = - pto_state.upper_hyd_press * buoy_utils::PASCAL_PER_PSI; - latent_data.electro_hydraulic.lower_hydraulic_pressure = - pto_state.lower_hyd_press * buoy_utils::PASCAL_PER_PSI; + latent_data.electro_hydraulic.upper_hydraulic_pressure = + pto_state.upper_hyd_press * buoy_utils::PASCAL_PER_PSI; + latent_data.electro_hydraulic.lower_hydraulic_pressure = + pto_state.lower_hyd_press * buoy_utils::PASCAL_PER_PSI; latent_data.electro_hydraulic.force = piston_force; - latent_data.electro_hydraulic.supplied_hydraulic_power = - -deltaP*this->dataPtr->functor.Q/buoy_utils::INLB_PER_NM; - latent_data.electro_hydraulic.hydraulic_motor_loss = - this->dataPtr->functor.HydraulicMotorLoss; - latent_data.electro_hydraulic.relief_valve_loss = - this->dataPtr->functor.ReliefValveLoss; - latent_data.electro_hydraulic.shaft_mech_power = + latent_data.electro_hydraulic.supplied_hydraulic_power = + -deltaP * this->dataPtr->functor.Q / buoy_utils::INLB_PER_NM; + latent_data.electro_hydraulic.hydraulic_motor_loss = + this->dataPtr->functor.HydraulicMotorLoss; + latent_data.electro_hydraulic.relief_valve_loss = + this->dataPtr->functor.ReliefValveLoss; + latent_data.electro_hydraulic.shaft_mech_power = this->dataPtr->functor.ShaftMechPower; - latent_data.electro_hydraulic.motor_drive_i2r_loss = + latent_data.electro_hydraulic.motor_drive_i2r_loss = this->dataPtr->functor.I2RLoss; - latent_data.electro_hydraulic.motor_drive_switching_loss = - this->dataPtr->functor.SwitchingLoss; - latent_data.electro_hydraulic.motor_drive_friction_loss = - this->dataPtr->functor.FrictionLoss; + latent_data.electro_hydraulic.motor_drive_switching_loss = + this->dataPtr->functor.SwitchingLoss; + latent_data.electro_hydraulic.motor_drive_friction_loss = + this->dataPtr->functor.FrictionLoss; latent_data.electro_hydraulic.load_dump_power = - I_Load * VBus; + I_Load * VBus; latent_data.electro_hydraulic.battery_i2r_loss = - I_Batt * I_Batt * this->dataPtr->Ri; + I_Batt * I_Batt * this->dataPtr->Ri; latent_data.electro_hydraulic.battery_storage_power = - I_Batt * VBus - latent_data.electro_hydraulic.battery_i2r_loss; + I_Batt * VBus - latent_data.electro_hydraulic.battery_i2r_loss; _ecm.SetComponentData( this->dataPtr->PrismaticJointEntity, diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index 91a49220..1b25f510 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -182,7 +182,7 @@ struct ElectroHydraulicSoln : Functor FrictionLoss = MotorDriveFrictionLoss(x[0U]); SwitchingLoss = MotorDriveSwitchingLoss(x[2U]); I2RLoss = MotorDriveISquaredRLoss(WindCurr); - BusPower = ShaftMechPower - (FrictionLoss+SwitchingLoss+I2RLoss); + BusPower = ShaftMechPower - (FrictionLoss + SwitchingLoss + I2RLoss); double Q_Relief = 0; if (x[1U] < -PressReliefSetPoint) { // Pressure relief is a one wave valve, // relieves when lower pressure is higher @@ -190,8 +190,8 @@ struct ElectroHydraulicSoln : Functor Q_Relief = (x[1U] + PressReliefSetPoint) * ReliefValveFlowPerPSI * buoy_utils::CubicInchesPerGallon / buoy_utils::SecondsPerMinute; } - ReliefValveLoss = Q_Relief*x[1U]/buoy_utils::INLB_PER_NM; // Result is Watts - + ReliefValveLoss = Q_Relief * x[1U] / buoy_utils::INLB_PER_NM; // Result is Watts + double Q_Motor = this->Q - Q_Relief; double Q_Ideal = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute; double Q_Leak = (1.0 - eff_v) * std::max(fabs(Q_Motor), fabs(Q_Ideal)) * sgn(x[1]); @@ -199,8 +199,8 @@ struct ElectroHydraulicSoln : Functor double T_Fluid = x[1U] * this->HydMotorDisp / (2.0 * M_PI); double T_Friction = -(1.0 - eff_m) * std::max(fabs(T_applied), fabs(T_Fluid)) * sgn(x[0]); - HydraulicMotorLoss = Q_Leak*x[1U]/buoy_utils::INLB_PER_NM - //Result is Watts - T_Friction*x[0U]*2.0 * M_PI/(buoy_utils::INLB_PER_NM*buoy_utils::SecondsPerMinute); + HydraulicMotorLoss = Q_Leak * x[1U] / buoy_utils::INLB_PER_NM - // Result is Watts + T_Friction * x[0U] * 2.0 * M_PI / (buoy_utils::INLB_PER_NM * buoy_utils::SecondsPerMinute); fvec[0U] = Q_Motor - Q_Leak - Q_Ideal; fvec[1U] = T_applied + T_Friction + T_Fluid; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 5733613c..a778ac43 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -118,9 +118,9 @@ struct ElectroHydraulic double motor_drive_i2r_loss{0.0}; // Watts double motor_drive_switching_loss{0.0}; // Watts double motor_drive_friction_loss{0.0}; // Watts - double load_dump_power{0.0}; // Watts + double load_dump_power{0.0}; // Watts double battery_i2r_loss{0.0}; // Watts - double battery_storage_power{0.0}; // Watts + double battery_storage_power{0.0}; // Watts bool operator==(const ElectroHydraulic & that) const { From fbe8a5d62dca0faa7071782f122f3812255564b1 Mon Sep 17 00:00:00 2001 From: andermi Date: Thu, 21 Sep 2023 12:30:01 -0700 Subject: [PATCH 26/26] Add wave body total power to latent data (#165) * added wave_body total forces Signed-off-by: Michael Anderson * added buoy pose and twist to latent data Signed-off-by: Michael Anderson * added pose/twist to operator== for latent data Signed-off-by: Michael Anderson * init pose/twist Signed-off-by: Michael Anderson * linters Signed-off-by: Michael Anderson --------- Signed-off-by: Michael Anderson --- .../src/LatentData/LatentData/LatentData.hpp | 11 ++++++ .../LatentData/LatentDataPublisher.cpp | 38 ++++++++++++++++++- .../WaveBodyInteractions.cpp | 8 ++++ 3 files changed, 55 insertions(+), 2 deletions(-) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index a778ac43..d2016d65 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -148,22 +148,33 @@ struct WaveBody { bool valid{false}; + gz::math::Pose3 pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + gz::math::Pose3 twist{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + gz::math::Vector3d buoyant_force{0.0, 0.0, 0.0}; gz::math::Vector3d buoyant_moment{0.0, 0.0, 0.0}; + double buoyancy_total_power{0.0}; gz::math::Vector3d radiation_force{0.0, 0.0, 0.0}; gz::math::Vector3d radiation_moment{0.0, 0.0, 0.0}; + double radiation_total_power{0.0}; gz::math::Vector3d exciting_force{0.0, 0.0, 0.0}; gz::math::Vector3d exciting_moment{0.0, 0.0, 0.0}; + double excitation_total_power{0.0}; bool operator==(const WaveBody & that) const { bool equal = (this->valid == that.valid); + equal &= (this->pose == that.pose); + equal &= (this->twist == that.twist); equal &= (this->buoyant_force == that.buoyant_force); equal &= (this->buoyant_moment == that.buoyant_moment); + equal &= fabs(this->buoyancy_total_power - that.buoyancy_total_power) < 1e-7F; equal &= (this->radiation_force == that.radiation_force); equal &= (this->radiation_moment == that.radiation_moment); + equal &= fabs(this->radiation_total_power - that.radiation_total_power) < 1e-7F; equal &= (this->exciting_force == that.exciting_force); equal &= (this->exciting_moment == that.exciting_moment); + equal &= fabs(this->excitation_total_power - that.excitation_total_power) < 1e-7F; return equal; } diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index e313a839..666a0c56 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -294,6 +294,34 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.electro_hydraulic.battery_storage_power = latent_data.electro_hydraulic.battery_storage_power; + this->dataPtr->latent_data_.wave_body.pose.position.x = + latent_data.wave_body.pose.X(); + this->dataPtr->latent_data_.wave_body.pose.position.y = + latent_data.wave_body.pose.Y(); + this->dataPtr->latent_data_.wave_body.pose.position.z = + latent_data.wave_body.pose.Z(); + this->dataPtr->latent_data_.wave_body.pose.orientation.x = + latent_data.wave_body.pose.Rot().X(); + this->dataPtr->latent_data_.wave_body.pose.orientation.y = + latent_data.wave_body.pose.Rot().Y(); + this->dataPtr->latent_data_.wave_body.pose.orientation.z = + latent_data.wave_body.pose.Rot().Z(); + this->dataPtr->latent_data_.wave_body.pose.orientation.w = + latent_data.wave_body.pose.Rot().W(); + + this->dataPtr->latent_data_.wave_body.twist.linear.x = + latent_data.wave_body.twist.X(); + this->dataPtr->latent_data_.wave_body.twist.linear.y = + latent_data.wave_body.twist.Y(); + this->dataPtr->latent_data_.wave_body.twist.linear.z = + latent_data.wave_body.twist.Z(); + this->dataPtr->latent_data_.wave_body.twist.angular.x = + latent_data.wave_body.twist.Roll(); + this->dataPtr->latent_data_.wave_body.twist.angular.y = + latent_data.wave_body.twist.Pitch(); + this->dataPtr->latent_data_.wave_body.twist.angular.z = + latent_data.wave_body.twist.Yaw(); + this->dataPtr->latent_data_.wave_body.buoyancy.force.x = latent_data.wave_body.buoyant_force.X(); this->dataPtr->latent_data_.wave_body.buoyancy.force.y = @@ -306,6 +334,9 @@ void LatentDataPublisher::PostUpdate( latent_data.wave_body.buoyant_moment.Y(); this->dataPtr->latent_data_.wave_body.buoyancy.torque.z = latent_data.wave_body.buoyant_moment.Z(); + this->dataPtr->latent_data_.wave_body.buoyancy_total_power = + latent_data.wave_body.buoyancy_total_power; + this->dataPtr->latent_data_.wave_body.radiation.force.x = latent_data.wave_body.radiation_force.X(); this->dataPtr->latent_data_.wave_body.radiation.force.y = @@ -318,6 +349,9 @@ void LatentDataPublisher::PostUpdate( latent_data.wave_body.radiation_moment.Y(); this->dataPtr->latent_data_.wave_body.radiation.torque.z = latent_data.wave_body.radiation_moment.Z(); + this->dataPtr->latent_data_.wave_body.radiation_total_power = + latent_data.wave_body.radiation_total_power; + this->dataPtr->latent_data_.wave_body.excitation.force.x = latent_data.wave_body.exciting_force.X(); this->dataPtr->latent_data_.wave_body.excitation.force.y = @@ -330,12 +364,12 @@ void LatentDataPublisher::PostUpdate( latent_data.wave_body.exciting_moment.Y(); this->dataPtr->latent_data_.wave_body.excitation.torque.z = latent_data.wave_body.exciting_moment.Z(); + this->dataPtr->latent_data_.wave_body.excitation_total_power = + latent_data.wave_body.excitation_total_power; this->dataPtr->latent_data_.piston_friction_force = latent_data.piston_friction_force; - // TODO(andermi) fill in other stuff - this->dataPtr->data_valid_ = latent_data.valid(); data.unlock(); diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index a9b6eb0b..3b822159 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -362,12 +362,20 @@ void WaveBodyInteractions::PreUpdate( } latent_data.wave_body.valid = true; + latent_data.wave_body.pose = w_Pose_p; + latent_data.wave_body.twist = + gz::math::Pose3( + w_xdot_p.X(), w_xdot_p.Y(), w_xdot_p.Z(), + w_omega_p.X(), w_omega_p.Y(), w_omega_p.Z()); latent_data.wave_body.buoyant_force = w_FBp; latent_data.wave_body.buoyant_moment = w_MBp; + latent_data.wave_body.buoyancy_total_power = w_FBp.Dot(w_xdot_p) + w_MBp.Dot(w_omega_p); latent_data.wave_body.radiation_force = w_FRp; latent_data.wave_body.radiation_moment = w_MRp; + latent_data.wave_body.radiation_total_power = w_FRp.Dot(w_xdot_p) + w_MRp.Dot(w_omega_p); latent_data.wave_body.exciting_force = w_FEp; latent_data.wave_body.exciting_moment = w_MEp; + latent_data.wave_body.excitation_total_power = w_FEp.Dot(w_xdot_p) + w_MEp.Dot(w_omega_p); _ecm.SetComponentData( this->dataPtr->model.Entity(),