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.
-