diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 93e504758d..517519566a 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -212,6 +212,15 @@ void DetachableJoint::Configure(const Entity &_entity, this->validConfig = true; } +void DetachableJoint::Reset(const gz::sim::UpdateInfo &/*_info*/, + gz::sim::EntityComponentManager &/*_ecm*/) +{ + // When the physics engine is resetted the detachable joint is removed + // We need to reattach it + this->attachRequested = true; + this->isAttached = false; +} + ////////////////////////////////////////////////// void DetachableJoint::PreUpdate( const UpdateInfo &/*_info*/, @@ -314,9 +323,10 @@ void DetachableJoint::OnDetachRequest(const msgs::Empty &) } GZ_ADD_PLUGIN(DetachableJoint, - System, - DetachableJoint::ISystemConfigure, - DetachableJoint::ISystemPreUpdate) + System, + DetachableJoint::ISystemConfigure, + DetachableJoint::ISystemPreUpdate, + DetachableJoint::ISystemReset) GZ_ADD_PLUGIN_ALIAS(DetachableJoint, "gz::sim::systems::DetachableJoint") diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index e6cb491285..8e5233f411 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -73,7 +73,8 @@ namespace systems class DetachableJoint : public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemReset { /// Documentation inherited public: DetachableJoint() = default; @@ -89,6 +90,10 @@ namespace systems const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) final; + /// Documentation inherited + public: void Reset(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + /// \brief Gazebo communication node. private: transport::Node node;