From 861865f16ba3dc4255b6dfe2a085aa3ef4e9a2e0 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 17 May 2021 15:25:04 -0700 Subject: [PATCH 01/13] Implements vehicle damage based on kinetic energy Signed-off-by: Nate Koenig --- subt_ign/include/subt_ign/GameLogicPlugin.hh | 5 + subt_ign/src/GameLogicPlugin.cc | 185 +++++++++++++++++++ 2 files changed, 190 insertions(+) diff --git a/subt_ign/include/subt_ign/GameLogicPlugin.hh b/subt_ign/include/subt_ign/GameLogicPlugin.hh index 1cb128fd..386f7456 100644 --- a/subt_ign/include/subt_ign/GameLogicPlugin.hh +++ b/subt_ign/include/subt_ign/GameLogicPlugin.hh @@ -28,6 +28,7 @@ namespace subt class GameLogicPlugin: public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate, public ignition::gazebo::ISystemPostUpdate { /// \brief Constructor @@ -42,6 +43,10 @@ namespace subt ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &_eventMgr) override; + // Documentation inherited + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + // Documentation inherited public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) override; diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 3d679ef9..25ef34cf 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -31,6 +31,13 @@ #include #include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -76,6 +83,7 @@ IGNITION_ADD_PLUGIN( subt::GameLogicPlugin, ignition::gazebo::System, subt::GameLogicPlugin::ISystemConfigure, + subt::GameLogicPlugin::ISystemPreUpdate, subt::GameLogicPlugin::ISystemPostUpdate) using namespace ignition; @@ -83,6 +91,23 @@ using namespace gazebo; using namespace systems; using namespace subt; +/// \brief Kinetic energy information used to determine when a crash occurs. +class KineticEnergyInfo +{ + /// \brief Value of the previous iteration's kinetic energy. + public: double prevKineticEnergy{0.0}; + + /// \brief Threshold past which a crash has happened. Negative number + /// indicates that unlimited kinetic energy is allowed. + public: double kineticEnergyThreshold{-1.0}; + + /// \brief The link used to get kinetic energy. + public: gazebo::Entity link; + + /// \brief Name of the robot. + public: std::string robotName; +}; + class subt::GameLogicPluginPrivate { /// \brief Write a simulation timestamp to a logfile. @@ -525,6 +550,9 @@ class subt::GameLogicPluginPrivate /// \brief Mutex to protect the eventCounter. public: std::mutex eventCounterMutex; + + /// \brief Kinetic energy information for each robot. + public: std::map keInfo; }; ////////////////////////////////////////////////// @@ -1012,6 +1040,163 @@ void GameLogicPluginPrivate::OnEvent(const ignition::msgs::Pose &_msg) } } +////////////////////////////////////////////////// +void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + // Height used to determine the KE threshold. Increasing this value will + // lower the threshold, meaning a less violent collision will disable + // the robot. + const double keHeight = 0.077; + + if (!this->dataPtr->started) + { + _ecm.Each( + [&](const gazebo::Entity &, + const gazebo::components::Sensor *, + const gazebo::components::ParentEntity *_parent) -> bool + { + // Get the model. We are assuming that a sensor is attached to + // a link. + auto model = _ecm.Component( + _parent->Data()); + if (model) + { + // Get the model name + auto mName = + _ecm.Component(model->Data()); + + // Skip if we already have information for this robot. + if (this->dataPtr->keInfo.find(model->Data()) != + this->dataPtr->keInfo.end()) { + return true; + } + + std::vector links = _ecm.EntitiesByComponents( + components::ParentEntity(model->Data()), components::Link()); + + // Get the mass for the robot by summing the mass of each link, + // and use the canonical link for KE computation. + double mass = 0.0; + for (const Entity &link : links) + { + auto inertial = _ecm.Component(link); + auto canonical = _ecm.Component(link); + mass += inertial->Data().MassMatrix().Mass(); + + if (canonical) + { + this->dataPtr->keInfo[model->Data()].link += link; + // Create a world pose component if one is not present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, components::WorldPose()); + } + + // Create an inertial component if one is not present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, components::Inertial()); + } + + // Create a world linear velocity component if one is not + // present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, + components::WorldLinearVelocity()); + } + + // Create an angular velocity component if one is not present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, + components::AngularVelocity()); + } + + // Create a world angular velocity component if one is not + // present. + if (!_ecm.Component( + link)) + { + _ecm.CreateComponent(link, + components::WorldAngularVelocity()); + } + } + } + + if (mass > 0) + { + // This sets the kinetic energy threshold for the robot. It is + // based on the kinetic energy formula KE = 0.5 * M * v * v + // + // M is the mass of the robot. + // v is velocity of the robot. We are using the velocity acheived + // by falling from a height. + this->dataPtr->keInfo[model->Data()].kineticEnergyThreshold = + 0.5 * mass * std::pow(sqrt((2 * keHeight) / 9.8) * 9.8, 2); + this->dataPtr->keInfo[model->Data()].robotName = mName->Data(); + + // Create a halt motion component if one is not + // present. + if (!_ecm.Component(model->Data())) + { + _ecm.CreateComponent(model->Data(), components::HaltMotion()); + } + } + } + return true; + }); + } + + // Check for crashes + for (auto &ke : this->dataPtr->keInfo) + { + ignition::gazebo::Link link(ke.second.link); + 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 = ke.second.prevKineticEnergy - currKineticEnergy; + ke.second.prevKineticEnergy = currKineticEnergy; + + // Crash if past the threshold. + if (ke.second.kineticEnergyThreshold > 0 && + deltaKE >= ke.second.kineticEnergyThreshold) + { + int64_t sec, nsec; + std::tie(sec, nsec) = ignition::math::durationToSecNsec(_info.simTime); + ignition::msgs::Time localSimTime; + localSimTime.set_sec(sec); + localSimTime.set_nsec(nsec); + + std::lock_guard lock(this->dataPtr->eventCounterMutex); + // _resp.set_report_status("scoring finished"); + std::ostringstream stream; + stream + << "- event:\n" + << " id: " << this->dataPtr->eventCounter << "\n" + << " type: kinetic\n" + << " time_sec: " << localSimTime.sec() << "\n" + << " robot: " << ke.second.robotName << std::endl; + this->dataPtr->LogEvent(stream.str()); + this->dataPtr->PublishRobotEvent( + localSimTime, "kinetic", ke.second.robotName, + this->dataPtr->eventCounter); + this->dataPtr->eventCounter++; + + auto *haltMotionComp = _ecm.Component(ke.first); + if (haltMotionComp && !haltMotionComp->Data()) + { + haltMotionComp->Data() = true; + } + } + } + } +} + ////////////////////////////////////////////////// void GameLogicPlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, From fb8589281fe946af239cdac5d534fe53e9e6b871 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 19 May 2021 16:55:50 -0700 Subject: [PATCH 02/13] Added else Signed-off-by: Nate Koenig --- subt_ign/src/GameLogicPlugin.cc | 78 +++++++++++++++++---------------- 1 file changed, 41 insertions(+), 37 deletions(-) diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 25ef34cf..7167540b 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -1149,48 +1149,52 @@ void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, return true; }); } - - // Check for crashes - for (auto &ke : this->dataPtr->keInfo) + else { - ignition::gazebo::Link link(ke.second.link); - if (std::nullopt != link.WorldKineticEnergy(_ecm)) + // Check for crashes + for (auto &ke : this->dataPtr->keInfo) { - double currKineticEnergy = *link.WorldKineticEnergy(_ecm); + ignition::gazebo::Link link(ke.second.link); + 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 = ke.second.prevKineticEnergy - currKineticEnergy; - ke.second.prevKineticEnergy = currKineticEnergy; + // We only care about positive values of this (the links looses energy) + double deltaKE = ke.second.prevKineticEnergy - currKineticEnergy; + ke.second.prevKineticEnergy = currKineticEnergy; - // Crash if past the threshold. - if (ke.second.kineticEnergyThreshold > 0 && - deltaKE >= ke.second.kineticEnergyThreshold) - { - int64_t sec, nsec; - std::tie(sec, nsec) = ignition::math::durationToSecNsec(_info.simTime); - ignition::msgs::Time localSimTime; - localSimTime.set_sec(sec); - localSimTime.set_nsec(nsec); - - std::lock_guard lock(this->dataPtr->eventCounterMutex); - // _resp.set_report_status("scoring finished"); - std::ostringstream stream; - stream - << "- event:\n" - << " id: " << this->dataPtr->eventCounter << "\n" - << " type: kinetic\n" - << " time_sec: " << localSimTime.sec() << "\n" - << " robot: " << ke.second.robotName << std::endl; - this->dataPtr->LogEvent(stream.str()); - this->dataPtr->PublishRobotEvent( - localSimTime, "kinetic", ke.second.robotName, - this->dataPtr->eventCounter); - this->dataPtr->eventCounter++; - - auto *haltMotionComp = _ecm.Component(ke.first); - if (haltMotionComp && !haltMotionComp->Data()) + // Crash if past the threshold. + if (ke.second.kineticEnergyThreshold > 0 && + deltaKE >= ke.second.kineticEnergyThreshold) { - haltMotionComp->Data() = true; + int64_t sec, nsec; + std::tie(sec, nsec) = + ignition::math::durationToSecNsec(_info.simTime); + ignition::msgs::Time localSimTime; + localSimTime.set_sec(sec); + localSimTime.set_nsec(nsec); + + std::lock_guard lock(this->dataPtr->eventCounterMutex); + // _resp.set_report_status("scoring finished"); + std::ostringstream stream; + stream + << "- event:\n" + << " id: " << this->dataPtr->eventCounter << "\n" + << " type: kinetic\n" + << " time_sec: " << localSimTime.sec() << "\n" + << " robot: " << ke.second.robotName << std::endl; + this->dataPtr->LogEvent(stream.str()); + this->dataPtr->PublishRobotEvent( + localSimTime, "kinetic", ke.second.robotName, + this->dataPtr->eventCounter); + this->dataPtr->eventCounter++; + + auto *haltMotionComp = + _ecm.Component(ke.first); + if (haltMotionComp && !haltMotionComp->Data()) + { + haltMotionComp->Data() = true; + } } } } From 651fb041b3a7a8b103caf9bee5e51d540f878e5b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 20 May 2021 08:02:01 -0700 Subject: [PATCH 03/13] Added ke_height as sdf parameter Signed-off-by: Nate Koenig --- subt_ign/launch/cloudsim_sim.ign | 2 +- subt_ign/launch/competition.ign | 1 + subt_ign/src/GameLogicPlugin.cc | 17 +++++++++++------ 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index 112c692d..3a39a3e0 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -435,9 +435,9 @@ entity_type="world" filename="libGameLogicPlugin.so" name="subt::GameLogicPlugin"> - <%= $worldName %> <%= $ros %> + 0.077 <%= $durationSec %> diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index 3fcb690b..4a1c4a91 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -363,6 +363,7 @@ <%= $worldName %> false + 0.077 <%= $durationSec %> diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 7167540b..9c4eb375 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -553,6 +553,12 @@ class subt::GameLogicPluginPrivate /// \brief Kinetic energy information for each robot. public: std::map keInfo; + + // \brief Height used to determine the KE threshold. + // Increasing this value will lower the threshold, meaning a + // less violent collision will disable the robot. This value can be + // adjusted via this plugin's SDF parameters. + public: double keHeight = 0.077; }; ////////////////////////////////////////////////// @@ -628,6 +634,9 @@ void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/, } } + this->dataPtr->keHeight = + _sdf->Get("ke_height", this->dataPtr->keHeight).first; + const sdf::ElementPtr rosElem = const_cast(_sdf.get())->GetElement("ros"); if (rosElem) @@ -1044,11 +1053,6 @@ void GameLogicPluginPrivate::OnEvent(const ignition::msgs::Pose &_msg) void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { - // Height used to determine the KE threshold. Increasing this value will - // lower the threshold, meaning a less violent collision will disable - // the robot. - const double keHeight = 0.077; - if (!this->dataPtr->started) { _ecm.EachdataPtr->keInfo[model->Data()].kineticEnergyThreshold = - 0.5 * mass * std::pow(sqrt((2 * keHeight) / 9.8) * 9.8, 2); + 0.5 * mass * std::pow( + sqrt((2 * this->dataPtr->keHeight) / 9.8) * 9.8, 2); this->dataPtr->keInfo[model->Data()].robotName = mName->Data(); // Create a halt motion component if one is not From 615557b83f2ac60b76b1739aafd83e9342436277 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 20 May 2021 13:26:18 -0700 Subject: [PATCH 04/13] Adding crash factor to robot types Signed-off-by: Nate Koenig --- .../include/subt_ign/RobotPlatformTypes.hh | 72 +++++++++---------- subt_ign/launch/cloudsim_sim.ign | 2 +- subt_ign/launch/competition.ign | 2 +- subt_ign/src/GameLogicPlugin.cc | 21 ++++-- 4 files changed, 55 insertions(+), 42 deletions(-) diff --git a/subt_ign/include/subt_ign/RobotPlatformTypes.hh b/subt_ign/include/subt_ign/RobotPlatformTypes.hh index e9cf7054..fb2a0f49 100644 --- a/subt_ign/include/subt_ign/RobotPlatformTypes.hh +++ b/subt_ign/include/subt_ign/RobotPlatformTypes.hh @@ -16,42 +16,42 @@ */ #ifndef SUBT_IGN_ROBOTPLATFORMTYPES_HH_ #define SUBT_IGN_ROBOTPLATFORMTYPES_HH_ -/// \brief List of robot platform types. This is used to count unique robot -/// types. -const std::vector robotPlatformTypes = { - "ABSOLEM", - "ALLIE", - "ANYMAL_B", - "ANYMAL_C", - "CRYSTAL", - "DS1", - "DTR", - "FREYJA", - "GAGARIN" - "HD2", - "HOVERMAP", - "HUSKY", - "KAREN", - "KLOUBAK", - "LILY", - "M100", - "MARV", - "MIKE", - "OZBOT_ATR", - "PAM", - "QAV500", - "R2", - "R3", - "RMF", - "ROCKY", - "SHAFTER", - "SPOT", - "TEAMBASE", - "X1", - "X2", - "X3", - "X4", - "X500", +#include +/// \brief List of robot platform types and kinetic energy threshold factor. This is used to count unique robot types and determine crashes. +const std::map robotPlatformTypes = { + {"ABSOLEM", 1}, + {"ALLIE", 1}, + {"ANYMAL_B", 1}, + {"ANYMAL_C", 1}, + {"CRYSTAL", 1}, + {"DS1", 1}, + {"DTR", 1}, + {"FREYJA", 1}, + {"GAGARIN", 1}, + {"HD2", 1}, + {"HOVERMAP", 1}, + {"HUSKY", 1}, + {"KAREN", 1}, + {"KLOUBAK", 1}, + {"LILY", 1}, + {"M100", 1}, + {"MARV", 1}, + {"MIKE", 1}, + {"OZBOT_ATR", 1}, + {"PAM", 1}, + {"QAV500", 1}, + {"R2", 1}, + {"R3", 1}, + {"RMF", 1}, + {"ROCKY", 1}, + {"SHAFTER", 1}, + {"SPOT", 1}, + {"TEAMBASE", 1}, + {"X1", 1}, + {"X2", 1}, + {"X3", 7}, + {"X4", 7}, + {"X500", 1}, }; #endif diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index 3a39a3e0..9c84ff20 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -437,7 +437,7 @@ name="subt::GameLogicPlugin"> <%= $worldName %> <%= $ros %> - 0.077 + 0.078 <%= $durationSec %> diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index 4a1c4a91..a5d53023 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -363,7 +363,7 @@ <%= $worldName %> false - 0.077 + 0.078 <%= $durationSec %> diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 9c4eb375..85ea8b25 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -1166,8 +1166,20 @@ void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, // We only care about positive values of this (the links looses energy) double deltaKE = ke.second.prevKineticEnergy - currKineticEnergy; + + // Debug: Compute the factor needed to hit the threshold. + if (deltaKE > 0.01) + { + double factor = ke.second.kineticEnergyThreshold / deltaKE; + std::cout << "KE[" << deltaKE << "] Thresh[" << ke.second.kineticEnergyThreshold << "] Factor[" << factor << "]\n"; + } + + // Apply KE factor. + deltaKE *= robotPlatformTypes.at( + this->dataPtr->robotFullTypes[ke.second.robotName].first); ke.second.prevKineticEnergy = currKineticEnergy; + // Crash if past the threshold. if (ke.second.kineticEnergyThreshold > 0 && deltaKE >= ke.second.kineticEnergyThreshold) @@ -1297,22 +1309,23 @@ void GameLogicPlugin::PostUpdate( model->Data()); // Store unique robot platform information. - for (const std::string &type : robotPlatformTypes) + for (const std::pair &typeKE : + robotPlatformTypes) { std::string platformNameUpper = filePath->Data(); std::transform(platformNameUpper.begin(), platformNameUpper.end(), platformNameUpper.begin(), ::toupper); - if (platformNameUpper.find(type) != std::string::npos) + if (platformNameUpper.find(typeKE.first) != std::string::npos) { - this->dataPtr->robotTypes.insert(type); + this->dataPtr->robotTypes.insert(typeKE.first); // The full type is in the directory name, which is third // from the end (.../TYPE/VERSION/model.sdf). std::vector pathParts = ignition::common::split(platformNameUpper, "/"); this->dataPtr->robotFullTypes[mName->Data()] = - {type, pathParts[pathParts.size()-3]}; + {typeKE.first, pathParts[pathParts.size()-3]}; } } From 2fae2debd86348bb18f378983287ff84b64e6738 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 20 May 2021 13:32:29 -0700 Subject: [PATCH 05/13] Updated X4 Signed-off-by: Nate Koenig --- subt_ign/include/subt_ign/RobotPlatformTypes.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subt_ign/include/subt_ign/RobotPlatformTypes.hh b/subt_ign/include/subt_ign/RobotPlatformTypes.hh index fb2a0f49..505f91f9 100644 --- a/subt_ign/include/subt_ign/RobotPlatformTypes.hh +++ b/subt_ign/include/subt_ign/RobotPlatformTypes.hh @@ -50,7 +50,7 @@ const std::map robotPlatformTypes = { {"X1", 1}, {"X2", 1}, {"X3", 7}, - {"X4", 7}, + {"X4", 7.2}, {"X500", 1}, }; From 56f62bcb12661f5e8338fa7f9abdadaa22775b59 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 21 May 2021 09:42:10 -0700 Subject: [PATCH 06/13] review comments Signed-off-by: Nate Koenig --- subt_ign/src/GameLogicPlugin.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 85ea8b25..6049881e 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -1192,12 +1192,11 @@ void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, localSimTime.set_nsec(nsec); std::lock_guard lock(this->dataPtr->eventCounterMutex); - // _resp.set_report_status("scoring finished"); std::ostringstream stream; stream << "- event:\n" << " id: " << this->dataPtr->eventCounter << "\n" - << " type: kinetic\n" + << " type: collision\n" << " time_sec: " << localSimTime.sec() << "\n" << " robot: " << ke.second.robotName << std::endl; this->dataPtr->LogEvent(stream.str()); From 76465389eee3ed29fb9326946cbc4ef48e43eb12 Mon Sep 17 00:00:00 2001 From: Angela Maio Date: Fri, 21 May 2021 14:32:38 -0400 Subject: [PATCH 07/13] fix RobotPlatformTypes formatting Signed-off-by: Angela Maio --- subt_ign/include/subt_ign/RobotPlatformTypes.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subt_ign/include/subt_ign/RobotPlatformTypes.hh b/subt_ign/include/subt_ign/RobotPlatformTypes.hh index 9e46f8f7..982a8d6b 100644 --- a/subt_ign/include/subt_ign/RobotPlatformTypes.hh +++ b/subt_ign/include/subt_ign/RobotPlatformTypes.hh @@ -31,7 +31,7 @@ const std::map robotPlatformTypes = { {"HD2", 1}, {"HOVERMAP", 1}, {"HUSKY", 1}, - {"JEANINE" 1}, + {"JEANINE", 1}, {"KAREN", 1}, {"KLOUBAK", 1}, {"LILY", 1}, @@ -52,7 +52,7 @@ const std::map robotPlatformTypes = { {"X2", 1}, {"X3", 7}, {"X4", 7.2}, - {"X500", 1}, + {"X500", 1} }; #endif From 9fa1bd968b864afc0b8b4728c3f366efff4dddd2 Mon Sep 17 00:00:00 2001 From: Angela Maio Date: Fri, 21 May 2021 15:43:40 -0400 Subject: [PATCH 08/13] collision event name for robot event msg Signed-off-by: Angela Maio --- subt_ign/src/GameLogicPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 6049881e..7edfbce3 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -1201,7 +1201,7 @@ void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, << " robot: " << ke.second.robotName << std::endl; this->dataPtr->LogEvent(stream.str()); this->dataPtr->PublishRobotEvent( - localSimTime, "kinetic", ke.second.robotName, + localSimTime, "collision", ke.second.robotName, this->dataPtr->eventCounter); this->dataPtr->eventCounter++; From 102aa622dc0ad1b5eeef35903dc42774ed889f7c Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 25 May 2021 08:04:33 -0700 Subject: [PATCH 09/13] simplify math Signed-off-by: Nate Koenig --- subt_ign/src/GameLogicPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 7edfbce3..12a79073 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -1140,7 +1140,7 @@ void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, // by falling from a height. this->dataPtr->keInfo[model->Data()].kineticEnergyThreshold = 0.5 * mass * std::pow( - sqrt((2 * this->dataPtr->keHeight) / 9.8) * 9.8, 2); + sqrt(2 * this->dataPtr->keHeight * 9.8), 2); this->dataPtr->keInfo[model->Data()].robotName = mName->Data(); // Create a halt motion component if one is not From 8c288eef789ae170a2f6ff9f34f070ea4ce99bb7 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 25 May 2021 11:17:54 -0700 Subject: [PATCH 10/13] tweak thresholds Signed-off-by: Nate Koenig --- subt_ign/include/subt_ign/RobotPlatformTypes.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subt_ign/include/subt_ign/RobotPlatformTypes.hh b/subt_ign/include/subt_ign/RobotPlatformTypes.hh index 982a8d6b..891350a9 100644 --- a/subt_ign/include/subt_ign/RobotPlatformTypes.hh +++ b/subt_ign/include/subt_ign/RobotPlatformTypes.hh @@ -50,9 +50,9 @@ const std::map robotPlatformTypes = { {"TEAMBASE", 1}, {"X1", 1}, {"X2", 1}, - {"X3", 7}, + {"X3", 7.2}, {"X4", 7.2}, - {"X500", 1} + {"X500", 14} }; #endif From b7a9e6581aa52479c3b8c35778afbcb0e44e2106 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 25 May 2021 15:04:37 -0700 Subject: [PATCH 11/13] Adjust values, and comment out debug statement Signed-off-by: Nate Koenig --- .../include/subt_ign/RobotPlatformTypes.hh | 24 +++++++++---------- subt_ign/src/GameLogicPlugin.cc | 13 +++++----- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/subt_ign/include/subt_ign/RobotPlatformTypes.hh b/subt_ign/include/subt_ign/RobotPlatformTypes.hh index 891350a9..cbdae304 100644 --- a/subt_ign/include/subt_ign/RobotPlatformTypes.hh +++ b/subt_ign/include/subt_ign/RobotPlatformTypes.hh @@ -23,36 +23,36 @@ const std::map robotPlatformTypes = { {"ALLIE", 1}, {"ANYMAL_B", 1}, {"ANYMAL_C", 1}, - {"CRYSTAL", 1}, - {"DS1", 1}, + {"CRYSTAL", 8}, // UAV, no prop guards + {"DS1", 6}, // UAV, prop guards {"DTR", 1}, {"FREYJA", 1}, - {"GAGARIN", 1}, + {"GAGARIN", 6}, // UAV, prop guards {"HD2", 1}, - {"HOVERMAP", 1}, + {"HOVERMAP", 6}, // UAV, prop guards {"HUSKY", 1}, {"JEANINE", 1}, {"KAREN", 1}, {"KLOUBAK", 1}, {"LILY", 1}, - {"M100", 1}, + {"M100", 8}, // UAV, no prop guards {"MARV", 1}, {"MIKE", 1}, {"OZBOT_ATR", 1}, - {"PAM", 1}, - {"QAV500", 1}, + {"PAM", 6}, // UAV, prop guards + {"QAV500", 6}, // UAV, prop guards {"R2", 1}, {"R3", 1}, - {"RMF", 1}, + {"RMF", 6}, // UAV, prop guards {"ROCKY", 1}, - {"SHAFTER", 1}, + {"SHAFTER", 8}, // UAV, no prop guards {"SPOT", 1}, {"TEAMBASE", 1}, {"X1", 1}, {"X2", 1}, - {"X3", 7.2}, - {"X4", 7.2}, - {"X500", 14} + {"X3", 8}, // UAV, no prop guards + {"X6", 8}, // UAV, no prop guards + {"X500", 8} // UAV, no prop guards }; #endif diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 12a79073..4c8c0a92 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -1168,18 +1168,19 @@ void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, double deltaKE = ke.second.prevKineticEnergy - currKineticEnergy; // Debug: Compute the factor needed to hit the threshold. - if (deltaKE > 0.01) - { - double factor = ke.second.kineticEnergyThreshold / deltaKE; - std::cout << "KE[" << deltaKE << "] Thresh[" << ke.second.kineticEnergyThreshold << "] Factor[" << factor << "]\n"; - } + // if (deltaKE > 0.01) + // { + // double factor = ke.second.kineticEnergyThreshold / deltaKE; + // std::cout << "KE[" << deltaKE << "] Thresh[" + // << ke.second.kineticEnergyThreshold << "] Factor[" + // << factor << "]\n"; + // } // Apply KE factor. deltaKE *= robotPlatformTypes.at( this->dataPtr->robotFullTypes[ke.second.robotName].first); ke.second.prevKineticEnergy = currKineticEnergy; - // Crash if past the threshold. if (ke.second.kineticEnergyThreshold > 0 && deltaKE >= ke.second.kineticEnergyThreshold) From 3a7692b8615143a021251ee2efe1ee2fc15b897d Mon Sep 17 00:00:00 2001 From: Arthur Schang Date: Tue, 25 May 2021 19:39:17 -0400 Subject: [PATCH 12/13] Updated collision tolerant UAVs to have the same charcteristics as UGVs. Fixed a typo where X4 was named X6. --- subt_ign/include/subt_ign/RobotPlatformTypes.hh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/subt_ign/include/subt_ign/RobotPlatformTypes.hh b/subt_ign/include/subt_ign/RobotPlatformTypes.hh index cbdae304..62bb3e41 100644 --- a/subt_ign/include/subt_ign/RobotPlatformTypes.hh +++ b/subt_ign/include/subt_ign/RobotPlatformTypes.hh @@ -27,7 +27,7 @@ const std::map robotPlatformTypes = { {"DS1", 6}, // UAV, prop guards {"DTR", 1}, {"FREYJA", 1}, - {"GAGARIN", 6}, // UAV, prop guards + {"GAGARIN", 1}, // UAV, collision tolerant {"HD2", 1}, {"HOVERMAP", 6}, // UAV, prop guards {"HUSKY", 1}, @@ -43,7 +43,7 @@ const std::map robotPlatformTypes = { {"QAV500", 6}, // UAV, prop guards {"R2", 1}, {"R3", 1}, - {"RMF", 6}, // UAV, prop guards + {"RMF", 1}, // UAV, collision tolerant {"ROCKY", 1}, {"SHAFTER", 8}, // UAV, no prop guards {"SPOT", 1}, @@ -51,7 +51,7 @@ const std::map robotPlatformTypes = { {"X1", 1}, {"X2", 1}, {"X3", 8}, // UAV, no prop guards - {"X6", 8}, // UAV, no prop guards + {"X4", 8}, // UAV, no prop guards {"X500", 8} // UAV, no prop guards }; From a27385736810c8a9cf45a735e3080d8dbb942a1c Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 25 May 2021 16:54:05 -0700 Subject: [PATCH 13/13] Added debug messages Signed-off-by: Nate Koenig --- subt_ign/src/GameLogicPlugin.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 4c8c0a92..aefe15f4 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -1210,6 +1210,7 @@ void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, _ecm.Component(ke.first); if (haltMotionComp && !haltMotionComp->Data()) { + igndbg << "Robot[" << ke.second.robotName << "] has crashed!\n"; haltMotionComp->Data() = true; } }