From 5251ce3f733a34ebe6dd2ba0d137ea843e8192c3 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Fri, 1 Sep 2023 09:51:12 -0700 Subject: [PATCH] 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 =