diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 295dd8e644..4a11ce8f36 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -148,8 +148,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, this->dataPtr->verbose = _sdf->Get("verbose", true).first; // Initial wrenches - auto ptr = const_cast(_sdf.get()); - for (auto elem = ptr->GetElement("persistent"); + for (auto elem = _sdf->FindElement("persistent"); elem != nullptr; elem = elem->GetNextElement("persistent")) { @@ -163,7 +162,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, msg.mutable_entity()->set_name(elem->Get("entity_name")); - auto typeStr = elem->GetElement("entity_type")->Get(); + auto typeStr = elem->FindElement("entity_type")->Get(); if (typeStr == "link") { msg.mutable_entity()->set_type(msgs::Entity::LINK); @@ -182,12 +181,12 @@ void ApplyLinkWrench::Configure(const Entity &_entity, if (elem->HasElement("force")) { msgs::Set(msg.mutable_wrench()->mutable_force(), - elem->GetElement("force")->Get()); + elem->FindElement("force")->Get()); } if (elem->HasElement("torque")) { msgs::Set(msg.mutable_wrench()->mutable_torque(), - elem->GetElement("torque")->Get()); + elem->FindElement("torque")->Get()); } this->dataPtr->OnWrenchPersistent(msg); }