Skip to content

Commit

Permalink
Kinetic energy monitor plugin (#492)
Browse files Browse the repository at this point in the history 
Signed-off-by: Gonzalo de Pedro <[email protected]>
Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>
  • Loading branch information
@gonzodepedro @chapulina
3 people authored Dec 28, 2020
1 parent efe8ea1 commit a830b44
Show file tree
Hide file tree
Showing 8 changed files with 559 additions and 0 deletions.
116 changes: 116 additions & 0 deletions examples/worlds/kinetic_energy_monitor.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,116 @@
<?xml version="1.0" ?>
<!--
Ignition Gazebo kinetic energy monitory plugin demo
Monitor output using:
ign topic -et "/model/sphere/kinetic_energy"
-->
<sdf version="1.6">
<world name="kinetic_energy_monitor">

<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-user-commands-system"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="sphere">
<pose>0 0 5 0 0 0</pose>
<link name="sphere_link">
<inertial>
<inertia>
<ixx>3</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>3</iyy>
<iyz>0</iyz>
<izz>3</izz>
</inertia>
<mass>3.0</mass>
</inertial>
<collision name="sphere_collision">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</collision>

<visual name="sphere_visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1 1</ambient>
<diffuse>0 0 1 1</diffuse>
<specular>0 0 1 1</specular>
</material>
</visual>
</link>

<plugin
filename="ignition-gazebo-kinetic-energy-monitor-system"
name="ignition::gazebo::systems::KineticEnergyMonitor">
<link_name>sphere_link</link_name>
<kinetic_energy_threshold>100</kinetic_energy_threshold>
</plugin>

</model>

</world>
</sdf>

1 change: 1 addition & 0 deletions src/systems/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
6 changes: 6 additions & 0 deletions src/systems/kinetic_energy_monitor/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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}
)
200 changes: 200 additions & 0 deletions src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc
View file
Original file line number Diff line number Diff line change
@@ -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 <google/protobuf/message.h>
#include <ignition/msgs/double.pb.h>

#include <string>

#include <ignition/gazebo/components/AngularVelocity.hh>
#include <ignition/gazebo/components/Link.hh>
#include <ignition/gazebo/components/LinearVelocity.hh>
#include <ignition/gazebo/components/Name.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <ignition/gazebo/components/World.hh>
#include <ignition/gazebo/components/Inertial.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Link.hh>
#include <ignition/gazebo/Model.hh>
#include <ignition/gazebo/Util.hh>

#include <ignition/plugin/Register.hh>

#include <ignition/transport/Node.hh>

#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<KineticEnergyMonitorPrivate>())
{
}

//////////////////////////////////////////////////
KineticEnergyMonitor::~KineticEnergyMonitor() = default;

//////////////////////////////////////////////////
void KineticEnergyMonitor::Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_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<std::string>("link_name");
}

if (linkName.empty())
{
ignerr << "found an empty <link_name> 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<double>(
"kinetic_energy_threshold", 7.0).first;

std::string defaultTopic{"/model/" + this->dataPtr->modelName +
"/kinetic_energy"};
std::string topic = sdfClone->Get<std::string>("topic", defaultTopic).first;

ignmsg << "KineticEnergyMonitor publishing messages on "
<< "[" << topic << "]" << std::endl;

transport::Node node;
this->dataPtr->pub = node.Advertise<msgs::Double>(topic);

if (!_ecm.Component<components::WorldPose>(this->dataPtr->linkEntity))
{
_ecm.CreateComponent(this->dataPtr->linkEntity,
components::WorldPose());
}

if (!_ecm.Component<components::Inertial>(this->dataPtr->linkEntity))
{
_ecm.CreateComponent(this->dataPtr->linkEntity, components::Inertial());
}

// Create a world linear velocity component if one is not present.
if (!_ecm.Component<components::WorldLinearVelocity>(
this->dataPtr->linkEntity))
{
_ecm.CreateComponent(this->dataPtr->linkEntity,
components::WorldLinearVelocity());
}

// Create an angular velocity component if one is not present.
if (!_ecm.Component<components::AngularVelocity>(
this->dataPtr->linkEntity))
{
_ecm.CreateComponent(this->dataPtr->linkEntity,
components::AngularVelocity());
}

// Create an angular velocity component if one is not present.
if (!_ecm.Component<components::WorldAngularVelocity>(
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")
Loading

0 comments on commit a830b44

Please sign in to comment.