diff --git a/examples/worlds/dynamic_detachable_joint.sdf b/examples/worlds/dynamic_detachable_joint.sdf
new file mode 100644
index 0000000000..58cc5ebcc8
--- /dev/null
+++ b/examples/worlds/dynamic_detachable_joint.sdf
@@ -0,0 +1,518 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 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
+ 100 100
+
+
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+ 0.031 0.043 0.1 1
+ 0.031 0.043 0.1 1
+ 0.031 0.043 0.1 1
+
+
+
+
+
+
+
+ 0.5 0 0.75 0 0 0
+ false
+
+
+ 40
+
+
+ 0 0 -0.70 0 0 0
+
+
+ 0.3
+ 0.10
+
+
+
+
+ 0 0 -0.70 0 0 0
+
+
+ 0.3
+ 0.10
+
+
+
+ 0.15 0.15 0.15 1
+ 0.15 0.15 0.15 1
+
+
+
+ 0 0 -0.075 0 0 0
+
+
+ 0.1 0.1 1.35
+
+
+
+
+ 0 0 -0.075 0 0 0
+
+
+ 0.1 0.1 1.35
+
+
+
+ 0.15 0.15 0.15 1
+ 0.15 0.15 0.15 1
+
+
+
+
+
+ 0.35 0 0.25 0 0 0
+
+
+
+ 0.7 0.05 0.05
+
+
+
+
+
+
+ 0.7 0.05 0.05
+
+
+
+ 0.15 0.15 0.15 1
+ 0.15 0.15 0.15 1
+
+
+
+
+ base_link
+ arm
+
+
+
+ cmd_vel
+
+
+
+ arm
+ /payload/attach_detach
+ /payload/child_state
+ 0.3
+
+
+
+
+
+ 1.4 0 0 0 0 0
+ false
+
+
+ 0 0 0.375 0 0 0
+
+ 20
+
+
+
+
+ 0.25
+ 0.75
+
+
+
+
+
+
+ 0.25
+ 0.75
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+
+
+ 0 0 0.775 0 0 0
+
+ 5
+
+
+
+
+ 0.5
+ 0.05
+
+
+
+
+
+
+ 0.5
+ 0.05
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+
+
+
+ base
+ turntable
+ 0 0 0.75 0 0 0
+
+ 0 0 1
+
+ -1e16
+ 1e16
+
+
+
+
+ turntable_joint
+ 0.4
+
+
+
+
+
+ 1.400 0.350 0.85 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 0.05 0.05 0.05 1
+ 0.05 0.05 0.05 1
+
+
+
+
+
+ 1.733 0.108 0.85 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 0 0 1 1
+ 0 0 1 1
+
+
+
+
+
+ 1.606 -0.283 0.85 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 0 1 0 1
+ 0 1 0 1
+
+
+
+
+
+ 1.194 -0.283 0.85 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 1 1 0 1
+ 1 1 0 1
+
+
+
+
+
+ 1.067 0.108 0.85 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 1 0 0 1
+ 1 0 0 1
+
+
+
+
+
+
+
+ -0.2 0 0 0 0 0
+ true
+
+
+ 0.30 0 0.35 0 0 0
+
+
+ 0.02 0.6 0.7
+
+
+
+
+ -0.30 0 0.35 0 0 0
+
+
+ 0.02 0.6 0.7
+
+
+
+
+ 0 0.30 0.35 0 0 0
+
+
+ 0.6 0.02 0.7
+
+
+
+
+ 0 -0.30 0.35 0 0 0
+
+
+ 0.6 0.02 0.7
+
+
+
+
+
+ 0.30 0 0.35 0 0 0
+
+
+ 0.02 0.6 0.7
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+ -0.30 0 0.35 0 0 0
+
+
+ 0.02 0.6 0.7
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+ 0 0.30 0.35 0 0 0
+
+
+ 0.6 0.02 0.7
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+ 0 -0.30 0.35 0 0 0
+
+
+ 0.6 0.02 0.7
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+
+
+
diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index 84255b30ff..4127d1db89 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -92,6 +92,7 @@ add_subdirectory(comms_endpoint)
add_subdirectory(contact)
add_subdirectory(camera_video_recorder)
add_subdirectory(detachable_joint)
+add_subdirectory(dynamic_detachable_joint)
add_subdirectory(diff_drive)
add_subdirectory(drive_to_pose_controller)
add_subdirectory(dvl)
diff --git a/src/systems/dynamic_detachable_joint/CMakeLists.txt b/src/systems/dynamic_detachable_joint/CMakeLists.txt
new file mode 100644
index 0000000000..07b74ef1e0
--- /dev/null
+++ b/src/systems/dynamic_detachable_joint/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_add_system(dynamic-detachable-joint
+ SOURCES
+ DynamicDetachableJoint.cc
+ PUBLIC_LINK_LIBS
+ gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
+)
diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc
new file mode 100644
index 0000000000..65405bd6aa
--- /dev/null
+++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc
@@ -0,0 +1,383 @@
+/*
+ * Copyright (C) 2025 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.
+ *
+ * Author: Adarsh Karan K P, Neobotix GmbH
+ *
+ */
+
+#include "DynamicDetachableJoint.hh"
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+
+#include "gz/sim/components/DetachableJoint.hh"
+#include "gz/sim/components/Link.hh"
+#include "gz/sim/components/Model.hh"
+#include "gz/sim/components/Name.hh"
+#include "gz/sim/components/ParentEntity.hh"
+#include "gz/sim/components/Pose.hh"
+
+#include "gz/sim/EntityComponentManager.hh"
+#include "gz/sim/EventManager.hh"
+#include "gz/sim/Model.hh"
+#include "gz/sim/Util.hh"
+
+using namespace gz;
+using namespace sim;
+using namespace systems;
+
+/////////////////////////////////////////////////
+void DynamicDetachableJoint::Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &/*_eventMgr*/)
+{
+ this->model = Model(_entity);
+ if (!this->model.Valid(_ecm))
+ {
+ gzerr << "DynamicDetachableJoint should be attached to a model entity. "
+ << "Failed to initialize." << std::endl;
+ return;
+ }
+
+ if (_sdf->HasElement("parent_link"))
+ {
+ auto parentLinkName = _sdf->Get("parent_link");
+ this->parentLinkEntity = this->model.LinkByName(_ecm, parentLinkName);
+ if (kNullEntity == this->parentLinkEntity)
+ {
+ gzerr << "Link with name " << parentLinkName
+ << " not found in model " << this->model.Name(_ecm)
+ << ". Make sure the parameter 'parent_link' has the "
+ << "correct value. Failed to initialize.\n";
+ return;
+ }
+ }
+ else
+ {
+ gzerr << "'parent_link' is a required parameter for DynamicDetachableJoint. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ // Setup attach distance threshold
+ auto [value, found] = _sdf->Get("attach_distance", this->defaultAttachDistance);
+ if (!found)
+ {
+ gzwarn << "No 'attach_distance' specified in sdf, using default value of "
+ << this->defaultAttachDistance << " meters.\n";
+ }
+ else
+ {
+ gzmsg << "Found 'attach_distance' in sdf: " << value << " meters.\n";
+ }
+
+ this->attachDistance = value;
+ gzmsg << "Final attachDistance set to: " << this->attachDistance << " meters.\n";
+
+ // Setup service
+ // Check if the SDF has a service_name element
+ std::vector serviceNames;
+ if (_sdf->HasElement("service_name"))
+ {
+ // If it does, add it to the list of service names
+ serviceNames.push_back(_sdf->Get("service_name"));
+ }
+ // Add a fallback service name
+ serviceNames.push_back("/model/" + this->model.Name(_ecm) +
+ "/dynamic_detachable_joint/attach_detach");
+
+ // Get the valid service name
+ this->serviceName = validTopic(serviceNames);
+ if (this->serviceName.empty())
+ {
+ gzerr << "No valid service name for DynamicDetachableJoint could be found.\n";
+ return;
+ }
+ gzdbg << "Using service: " << this->serviceName << std::endl;
+
+ // Advertise the service
+ if (!this->node.Advertise(this->serviceName, &DynamicDetachableJoint::OnServiceRequest, this))
+ {
+ gzerr << "Error advertising service [" << this->serviceName << "]" << std::endl;
+ return;
+ }
+
+ // Setup output topic
+ std::vector outputTopics;
+ if (_sdf->HasElement("output_topic"))
+ {
+ outputTopics.push_back(_sdf->Get("output_topic"));
+ }
+
+ outputTopics.push_back("/model/" + this->model.Name(_ecm) +
+ "/dynamic_detachable_joint/state");
+
+ this->outputTopic = validTopic(outputTopics);
+ if (this->outputTopic.empty())
+ {
+ gzerr << "No valid output topics for DynamicDetachableJoint could be found.\n";
+ return;
+ }
+ gzdbg << "Output topic is: " << this->outputTopic << std::endl;
+
+ // Setup publisher for output topic
+ this->outputPub = this->node.Advertise(
+ this->outputTopic);
+ if (!this->outputPub)
+ {
+ gzerr << "Error advertising topic [" << this->outputTopic << "]"
+ << std::endl;
+ return;
+ }
+
+ this->validConfig = true;
+}
+
+//////////////////////////////////////////////////
+void DynamicDetachableJoint::PreUpdate(
+ const UpdateInfo &/*_info*/,
+ EntityComponentManager &_ecm)
+{
+ GZ_PROFILE("DynamicDetachableJoint::PreUpdate");
+ std::lock_guard lock(this->mutex);
+
+ // only allow attaching if child entity is detached
+ if (this->validConfig && !this->isAttached)
+ {
+ // return if attach is not requested.
+ if (!this->attachRequested)
+ {
+ return;
+ }
+ // Look for the child model and link
+ Entity modelEntity{kNullEntity};
+
+ // if child model is the parent model
+ if ("__model__" == this->childModelName)
+ {
+ modelEntity = this->model.Entity();
+ }
+ else
+ {
+ // Querying the ECM for the child model entity
+ modelEntity = _ecm.EntityByComponents(
+ components::Model(), components::Name(this->childModelName));
+ }
+
+ // if child model is not found
+ if (kNullEntity == modelEntity)
+ {
+ gzerr << "Attach Failed. child model [" << this->childModelName
+ << "] could not be found.\n";
+ this->attachRequested = false; // reset attach request
+ return;
+ }
+
+ this->childLinkEntity = _ecm.EntityByComponents(
+ components::Link(),
+ components::ParentEntity(modelEntity),
+ components::Name(this->childLinkName));
+
+ // if child link is not found
+ if (kNullEntity == this->childLinkEntity)
+ {
+ gzerr << "Attach Failed. child link [" << this->childLinkName
+ << "] could not be found.\n";
+ this->attachRequested = false; // reset attach request
+ return;
+ }
+
+ // store the child and parent link poses in the world frame
+ math::Pose3d childPose = gz::sim::worldPose(this->childLinkEntity, _ecm);
+ math::Pose3d parentPose = gz::sim::worldPose(this->parentLinkEntity, _ecm);
+
+ auto dist = childPose.Pos().Distance(parentPose.Pos());
+ gzdbg << "Centre-to-centre distance: " << dist << " m" << std::endl;
+
+ // Check if the child link is within the attach distance
+ if (dist > this->attachDistance)
+ {
+ gzerr << "Attach Failed. Child Link [" << this->childLinkName
+ << "] is too far from parent. Distance: " << dist
+ << "m, threshold: " << this->attachDistance << "m" << std::endl;
+ this->attachRequested = false; // reset attach request
+ return;
+ }
+ // If the child link is within the attach distance, proceed to attach
+ gzmsg << "Attach Success. Child model [" << this->childModelName
+ << "] link [" << this->childLinkName << "] attached to parent link. "
+ << "Distance: " << dist << "m" << std::endl;
+
+ // Attach the models
+ // We do this by creating a detachable joint entity.
+ this->detachableJointEntity = _ecm.CreateEntity();
+
+ // creating the joint
+ _ecm.CreateComponent(
+ this->detachableJointEntity,
+ components::DetachableJoint({this->parentLinkEntity,
+ this->childLinkEntity, "fixed"}));
+ this->attachRequested = false;
+ this->isAttached = true;
+ // Keep track of the attached pair for future validation
+ this->attachedChildModelName = this->childModelName;
+ this->attachedChildLinkName = this->childLinkName;
+ this->PublishJointState(this->isAttached);
+ gzdbg << "Attaching entity: " << this->detachableJointEntity
+ << std::endl;
+ }
+
+ // only allow detaching if child entity is attached
+ if (this->isAttached)
+ {
+ if (this->detachRequested && (kNullEntity != this->detachableJointEntity))
+ {
+ // Detach the models
+ gzmsg << "Detach Success. Child model [" << this->attachedChildModelName
+ << "] link [" << this->attachedChildLinkName << "] detached from parent link." << std::endl;
+ gzdbg << "Removing entity: " << this->detachableJointEntity << std::endl;
+ _ecm.RequestRemoveEntity(this->detachableJointEntity);
+ this->detachableJointEntity = kNullEntity;
+ this->detachRequested = false;
+ this->isAttached = false;
+ // Publish using the last known attached pair, then clear them.
+ this->PublishJointState(this->isAttached);
+ this->attachedChildModelName.clear();
+ this->attachedChildLinkName.clear();
+ }
+ }
+}
+
+//////////////////////////////////////////////////
+bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachRequest &_req,
+ gz::msgs::AttachDetachResponse &_res)
+{
+ GZ_PROFILE("DynamicDetachableJoint::OnServiceRequest");
+ std::lock_guard lock(this->mutex);
+
+ // Check if the request is valid
+ if (_req.child_model_name().empty() || _req.child_link_name().empty() )
+ {
+ _res.set_success(false);
+ _res.set_message("Invalid request: child_model_name and child_link_name must be set.");
+ return true;
+ }
+
+ if (_req.command().empty())
+ {
+ _res.set_success(false);
+ _res.set_message("Invalid request: command must be 'attach' or 'detach'.");
+ return true;
+ }
+
+ // If attach is requested
+ if (_req.command() == "attach")
+ {
+ if (this->isAttached)
+ {
+ _res.set_success(false);
+ _res.set_message("Already attached to child model [" + this->attachedChildModelName +
+ "] at link [" + this->attachedChildLinkName + "].");
+ gzdbg << "Already attached to child model [" << this->attachedChildModelName
+ << "] at link [" << this->attachedChildLinkName << "]" << std::endl;
+ return true;
+ }
+
+ // set the child model and link names from the request
+ this->childModelName = _req.child_model_name();
+ this->childLinkName = _req.child_link_name();
+ this->attachRequested = true;
+ _res.set_success(true);
+ _res.set_message("Attach request accepted for child model [" + this->childModelName +
+ "] at link [" + this->childLinkName + "].");
+ }
+
+ // If detach is requested
+ else if (_req.command() == "detach")
+ {
+ if (!this->isAttached)
+ {
+ _res.set_success(false);
+ _res.set_message(std::string("Detach request received for ")
+ + this->attachedChildModelName + "/" + this->attachedChildLinkName);
+ gzdbg << "Already detached" << std::endl;
+ return true;
+ }
+
+ // Validate that the request matches what is actually attached.
+ const auto &reqModel = _req.child_model_name();
+ const auto &reqLink = _req.child_link_name();
+ if (reqModel != this->attachedChildModelName ||
+ reqLink != this->attachedChildLinkName)
+ {
+ _res.set_success(false);
+ _res.set_message(
+ "Detach rejected: requested [" + reqModel + "] link [" + reqLink +
+ "] but currently attached to [" + this->attachedChildModelName + "] link [" +
+ this->attachedChildLinkName + "]."
+ );
+ gzerr << "Detach rejected: requested [" << reqModel << "] link [" << reqLink
+ << "] but currently attached to [" << this->attachedChildModelName << "] link ["
+ << this->attachedChildLinkName << "]." << std::endl;
+ return true;
+ }
+
+ this->detachRequested = true;
+ _res.set_success(true);
+ _res.set_message("Detach request accepted for child model [" + this->attachedChildModelName +
+ "] at link [" + this->attachedChildLinkName + "].");
+ }
+
+ else
+ {
+ _res.set_success(false);
+ _res.set_message("Invalid command. Use 'attach' or 'detach'.");
+ return true;
+ }
+ return true;
+ }
+
+//////////////////////////////////////////////////
+void DynamicDetachableJoint::PublishJointState(bool attached)
+{
+ gz::msgs::Entity stateMsg;
+ if (attached)
+ {
+ stateMsg.set_id(this->childLinkEntity);
+ stateMsg.set_type(gz::msgs::Entity::LINK);
+ }
+ else
+ {
+ stateMsg.set_id(kNullEntity);
+ stateMsg.set_type(gz::msgs::Entity::NONE);
+ }
+ this->outputPub.Publish(stateMsg);
+}
+
+GZ_ADD_PLUGIN(DynamicDetachableJoint,
+ System,
+ DynamicDetachableJoint::ISystemConfigure,
+ DynamicDetachableJoint::ISystemPreUpdate)
+
+GZ_ADD_PLUGIN_ALIAS(DynamicDetachableJoint,
+ "gz::sim::systems::DynamicDetachableJoint")
diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh
new file mode 100644
index 0000000000..8bee06876d
--- /dev/null
+++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2025 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.
+ *
+ * Author: Adarsh Karan K P, Neobotix GmbH
+ *
+ */
+
+#ifndef GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_
+#define GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_
+
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+
+#include "gz/sim/EntityComponentManager.hh"
+#include "gz/sim/EventManager.hh"
+
+#include "gz/sim/Model.hh"
+#include "gz/sim/System.hh"
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems
+{
+/// \brief A system that dynamically creates detachable joints between models
+/// via a service interface, allowing runtime attachment and detachment of any
+/// model with a parent model that is defined in the SDF/URDF.
+///
+/// Unlike the standard DetachableJoint which requires child model specification
+/// in the SDF, this system accepts child model and link names via service calls.
+/// Hence, it is a more flexible extension of the existing DetachableJoint system.
+///
+/// ## System Parameters
+///
+/// - ``: Name of the link in the parent model to be used in
+/// creating a fixed joint with a link in the child model. Required.
+///
+/// - `` (optional): Maximum distance in meters between parent
+/// and child links to allow attachment. Defaults to 0.1 meters.
+///
+/// - `` (optional): Service name for attach/detach requests.
+/// Defaults to `/model/{model_name}/dynamic_detachable_joint/attach_detach`.
+///
+/// - `` (optional): Topic name for publishing attachment state.
+/// Defaults to `/model/{model_name}/detachable_joint/state`.
+
+ class DynamicDetachableJoint
+ : public System,
+ public ISystemConfigure,
+ public ISystemPreUpdate
+ {
+ /// Documentation inherited
+ public: DynamicDetachableJoint() = default;
+
+ /// Documentation inherited
+ public: void Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &_eventMgr) final;
+
+ /// Documentation inherited
+ public: void PreUpdate(
+ const gz::sim::UpdateInfo &_info,
+ gz::sim::EntityComponentManager &_ecm) final;
+
+ /// \brief Gazebo communication node.
+ private: transport::Node node;
+
+ /// \brief A publisher to send state of the detachment
+ private: transport::Node::Publisher outputPub;
+
+ /// \brief Callback for attach/detach service request
+ /// \param[in] _req Request message containing child model/link names and command
+ /// \param[out] _res Response message with success status and message
+ /// \return Always returns true to indicate message was processed
+ private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req,
+ gz::msgs::AttachDetachResponse &_res);
+
+ /// \brief Helper function to publish the state of the detachment
+ private: void PublishJointState(bool attached);
+
+ /// \brief The model associated with this system.
+ private: Model model;
+
+ /// \brief Name of child model
+ private: std::string childModelName;
+
+ /// \brief Name of attachment link in the child model
+ private: std::string childLinkName;
+
+ /// \brief Variables to keep track of what is currently attached
+ private: std::string attachedChildModelName;
+ private: std::string attachedChildLinkName;
+
+ /// \brief Minimum distance to consider the child link as attached
+ private: double defaultAttachDistance = 0.1;
+
+ /// \brief Distance to consider the child link as attached
+ private: double attachDistance{defaultAttachDistance};
+ /// \brief Service name to be used for attaching or detaching connections
+ private: std::string serviceName;
+
+ /// \brief Topic to be used for publishing detached state
+ private: std::string outputTopic;
+
+ /// \brief Entity of attachment link in the parent model
+ private: Entity parentLinkEntity{kNullEntity};
+
+ /// \brief Entity of attachment link in the child model
+ private: Entity childLinkEntity{kNullEntity};
+
+ /// \brief Entity of the detachable joint created by this system
+ private: Entity detachableJointEntity{kNullEntity};
+
+ /// \brief Whether detachment has been requested
+ private: bool detachRequested{false};
+
+ /// \brief Whether attachment has been requested
+ private: bool attachRequested{false};
+
+ /// \brief Whether child entity is attached
+ private: bool isAttached{false};
+
+ /// \brief Whether all parameters are valid and the system can proceed
+ private: bool validConfig{false};
+
+ /// \brief Mutex to protect access to member variables
+ private: std::mutex mutex;
+ };
+ }
+}
+}
+}
+
+#endif
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 08eea60ed2..4fbf5bac2d 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -20,6 +20,7 @@ set(tests
detachable_joint.cc
diff_drive_system.cc
drive_to_pose_controller_system.cc
+ dynamic_detachable_joint.cc
each_new_removed.cc
entity_erase.cc
entity_system.cc
diff --git a/test/integration/dynamic_detachable_joint.cc b/test/integration/dynamic_detachable_joint.cc
new file mode 100644
index 0000000000..11185718ce
--- /dev/null
+++ b/test/integration/dynamic_detachable_joint.cc
@@ -0,0 +1,585 @@
+/*
+ * Copyright (C) 2019 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.
+ *
+ * Author: Adarsh Karan K P, Neobotix GmbH
+ */
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include "gz/sim/Server.hh"
+#include "test_config.hh"
+
+#include "gz/sim/components/Model.hh"
+#include "gz/sim/components/Name.hh"
+#include "gz/sim/components/Pose.hh"
+
+#include "../helpers/Relay.hh"
+#include "../helpers/EnvTestFixture.hh"
+
+using namespace gz;
+using namespace sim;
+
+/// \brief Test DynamicDetachableJoint system
+class DynamicDetachableJointTest : public InternalFixture<::testing::Test>
+{
+ public: void StartServer(const std::string &_sdfFile)
+ {
+ ServerConfig serverConfig;
+ serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + _sdfFile);
+ this->server = std::make_unique(serverConfig);
+
+ EXPECT_FALSE(this->server->Running());
+ EXPECT_FALSE(*this->server->Running(0));
+ }
+
+ public: std::unique_ptr server;
+};
+
+// See https://github.com/gazebosim/gz-sim/issues/1175 for GZ_UTILS_TEST_DISABLED_ON_WIN32(AttachDetach)
+
+/////////////////////////////////////////////////
+// TEST 1
+// "AttachDetach"
+// This test checks the following:
+// 1. attach when within threshold → child (red_cube) follows parent, meaning when the rod moves up (+Z axis) the child moves along with it
+// 2. detach → child free-falls due to gravity
+TEST_F(DynamicDetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AttachDetach))
+{
+ using namespace std::chrono_literals;
+
+ // load the test world
+ this->StartServer("/test/worlds/dynamic_detachable_joint.sdf");
+
+ // A lambda that takes a model name and a mutable reference to a vector of
+ // poses and returns another lambda that can be passed to
+ // `Relay::OnPostUpdate`.
+ auto poseRecorder =
+ [](const std::string &_modelName, std::vector &_poses)
+ {
+ return [&, _modelName](const UpdateInfo &,
+ const EntityComponentManager &_ecm)
+ {
+ _ecm.Each(
+ [&](const Entity &_entity, const components::Model *,
+ const components::Name *_name,
+ const components::Pose *_pose) -> bool
+ {
+ if (_name->Data() == _modelName)
+ {
+ EXPECT_NE(kNullEntity, _entity);
+ _poses.push_back(_pose->Data());
+ }
+ return true;
+ });
+ };
+ };
+
+ // the arm (aka "rod") is the parent, the cube is the child
+ std::vector rodPoses, cubePoses;
+ test::Relay rodRelay, cubeRelay;
+ rodRelay.OnPostUpdate(poseRecorder("rod", rodPoses));
+ cubeRelay.OnPostUpdate(poseRecorder("red_cube", cubePoses));
+
+ this->server->AddSystem(rodRelay.systemPtr);
+ this->server->AddSystem(cubeRelay.systemPtr);
+
+ // Short wait time to ensure everything is initialized
+ const std::size_t waitIters{20};
+ this->server->Run(true, waitIters, false);
+ ASSERT_EQ(waitIters, cubePoses.size());
+ ASSERT_EQ(waitIters, rodPoses.size());
+ gzdbg << "[startup_wait] rod z=" << rodPoses.back().Pos().Z()
+ << ", cube z=" << cubePoses.back().Pos().Z() << std::endl;
+
+ transport::Node node;
+
+ // "attach" red_cube(which is on the ground) to the "rod" using the DynamicDetachableJoint service
+ {
+ msgs::AttachDetachRequest req;
+ req.set_child_model_name("red_cube");
+ req.set_child_link_name("link");
+ req.set_command("attach");
+ gzdbg << "[attach] request: model=red_cube link=link" << std::endl;
+
+ msgs::AttachDetachResponse rep;
+ bool result_1 = false;
+ bool outcome_1 = node.Request("/payload/attach_detach",
+ req, 1000u, rep, result_1);
+
+ // verify service call succeeded
+ ASSERT_TRUE(outcome_1);
+ ASSERT_TRUE(result_1);
+ ASSERT_TRUE(rep.success()) << rep.message();
+ gzdbg << "[attach] ok" << std::endl;
+ }
+
+ // Wait
+ {
+ // Reset poses
+ cubePoses.clear();
+ rodPoses.clear();
+
+ const std::size_t iters{60};
+ this->server->Run(true, iters, false);
+
+ // Verify if we recorded expected number of poses
+ ASSERT_EQ(iters, cubePoses.size());
+ ASSERT_EQ(iters, rodPoses.size());
+
+ // Additional check to ensure the red_cube is rigidly attached to the rod by comparing relative pose
+ auto rel0 = cubePoses.front().Pos() - rodPoses.front().Pos();
+ auto rel1 = cubePoses.back().Pos() - rodPoses.back().Pos();
+ EXPECT_LT((rel1 - rel0).Length(), 5e-4);
+ gzdbg << "[attached-idle] rel Δ=" << (rel1 - rel0).Length()
+ << " (expect ~0)" << std::endl;
+ }
+
+ // "lift" the rod up in Z so the attached red_cube is above ground
+ {
+ gzdbg << "[lift] moving rod from z 0.14m to 0.35m" << std::endl;
+ msgs::Pose poseReq;
+ poseReq.set_name("rod");
+ // keep x,y (same as sdf) and update only z
+ auto *pos = poseReq.mutable_position();
+ pos->set_x(0.30); pos->set_y(0.0); pos->set_z(0.35);
+ auto *ori = poseReq.mutable_orientation();
+ ori->set_x(0); ori->set_y(0); ori->set_z(0); ori->set_w(1);
+
+ msgs::Boolean rep;
+ bool result_2 = false;
+ bool outcome_2 = node.Request("/world/dynamic_detachable_joint_test/set_pose",
+ poseReq, 1000u, rep, result_2);
+
+ // verify service call succeeded
+ ASSERT_TRUE(outcome_2);
+ ASSERT_TRUE(result_2);
+ ASSERT_TRUE(rep.data());
+ gzdbg << "[lift] pose service ok" << std::endl;
+ }
+
+ // After lifting, child follows upward along with the parent rod
+ // this causes it's Z position to change
+ double pos_before_lift = cubePoses.back().Pos().Z();
+ {
+ cubePoses.clear();
+ rodPoses.clear();
+ const std::size_t iters{50};
+ this->server->Run(true, iters, false);
+ ASSERT_EQ(iters, cubePoses.size());
+ ASSERT_EQ(iters, rodPoses.size());
+
+ // Track how much the red_cube moved up
+ auto movedZ = cubePoses.back().Pos().Z() - pos_before_lift;
+ EXPECT_GT(movedZ, 0.15); // followed rod upwards
+ gzdbg << "[after-lift] red_cube Δz=" << movedZ << " (expect ~0.21)" << std::endl;
+
+ // Additional check to ensure the red_cube is rigidly attached to the rod by comparing relative pose
+ auto rel0 = cubePoses.front().Pos() - rodPoses.front().Pos();
+ auto rel1 = cubePoses.back().Pos() - rodPoses.back().Pos();
+ EXPECT_LT((rel1 - rel0).Length(), 5e-4);
+ }
+
+ // "detach" and verify free fall of the red_cube
+ {
+ gzdbg << "[detach] request: model=red_cube link=link" << std::endl;
+ msgs::AttachDetachRequest req;
+ req.set_child_model_name("red_cube");
+ req.set_child_link_name("link");
+ req.set_command("detach");
+
+ msgs::AttachDetachResponse rep;
+ bool result_3 = false;
+ bool outcome_3 = node.Request("/payload/attach_detach",
+ req, 1000u, rep, result_3);
+
+ // verify service call succeeded
+ ASSERT_TRUE(outcome_3);
+ ASSERT_TRUE(result_3);
+ ASSERT_TRUE(rep.success()) << rep.message();
+ gzdbg << "[detach] ok" << std::endl;
+ }
+
+ // The ideal distance an object falls under gravity is given as d = 0.5 * g * t^2
+ // Let it fall for about 150 ms, then check if dz (z distance travelled) > d
+ {
+ cubePoses.clear();
+ const std::size_t iters{150};
+ this->server->Run(true, iters, false);
+ ASSERT_EQ(iters, cubePoses.size());
+
+ const double dt = 0.001; // 1 ms from the test world
+ const double t = static_cast(iters - 1) * dt; // N poses give N-1 integration intervals
+ const double d = 0.5 * 9.8 * t * t;
+ const double dz = cubePoses.front().Pos().Z() - cubePoses.back().Pos().Z();
+
+ // Check if the red_cube fell "at least" the ideal distance
+ EXPECT_GT(dz, d);
+ gzdbg << "[free-fall] dz=" << dz << " > " << d
+ << " (t=" << t << "s, N=" << iters << ")" << std::endl;
+ }
+}
+
+/////////////////////////////////////////////////
+// TEST 2
+// "OutOfRange"
+// This test checks the following:
+// 1. move the parent (rod) far enough so that the center-to-center distance
+// to the red_cube exceeds the configured attach_distance (0.1 m)
+// 2. call "attach" → service accepts the request (rep.success() == true);
+// 3. verify the target child "red_cube" stayed on the ground (~z = 0.05)
+TEST_F(DynamicDetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OutOfRange))
+{
+ using namespace std::chrono_literals;
+
+ this->StartServer("/test/worlds/dynamic_detachable_joint.sdf");
+
+ // A lambda that records poses for a named model
+ auto poseRecorder =
+ [](const std::string &_name, std::vector &_poses)
+ {
+ return [&, _name](const UpdateInfo &, const EntityComponentManager &_ecm)
+ {
+ _ecm.Each(
+ [&](const Entity &, const components::Model *,
+ const components::Name *n, const components::Pose *p) -> bool
+ {
+ if (n->Data() == _name) _poses.push_back(p->Data());
+ return true;
+ });
+ };
+ };
+
+ std::vector rodPoses, cubePoses;
+ test::Relay rodRelay, cubeRelay;
+ rodRelay.OnPostUpdate(poseRecorder("rod", rodPoses));
+ cubeRelay.OnPostUpdate(poseRecorder("red_cube", cubePoses));
+
+ this->server->AddSystem(rodRelay.systemPtr);
+ this->server->AddSystem(cubeRelay.systemPtr);
+
+ // Short wait time to ensure everything is initialized
+ const std::size_t waitIters{20};
+ this->server->Run(true, waitIters, false);
+ ASSERT_EQ(waitIters, rodPoses.size());
+ ASSERT_EQ(waitIters, cubePoses.size());
+ gzdbg << "[startup_wait] rod z=" << rodPoses.back().Pos().Z()
+ << ", red_cube z=" << cubePoses.back().Pos().Z() << std::endl;
+
+ // Move rod high enough so center distance > 0.1 m (attach_distance).
+ // red_cube remains at center z = 0.05m and set rod z to 0.50m → gap = 0.45 m.
+ transport::Node node;
+ {
+ msgs::Pose poseReq;
+ poseReq.set_name("rod");
+ auto *pos = poseReq.mutable_position();
+ pos->set_x(0.30); pos->set_y(0.0); pos->set_z(0.50);
+ auto *ori = poseReq.mutable_orientation();
+ ori->set_x(0); ori->set_y(0); ori->set_z(0); ori->set_w(1);
+
+ msgs::Boolean rep;
+ bool result_1 = false;
+ bool outcome_1 = node.Request("/world/dynamic_detachable_joint_test/set_pose",
+ poseReq, 1000u, rep, result_1);
+
+ // verify service call succeeded
+ ASSERT_TRUE(outcome_1);
+ ASSERT_TRUE(result_1);
+ ASSERT_TRUE(rep.data());
+ gzdbg << "[lift-far] pose service ok (rod z=0.50)" << std::endl;
+ }
+ // Let the pose update apply before attempting attach
+ this->server->Run(true, 5, false);
+
+ // Additional check to ensure if gap is actually larger than attach_distance (0.1 m)
+ {
+ ASSERT_FALSE(rodPoses.empty());
+ ASSERT_FALSE(cubePoses.empty());
+ const double gap = std::abs(rodPoses.back().Pos().Z() - cubePoses.back().Pos().Z());
+ ASSERT_GT(gap, 0.1 + 1e-3);
+ }
+
+ // Attempt to "attach" → request is processed, but attach is rejected in PreUpdate of the plugin
+ // Error is logged if out of range, and we verify by poses that nothing attached
+ {
+ msgs::AttachDetachRequest req;
+ req.set_child_model_name("red_cube");
+ req.set_child_link_name("link");
+ req.set_command("attach");
+
+ msgs::AttachDetachResponse rep;
+ bool result_2 = false;
+ bool outcome_2 = node.Request("/payload/attach_detach",
+ req, 1000u, rep, result_2);
+
+ // verify service call succeeded
+ ASSERT_TRUE(outcome_2);
+ ASSERT_TRUE(result_2);
+ gzdbg << "[attach-out-of-range] request processed and an error is logged if out of range"
+ << std::endl;
+ }
+
+ // Additional check to ensure red_cube is still on the ground
+ // red_cube should still be on the ground (~0.05)
+ {
+ cubePoses.clear();
+ const std::size_t iters{40};
+ this->server->Run(true, iters, false);
+ ASSERT_EQ(iters, cubePoses.size());
+ EXPECT_NEAR(cubePoses.back().Pos().Z(), 0.05, 1e-2);
+ gzdbg << "[verify-ground] red_cube z=" << cubePoses.back().Pos().Z()
+ << " (expect ~0.05)" << std::endl;
+ }
+}
+
+/////////////////////////////////////////////////
+// TEST 3
+// "MultipleChildrenSequential"
+// This test checks the following
+// 1. attach lift detach each target child in turn (red_cube, green_cube, blue_cube)
+// 2. only the attached child follows the parent rod when it moves up (+Z axis)
+// 3. after detaching the target child free-falls due to gravity
+// 4. non-target children stay on the ground (~z = 0.05)
+TEST_F(DynamicDetachableJointTest,
+ GZ_UTILS_TEST_DISABLED_ON_WIN32(MultipleChildrenSequential))
+{
+ using namespace std::chrono_literals;
+
+ this->StartServer("/test/worlds/dynamic_detachable_joint.sdf");
+
+ // Record poses for rod and all 3 cubes
+ auto poseRecorder =
+ [](const std::string &_name, std::vector &_poses)
+ {
+ return [&, _name](const UpdateInfo &, const EntityComponentManager &_ecm)
+ {
+ _ecm.Each(
+ [&](const Entity &, const components::Model *,
+ const components::Name *n, const components::Pose *p) -> bool
+ {
+ if (n->Data() == _name) _poses.push_back(p->Data());
+ return true;
+ });
+ };
+ };
+
+ std::vector rodPoses, redPoses, greenPoses, bluePoses;
+ test::Relay rodRelay, redRelay, greenRelay, blueRelay;
+ rodRelay.OnPostUpdate(poseRecorder("rod", rodPoses));
+ redRelay.OnPostUpdate(poseRecorder("red_cube", redPoses));
+ greenRelay.OnPostUpdate(poseRecorder("green_cube", greenPoses));
+ blueRelay.OnPostUpdate(poseRecorder("blue_cube", bluePoses));
+
+ this->server->AddSystem(rodRelay.systemPtr);
+ this->server->AddSystem(redRelay.systemPtr);
+ this->server->AddSystem(greenRelay.systemPtr);
+ this->server->AddSystem(blueRelay.systemPtr);
+
+ // wait to ensure everything is initialized
+ const std::size_t waitIters{20};
+ this->server->Run(true, waitIters, false);
+ ASSERT_EQ(waitIters, rodPoses.size());
+ ASSERT_EQ(waitIters, redPoses.size());
+ ASSERT_EQ(waitIters, greenPoses.size());
+ ASSERT_EQ(waitIters, bluePoses.size());
+
+ transport::Node node;
+
+ struct Target { const char *name; double x; std::vector* poses; };
+ std::array targets{{
+ {"red_cube", 0.30, &redPoses},
+ {"green_cube", 0.10, &greenPoses},
+ {"blue_cube", 0.50, &bluePoses},
+ }};
+
+ auto setRodPose = [&](double x, double z)
+ {
+ msgs::Pose poseReq;
+ poseReq.set_name("rod");
+ auto *pos = poseReq.mutable_position();
+ pos->set_x(x); pos->set_y(0.0); pos->set_z(z);
+ auto *ori = poseReq.mutable_orientation();
+ ori->set_x(0); ori->set_y(0); ori->set_z(0); ori->set_w(1);
+
+ msgs::Boolean rep;
+ bool result_pose = false;
+ bool outcome_pose = node.Request("/world/dynamic_detachable_joint_test/set_pose",
+ poseReq, 1000u, rep, result_pose);
+
+ // verify service call succeeded
+ ASSERT_TRUE(outcome_pose);
+ ASSERT_TRUE(result_pose);
+ ASSERT_TRUE(rep.data());
+ };
+
+ auto nearGround = [](double z){ return std::abs(z - 0.05) < 1e-2; };
+
+ for (const auto &target : targets)
+ {
+ // Move rod above target within attach_distance (0.1 m)
+ setRodPose(target.x, 0.14);
+ this->server->Run(true, 5, false);
+
+ // "attach" target child to the rod using the service
+ {
+ gzdbg << "[attach] request: model=" << target.name << " link=link" << std::endl;
+
+ msgs::AttachDetachRequest req;
+ req.set_child_model_name(target.name);
+ req.set_child_link_name("link");
+ req.set_command("attach");
+
+ msgs::AttachDetachResponse rep;
+ bool result_attach = false;
+ bool outcome_attach = node.Request("/payload/attach_detach",
+ req, 1000u, rep, result_attach);
+
+ ASSERT_TRUE(outcome_attach);
+ ASSERT_TRUE(result_attach);
+ ASSERT_TRUE(rep.success()) << rep.message();
+ gzdbg << "[attach] ok target=" << target.name << std::endl;
+ }
+
+ // Step and verify
+ // 1. the child is rigidly attached to parent
+ // 2. other target cubes stay on ground
+ {
+ rodPoses.clear(); redPoses.clear(); greenPoses.clear(); bluePoses.clear();
+ const std::size_t iters{30};
+ this->server->Run(true, iters, false);
+
+ ASSERT_EQ(iters, rodPoses.size());
+ ASSERT_EQ(iters, redPoses.size());
+ ASSERT_EQ(iters, greenPoses.size());
+ ASSERT_EQ(iters, bluePoses.size());
+
+ const auto &v = *target.poses;
+ auto rel0 = v.front().Pos() - rodPoses.front().Pos();
+ auto rel1 = v.back().Pos() - rodPoses.back().Pos();
+ EXPECT_LT((rel1 - rel0).Length(), 5e-4);
+
+ if (target.name != std::string("red_cube"))
+ {
+ EXPECT_TRUE(nearGround(redPoses.back().Pos().Z()));
+ }
+ if (target.name != std::string("green_cube"))
+ {
+ EXPECT_TRUE(nearGround(greenPoses.back().Pos().Z()));
+ }
+ if (target.name != std::string("blue_cube"))
+ {
+ EXPECT_TRUE(nearGround(bluePoses.back().Pos().Z()));
+ }
+ }
+
+ // "lift" the rod to z 0.35 and verify only target follows upward
+ double pos_before_lift = target.poses->back().Pos().Z();
+ setRodPose(target.x, 0.35);
+ {
+ rodPoses.clear(); redPoses.clear(); greenPoses.clear(); bluePoses.clear();
+ const std::size_t iters{50};
+ this->server->Run(true, iters, false);
+
+ ASSERT_EQ(iters, rodPoses.size());
+ ASSERT_EQ(iters, redPoses.size());
+ ASSERT_EQ(iters, greenPoses.size());
+ ASSERT_EQ(iters, bluePoses.size());
+
+ double movedZ = target.poses->back().Pos().Z() - pos_before_lift;
+ EXPECT_GT(movedZ, 0.15);
+
+ if (target.name != std::string("red_cube"))
+ {
+ EXPECT_TRUE(nearGround(redPoses.back().Pos().Z()));
+ }
+ if (target.name != std::string("green_cube"))
+ {
+ EXPECT_TRUE(nearGround(greenPoses.back().Pos().Z()));
+ }
+ if (target.name != std::string("blue_cube"))
+ {
+ EXPECT_TRUE(nearGround(bluePoses.back().Pos().Z()));
+ }
+ }
+
+ // "detach" the target and verify free-fall
+ {
+ gzdbg << "[detach] request: model=" << target.name << " link=link" << std::endl;
+
+ msgs::AttachDetachRequest req;
+ req.set_child_model_name(target.name);
+ req.set_child_link_name("link");
+ req.set_command("detach");
+
+ msgs::AttachDetachResponse rep;
+ bool result_detach = false;
+ bool outcome_detach = node.Request("/payload/attach_detach",
+ req, 1000u, rep, result_detach);
+
+ ASSERT_TRUE(outcome_detach);
+ ASSERT_TRUE(result_detach);
+ ASSERT_TRUE(rep.success()) << rep.message();
+ gzdbg << "[detach] ok target=" << target.name << std::endl;
+ }
+ {
+ rodPoses.clear(); redPoses.clear(); greenPoses.clear(); bluePoses.clear();
+ const std::size_t iters{150};
+ this->server->Run(true, iters, false);
+
+ ASSERT_EQ(iters, redPoses.size());
+ ASSERT_EQ(iters, greenPoses.size());
+ ASSERT_EQ(iters, bluePoses.size());
+
+ const double dt = 0.001; // 1 ms from the test world
+ const double t = static_cast(iters - 1) * dt; // N poses give N-1 integration intervals
+ const double d = 0.5 * 9.8 * t * t;
+ const auto &v = *target.poses;
+ const double dz = v.front().Pos().Z() - v.back().Pos().Z();
+ EXPECT_GT(dz, d);
+
+ if (target.name != std::string("red_cube"))
+ {
+ EXPECT_TRUE(nearGround(redPoses.back().Pos().Z()));
+ }
+ if (target.name != std::string("green_cube"))
+ {
+ EXPECT_TRUE(nearGround(greenPoses.back().Pos().Z()));
+ }
+ if (target.name != std::string("blue_cube"))
+ {
+ EXPECT_TRUE(nearGround(bluePoses.back().Pos().Z()));
+ }
+ }
+
+ // Let the detached target land then reset rod for next target
+ {
+ const std::size_t landIters{350};
+ this->server->Run(true, landIters, false);
+ EXPECT_NEAR(target.poses->back().Pos().Z(), 0.05, 1e-2);
+ }
+
+ setRodPose(target.x, 0.14);
+ this->server->Run(true, 5, false);
+ }
+}
diff --git a/test/worlds/dynamic_detachable_joint.sdf b/test/worlds/dynamic_detachable_joint.sdf
new file mode 100644
index 0000000000..c1217fa4c3
--- /dev/null
+++ b/test/worlds/dynamic_detachable_joint.sdf
@@ -0,0 +1,164 @@
+
+
+
+
+
+
+
+ 0.001
+ 0
+
+
+
+
+
+
+ true
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+
+
+
+
+
+ 0.30 0 0.14 0 0 0
+
+ false
+
+ 1.0
+
+
+
+
+ 0.6 0.05 0.05
+
+
+
+
+
+
+ 0.6 0.05 0.05
+
+
+
+ 0.15 0.15 0.15 1
+ 0.15 0.15 0.15 1
+
+
+
+
+
+ arm
+ /payload/attach_detach
+ /payload/child_state
+ 0.1
+
+
+
+
+
+ 0.10 0 0.05 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 0 1 0 1
+ 0 1 0 1
+
+
+
+
+
+
+ 0.30 0 0.05 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 1 0 0 1
+ 1 0 0 1
+
+
+
+
+
+
+ 0.50 0 0.05 0 0 0
+
+
+ 0.2
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ 0.1 0.1 0.1
+
+
+
+ 0 0 1 1
+ 0 0 1 1
+
+
+
+
+
+