From 3539321d7c07e8bf4ce3c84689b748a12e85edcc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Mon, 13 Jul 2020 18:13:09 +0200 Subject: [PATCH] Battery updates - part3 (#230) * More battery updates. Signed-off-by: Carlos Aguero * Tweaks Signed-off-by: Carlos Aguero * Updated tutorial and tweaks Signed-off-by: Carlos Aguero * Enable recharging Signed-off-by: Carlos Aguero * Tutorial tweaks. Signed-off-by: Carlos Aguero * Battery tweaks. Signed-off-by: Carlos Aguero * Add tau sanity checks. Signed-off-by: Carlos Aguero * Changelog Signed-off-by: Carlos Aguero Co-authored-by: Addisu Taddese --- Changelog.md | 3 + examples/worlds/linear_battery_demo.sdf | 16 +-- .../battery_plugin/LinearBatteryPlugin.cc | 124 +++++++++++++----- .../battery_plugin/LinearBatteryPlugin.hh | 14 +- tutorials/battery.md | 90 +++++++++++-- 5 files changed, 180 insertions(+), 67 deletions(-) diff --git a/Changelog.md b/Changelog.md index 6f5a4f1452..06b6fbad70 100644 --- a/Changelog.md +++ b/Changelog.md @@ -6,6 +6,9 @@ JointStatePublisher. * [Pull Request 222](https://github.com/ignitionrobotics/ign-gazebo/pull/222) +1. Fixed battery issues and updated tutorial. + * [Pull Request 230](https://github.com/ignitionrobotics/ign-gazebo/pull/230) + ### Ignition Gazebo 2.20.1 (2020-06-18) 1. Properly add new models into the scenegraph. With this fix, when a model is spawned it will be added into the graph and resulting calls to the `scene/info` service will return a correct `msgs::Scene`. diff --git a/examples/worlds/linear_battery_demo.sdf b/examples/worlds/linear_battery_demo.sdf index 5bb5d48974..032ce6d037 100644 --- a/examples/worlds/linear_battery_demo.sdf +++ b/examples/worlds/linear_battery_demo.sdf @@ -524,23 +524,19 @@ - + name="ignition::gazebo::systems::LinearBatteryPlugin"> linear_battery - 4.2 - 4.2 - -2.0 - 2.5 - 2.5 - 0.07 - 2.0 + 12.592 + 12.592 + 1.2009 + 6.6 + true true 3.0 2.1 - false true diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index bd824e45eb..8b9f1f8064 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -96,8 +97,8 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate /// \brief Battery inner resistance in Ohm. public: double r{0.0}; - /// \brief Current low-pass filter characteristic time in seconds. - public: double tau{0.0}; + /// \brief Current low-pass filter characteristic time in seconds [0, 1]. + public: double tau{1.0}; /// \brief Raw battery current in A. public: double iraw{0.0}; @@ -108,7 +109,7 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate /// \brief Instantaneous battery charge in Ah. public: double q{0.0}; - /// \brief State of charge + /// \brief State of charge [0, 1]. public: double soc{1.0}; /// \brief Recharge status @@ -117,9 +118,15 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate /// \brief Hours taken to fully charge battery public: double tCharge{0.0}; + /// \TODO(caguero) Remove this flag in Ignition Dome. + /// \brief Flag to enable some battery fixes. + public: bool fixIssue225{false}; + + /// \TODO(caguero) Remove in Ignition Dome. /// \brief Battery current for a historic time window public: std::deque iList; + /// \TODO(caguero) Remove in Ignition Dome. /// \brief Time interval for a historic time window public: std::deque dtList; @@ -129,8 +136,7 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate /// \brief Flag on whether the battery should start draining public: bool startDraining = true; - /// \brief The start time when battery starts draining - /// in seconds + /// \brief The start time when battery starts draining in seconds public: int drainStartTime = -1; /// \brief Book keep the last time printed, so as to not pollute dbg messages @@ -196,20 +202,49 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, if (_sdf->HasElement("open_circuit_voltage_linear_coef")) this->dataPtr->e1 = _sdf->Get("open_circuit_voltage_linear_coef"); + if (_sdf->HasElement("capacity")) + this->dataPtr->c = _sdf->Get("capacity"); + + if (this->dataPtr->c <= 0) + { + ignerr << "No or incorrect value specified. Capacity should be " + << "greater than 0.\n"; + return; + } + + this->dataPtr->q0 = this->dataPtr->c; if (_sdf->HasElement("initial_charge")) { this->dataPtr->q0 = _sdf->Get("initial_charge"); - this->dataPtr->q = this->dataPtr->q0; + if (this->dataPtr->q0 > this->dataPtr->c || this->dataPtr->q0 < 0) + { + ignerr << " value should be between [0, ]." + << std::endl; + this->dataPtr->q0 = + std::max(0.0, std::min(this->dataPtr->q0, this->dataPtr->c)); + ignerr << "Setting to [" << this->dataPtr->q0 + << "] instead." << std::endl; + } } - if (_sdf->HasElement("capacity")) - this->dataPtr->c = _sdf->Get("capacity"); + this->dataPtr->q = this->dataPtr->q0; if (_sdf->HasElement("resistance")) this->dataPtr->r = _sdf->Get("resistance"); if (_sdf->HasElement("smooth_current_tau")) + { this->dataPtr->tau = _sdf->Get("smooth_current_tau"); + if (this->dataPtr->tau <= 0) + { + ignerr << " value should be positive. " + << "Using [1] instead." << std::endl; + this->dataPtr->tau = 1; + } + } + + if (_sdf->HasElement("fix_issue_225")) + this->dataPtr->fixIssue225 = _sdf->Get("fix_issue_225"); if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) { @@ -291,22 +326,13 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, << "in LinearBatteryPlugin SDF" << std::endl; } - // Flag that indicates that the battery should start draining only on motion - if (_sdf->HasElement("start_on_motion")) - { - auto startOnMotion = _sdf->Get("start_on_motion"); - if (startOnMotion) - { - igndbg << "Start draining only on motion" << std::endl; - this->dataPtr->startDraining = false; - } - } - ignmsg << "LinearBatteryPlugin configured. Battery name: " << this->dataPtr->battery->Name() << std::endl; igndbg << "Battery initial voltage: " << this->dataPtr->battery->InitVoltage() << std::endl; + this->dataPtr->soc = this->dataPtr->q / this->dataPtr->c; + // Setup battery state topic std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/battery/" + this->dataPtr->battery->Name() + "/state"}; @@ -431,6 +457,17 @@ void LinearBatteryPlugin::Update(const UpdateInfo &_info, // Update actual battery this->dataPtr->stepSize = _info.dt; + // Sanity check: tau should be between [dt, +inf). + double dt = (std::chrono::duration_cast( + this->dataPtr->stepSize).count()) * 1e-9; + if (this->dataPtr->tau < dt) + { + ignerr << " should be in the range [dt, +inf) but is " + << "configured with [" << this->dataPtr->tau << "]. We'll be using " + << "[" << dt << "] instead" << std::endl; + this->dataPtr->tau = dt; + } + if (this->dataPtr->battery) { this->dataPtr->battery->Update(); @@ -448,8 +485,8 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &/*_ecm*/) { IGN_PROFILE("LinearBatteryPlugin::PostUpdate"); - // Nothing left to do if paused - if (_info.paused) + // Nothing left to do if paused or the publisher wasn't created. + if (_info.paused || !this->dataPtr->statePub) return; // Publish battery state @@ -460,7 +497,12 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info, msg.set_current(this->dataPtr->ismooth); msg.set_charge(this->dataPtr->q); msg.set_capacity(this->dataPtr->c); - msg.set_percentage(this->dataPtr->soc); + + if (this->dataPtr->fixIssue225) + msg.set_percentage(this->dataPtr->soc * 100); + else + msg.set_percentage(this->dataPtr->soc); + if (this->dataPtr->startCharging) msg.set_power_supply_status(msgs::BatteryState::CHARGING); else if (this->dataPtr->startDraining) @@ -478,9 +520,9 @@ double LinearBatteryPlugin::OnUpdateVoltage( { IGN_ASSERT(_battery != nullptr, "common::Battery is null."); - if (fabs(_battery->Voltage()) < 1e-3) + if (fabs(_battery->Voltage()) < 1e-3 && !this->dataPtr->startCharging) return 0.0; - if (this->dataPtr->StateOfCharge() < 0) + if (this->dataPtr->StateOfCharge() < 0 && !this->dataPtr->startCharging) return _battery->Voltage(); auto prevSocInt = static_cast(this->dataPtr->StateOfCharge() * 100); @@ -491,8 +533,11 @@ double LinearBatteryPlugin::OnUpdateVoltage( double totalpower = 0.0; double k = dt / this->dataPtr->tau; - for (auto powerLoad : _battery->PowerLoads()) - totalpower += powerLoad.second; + if (this->dataPtr->startDraining) + { + for (auto powerLoad : _battery->PowerLoads()) + totalpower += powerLoad.second; + } this->dataPtr->iraw = totalpower / _battery->Voltage(); @@ -506,14 +551,16 @@ double LinearBatteryPlugin::OnUpdateVoltage( this->dataPtr->ismooth = this->dataPtr->ismooth + k * (this->dataPtr->iraw - this->dataPtr->ismooth); - // Keep a list of historic currents and time intervals - if (this->dataPtr->iList.size() >= 100) + if (!this->dataPtr->fixIssue225) { - this->dataPtr->iList.pop_front(); - this->dataPtr->dtList.pop_front(); + if (this->dataPtr->iList.size() >= 100) + { + this->dataPtr->iList.pop_front(); + this->dataPtr->dtList.pop_front(); + } + this->dataPtr->iList.push_back(this->dataPtr->ismooth); + this->dataPtr->dtList.push_back(dt); } - this->dataPtr->iList.push_back(this->dataPtr->ismooth); - this->dataPtr->dtList.push_back(dt); // Convert dt to hours this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) / @@ -525,10 +572,15 @@ double LinearBatteryPlugin::OnUpdateVoltage( - this->dataPtr->r * this->dataPtr->ismooth; // Estimate state of charge - double isum = 0.0; - for (size_t i = 0; i < this->dataPtr->iList.size(); ++i) - isum += (this->dataPtr->iList[i] * this->dataPtr->dtList[i] / 3600.0); - this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; + if (this->dataPtr->fixIssue225) + this->dataPtr->soc = this->dataPtr->q / this->dataPtr->c; + else + { + double isum = 0.0; + for (size_t i = 0; i < this->dataPtr->iList.size(); ++i) + isum += (this->dataPtr->iList[i] * this->dataPtr->dtList[i] / 3600.0); + this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; + } // Throttle debug messages auto socInt = static_cast(this->dataPtr->StateOfCharge() * 100); diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.hh b/src/systems/battery_plugin/LinearBatteryPlugin.hh index 5f48afcb55..bf1b51eb57 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.hh +++ b/src/systems/battery_plugin/LinearBatteryPlugin.hh @@ -45,19 +45,19 @@ namespace systems /// Voltage at full charge /// Amount of voltage decrease when no /// charge - /// Initial charge of the battery - /// Total charge that the battery can hold - /// Internal resistance - /// coefficient for smoothing current - /// power load on battery (required) - /// if set to true, the battery will start draining - /// only if the robot has started moving + /// Initial charge of the battery (Ah) + /// Total charge that the battery can hold (Ah) + /// Internal resistance (Ohm) + /// coefficient for smoothing current [0, 1]. + /// power load on battery (required) (Watts) /// If true, the battery can be recharged /// If true, the start/stop signals for recharging the /// battery will also be available via topics. The /// regular Ignition services will still be available. /// Hours taken to fully charge the battery. /// (Required if is set to true) + /// True to change the battery behavior to fix some issues + /// described in https://github.com/ignitionrobotics/ign-gazebo/issues/225. class IGNITION_GAZEBO_VISIBLE LinearBatteryPlugin : public System, public ISystemConfigure, diff --git a/tutorials/battery.md b/tutorials/battery.md index 4b22eb39f1..771492de8d 100644 --- a/tutorials/battery.md +++ b/tutorials/battery.md @@ -2,43 +2,107 @@ The battery system keeps track of the battery charge on a robot model. -Currently, one battery per model is supported. When the battery drains completely, all joints of the corresponding model are turned off, meaning joint forces are set to 0. +Currently, one battery per model is supported. When the battery drains +completely, all joints of the corresponding model are turned off, meaning joint +forces are set to 0. All logic for battery consumption are encapsulated in a plugin. -## Try it out +## A perfect battery -A linear consumption battery plugin has been implemented. The battery can be added to a Model with: +An ideal battery has a constant voltage while discharging and no internal +resistance. Here's a minimum example of a perfect battery that can be added to a +model: ```{.xml} ... + name="ignition::gazebo::systems::LinearBatteryPlugin"> linear_battery 12.592 - 12.694 - -3.1424 - 1.1665 + 12.592 1.2009 - 0.061523 - 1.9499 6.6 + false ... ``` -`` is a consumer-specific parameter. You can set this to a high value to see what happens when the battery drains. All others are properties of the battery. -This has been added to a demo world, which can be run using: +Next, you can find a description of the SDF parameters used: + +``: The name of the battery. +``: Initial voltage of the battery (V). +``: Voltage at full charge (V). +``: Total charge that the battery can hold (Ah). +``: Power load on battery (W). +``: As reported [here](https://github.com/ignitionrobotics/ign-gazebo/issues/225), +there are some issues affecting batteries in Ignition Blueprint and Citadel. +This parameter fixes the issues. Feel free to omit the parameter if you have +legacy code and want to preserve the old behavior. + +When setting the ``, `` of the battery and its ``, +keep in mind the following formula: + +`battery_runtime` (hours) = `` * `` / `` + +### Known limitations + +If `` is not set, the battery drains at a faster (100x) rate. +In this case, the battery runtime should be calculated as follows: + +`battery_runtime` (hours) = `` * `` / (`` * 100) + + +## Try a more realistic battery + +If you need to model a more realistic battery, you can use the following +advanced SDF parameters: + +``: Amount of voltage decrease when no charge (V). +``: Initial charge of the battery (Ah). +``: Internal resistance (Ohm) +``: Coefficient for smoothing current. + +Please, refer to the battery specification to set the advanced values. + + +## Charging + +A battery can be charged if the SDF parameter `` is set to true. +Here are the relevant SDF parameters related with charging: + +``: As mentioned, it should be `true` to enable recharging. +``: Hours taken to fully charge the battery. Keep in mind that +this value assumes no battery load while charging. If the battery is under load, +it will take a longer time to recharge. +``: If true, the start/stop signals for recharging the +battery will also be available via topics. The regular Ignition services will +still be available. + +By default, two Ignition Transport services are available for managing charging: + +* `/model//battery//recharge/start`: Enable recharging. +* `/model//battery//recharge/stop`: Disable recharging. + +Both services accept an `ignition::msgs::Boolean` parameter. + +## Try out an example + +A battery has been added to a demo world, which can be run using: ``` cd ign-gazebo/examples/worlds ign gazebo -v 4 -r linear_battery_demo.sdf ``` -The blue vehicle on the left has a battery, while the one on the right does not. When the battery drains, the corresponding vehicle stops moving. +The blue vehicle on the left has a battery, while the one on the right does not. When the battery drains, the corresponding vehicle stops moving. Please, see +`ign-gazebo/examples/worlds/linear_battery_demo.sdf`, where you can +find the commands to visualize the state of the battery, as well as commands to +start and stop the recharging. + To control the vehicles with keyboard, run @@ -46,11 +110,9 @@ To control the vehicles with keyboard, run cd ign-gazebo/examples/standalone/keyboard/build ./keyboard ../keyboard.sdf ``` - See more about the usage of the keyboard plugin in `examples/standalone/keyboard/README.md`. ## Known Issues * The rate of consumption should be affected by torque. For example, going uphill should consume more power than going downhill. -