diff --git a/examples/worlds/kinetic_energy_monitor.sdf b/examples/worlds/kinetic_energy_monitor.sdf
new file mode 100644
index 0000000000..30de44ede4
--- /dev/null
+++ b/examples/worlds/kinetic_energy_monitor.sdf
@@ -0,0 +1,116 @@
+
+
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ true
+
+
+
+
+ 0 0 1
+
+
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+
+
+
+ 0 0 5 0 0 0
+
+
+
+ 3
+ 0
+ 0
+ 3
+ 0
+ 3
+
+ 3.0
+
+
+
+
+ 0.5
+
+
+
+
+
+
+
+ 0.5
+
+
+
+ 0 0 1 1
+ 0 0 1 1
+ 0 0 1 1
+
+
+
+
+
+ sphere_link
+ 100
+
+
+
+
+
+
+
diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index cf899f8476..c5eeabe6e7 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -85,6 +85,7 @@ add_subdirectory(imu)
add_subdirectory(joint_controller)
add_subdirectory(joint_position_controller)
add_subdirectory(joint_state_publisher)
+add_subdirectory(kinetic_energy_monitor)
add_subdirectory(lift_drag)
add_subdirectory(log)
add_subdirectory(log_video_recorder)
diff --git a/src/systems/kinetic_energy_monitor/CMakeLists.txt b/src/systems/kinetic_energy_monitor/CMakeLists.txt
new file mode 100644
index 0000000000..aa8ff86d29
--- /dev/null
+++ b/src/systems/kinetic_energy_monitor/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_add_system(kinetic-energy-monitor
+ SOURCES
+ KineticEnergyMonitor.cc
+ PUBLIC_LINK_LIBS
+ ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER}
+)
diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc
new file mode 100644
index 0000000000..60b1ab327e
--- /dev/null
+++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc
@@ -0,0 +1,200 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+
+#include "KineticEnergyMonitor.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace systems;
+
+/// \brief Private data class
+class ignition::gazebo::systems::KineticEnergyMonitorPrivate
+{
+ /// \brief Link of the model.
+ public: Entity linkEntity;
+
+ /// \brief Name of the model this plugin is attached to.
+ public: std::string modelName;
+
+ /// \brief Kinetic energy during the previous step.
+ public: double prevKineticEnergy {0.0};
+
+ /// \brief Kinetic energy threshold.
+ public: double keThreshold {7.0};
+
+ /// \brief Ignition communication publisher.
+ public: transport::Node::Publisher pub;
+
+ /// \brief The model this plugin is attached to.
+ public: Model model;
+};
+
+//////////////////////////////////////////////////
+KineticEnergyMonitor::KineticEnergyMonitor() : System(),
+ dataPtr(std::make_unique())
+{
+}
+
+//////////////////////////////////////////////////
+KineticEnergyMonitor::~KineticEnergyMonitor() = default;
+
+//////////////////////////////////////////////////
+void KineticEnergyMonitor::Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &/*_eventMgr*/)
+{
+ this->dataPtr->model = Model(_entity);
+
+ if (!this->dataPtr->model.Valid(_ecm))
+ {
+ ignerr << "KineticEnergyMonitor should be attached to a model "
+ << "entity. Failed to initialize." << std::endl;
+ return;
+ }
+
+ this->dataPtr->modelName = this->dataPtr->model.Name(_ecm);
+
+ auto sdfClone = _sdf->Clone();
+ std::string linkName;
+ if (sdfClone->HasElement("link_name"))
+ {
+ linkName = sdfClone->Get("link_name");
+ }
+
+ if (linkName.empty())
+ {
+ ignerr << "found an empty parameter. Failed to initialize."
+ << std::endl;
+ return;
+ }
+
+ // Get the link entity
+ this->dataPtr->linkEntity = this->dataPtr->model.LinkByName(_ecm, linkName);
+
+ if (this->dataPtr->linkEntity == kNullEntity)
+ {
+ ignerr << "Link " << linkName
+ << " could not be found. Failed to initialize.\n";
+ return;
+ }
+
+ this->dataPtr->keThreshold = sdfClone->Get(
+ "kinetic_energy_threshold", 7.0).first;
+
+ std::string defaultTopic{"/model/" + this->dataPtr->modelName +
+ "/kinetic_energy"};
+ std::string topic = sdfClone->Get("topic", defaultTopic).first;
+
+ ignmsg << "KineticEnergyMonitor publishing messages on "
+ << "[" << topic << "]" << std::endl;
+
+ transport::Node node;
+ this->dataPtr->pub = node.Advertise(topic);
+
+ if (!_ecm.Component(this->dataPtr->linkEntity))
+ {
+ _ecm.CreateComponent(this->dataPtr->linkEntity,
+ components::WorldPose());
+ }
+
+ if (!_ecm.Component(this->dataPtr->linkEntity))
+ {
+ _ecm.CreateComponent(this->dataPtr->linkEntity, components::Inertial());
+ }
+
+ // Create a world linear velocity component if one is not present.
+ if (!_ecm.Component(
+ this->dataPtr->linkEntity))
+ {
+ _ecm.CreateComponent(this->dataPtr->linkEntity,
+ components::WorldLinearVelocity());
+ }
+
+ // Create an angular velocity component if one is not present.
+ if (!_ecm.Component(
+ this->dataPtr->linkEntity))
+ {
+ _ecm.CreateComponent(this->dataPtr->linkEntity,
+ components::AngularVelocity());
+ }
+
+ // Create an angular velocity component if one is not present.
+ if (!_ecm.Component(
+ this->dataPtr->linkEntity))
+ {
+ _ecm.CreateComponent(this->dataPtr->linkEntity,
+ components::WorldAngularVelocity());
+ }
+}
+
+//////////////////////////////////////////////////
+void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/,
+ const EntityComponentManager &_ecm)
+{
+ if (this->dataPtr->linkEntity != kNullEntity)
+ {
+ Link link(this->dataPtr->linkEntity);
+ if (std::nullopt != link.WorldKineticEnergy(_ecm))
+ {
+ double currKineticEnergy = *link.WorldKineticEnergy(_ecm);
+
+ // We only care about positive values of this (the links looses energy)
+ double deltaKE = this->dataPtr->prevKineticEnergy - currKineticEnergy;
+ this->dataPtr->prevKineticEnergy = currKineticEnergy;
+
+ if (deltaKE > this->dataPtr->keThreshold)
+ {
+ ignmsg << this->dataPtr->modelName
+ << " Change in kinetic energy above threshold - deltaKE: "
+ << deltaKE << std::endl;
+ msgs::Double msg;
+ msg.set_data(deltaKE);
+ this->dataPtr->pub.Publish(msg);
+ }
+ }
+ }
+}
+
+IGNITION_ADD_PLUGIN(KineticEnergyMonitor, System,
+ KineticEnergyMonitor::ISystemConfigure,
+ KineticEnergyMonitor::ISystemPostUpdate
+)
+
+IGNITION_ADD_PLUGIN_ALIAS(KineticEnergyMonitor,
+ "ignition::gazebo::systems::KineticEnergyMonitor")
diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh
new file mode 100644
index 0000000000..de037608bd
--- /dev/null
+++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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 IGNITION_GAZEBO_SYSTEMS_KINETIC_ENERGY_MONITOR_HH_
+#define IGNITION_GAZEBO_SYSTEMS_KINETIC_ENERGY_MONITOR_HH_
+
+#include
+#include
+#include
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems
+{
+ // Forward declarations.
+ class KineticEnergyMonitorPrivate;
+
+ /// \brief A system that monitors the kinetic energy of a link in a model
+ /// and publishes when there is a lost of kinetic energy during a timestep
+ /// that surpasses a specific threshold.
+ /// This system can be used to detect when a model could be damaged.
+ ///
+ /// # System Parameters
+ ///
+ /// ``: Name of the link to monitor. This name must match
+ /// a name of link within the model.
+ ///
+ /// ``: Threshold, in Joule (J), after which
+ /// a message is generated on `` with the kinetic energy value that
+ /// surpassed the threshold.
+ ///
+ /// ``: Custom topic that this system will publish to when kinetic
+ /// energy surpasses the threshold. This element if optional, and the
+ /// default value is `/model/{name_of_model}/kinetic_energy`.
+ ///
+ /// # Example Usage
+ ///
+ /** \verbatim
+
+ 0 0 5 0 0 0
+
+
+
+ 3
+ 0
+ 0
+ 3
+ 0
+ 3
+
+ 3.0
+
+
+
+
+ 0.5
+
+
+
+
+
+
+ 0.5
+
+
+
+ 0 0 1 1
+ 0 0 1 1
+ 0 0 1 1
+
+
+
+
+ sphere_link
+ 100
+
+
+ \endverbatim */
+ class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor:
+ public System,
+ public ISystemConfigure,
+ public ISystemPostUpdate
+ {
+ /// \brief Constructor
+ public: KineticEnergyMonitor();
+
+ /// \brief Destructor
+ public: ~KineticEnergyMonitor() final;
+
+ // Documentation inherited
+ public: void Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &_eventMgr) final;
+
+ // Documentation inherited
+ public: void PostUpdate(const UpdateInfo &_info,
+ const EntityComponentManager &_ecm) final;
+
+ /// \brief Private data pointer.
+ private: std::unique_ptr dataPtr;
+ };
+}
+}
+}
+}
+#endif
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index bcb305e1b6..b3c33508fc 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -20,6 +20,7 @@ set(tests
joint_controller_system.cc
joint_position_controller_system.cc
joint_state_publisher_system.cc
+ kinetic_energy_monitor_system.cc
lift_drag_system.cc
level_manager.cc
level_manager_runtime_performers.cc
diff --git a/test/integration/kinetic_energy_monitor_system.cc b/test/integration/kinetic_energy_monitor_system.cc
new file mode 100644
index 0000000000..bd5b1853ec
--- /dev/null
+++ b/test/integration/kinetic_energy_monitor_system.cc
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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
+
+#include
+#include
+
+#include
+#include
+#include
+
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/test_config.hh"
+
+using namespace ignition;
+using namespace gazebo;
+
+/// \brief Test Kinetic Energy Monitor system
+class KineticEnergyMonitorTest : public ::testing::Test
+{
+ // Documentation inherited
+ protected: void SetUp() override
+ {
+ ignition::common::Console::SetVerbosity(4);
+ setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH",
+ (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1);
+ }
+};
+
+std::mutex mutex;
+std::vector dblMsgs;
+
+/////////////////////////////////////////////////
+void cb(const msgs::Double &_msg)
+{
+ mutex.lock();
+ dblMsgs.push_back(_msg);
+ mutex.unlock();
+}
+
+/////////////////////////////////////////////////
+// The test checks the world pose and sensor readings of a falling altimeter
+TEST_F(KineticEnergyMonitorTest, ModelFalling)
+{
+ // Start server
+ ServerConfig serverConfig;
+ const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+ "/test/worlds/altimeter.sdf";
+ serverConfig.SetSdfFile(sdfFile);
+
+ Server server(serverConfig);
+ EXPECT_FALSE(server.Running());
+ EXPECT_FALSE(*server.Running(0));
+
+ const std::string sensorName = "altimeter_sensor";
+
+ // subscribe to kinetic energy topic
+ std::string topic = "/model/altimeter_model/kinetic_energy";
+ transport::Node node;
+ node.Subscribe(topic, &cb);
+
+ // Run server
+ size_t iters = 1000u;
+ server.Run(true, iters, false);
+
+ // Wait for messages to be received
+ for (int sleep = 0; sleep < 30; ++sleep)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+ mutex.lock();
+ bool received = dblMsgs.size() > 0;
+ mutex.unlock();
+
+ if (received)
+ break;
+ }
+
+ mutex.lock();
+ EXPECT_EQ(1u, dblMsgs.size());
+ auto firstMsg = dblMsgs.front();
+ mutex.unlock();
+ EXPECT_GT(firstMsg.data(), 2);
+}
diff --git a/test/worlds/altimeter.sdf b/test/worlds/altimeter.sdf
index 4aaa125fc4..fa0a098272 100644
--- a/test/worlds/altimeter.sdf
+++ b/test/worlds/altimeter.sdf
@@ -72,6 +72,13 @@
true
+
+
+ link
+ 2
+