Skip to content

Conversation

AdarshKaran
Copy link

🎉 New feature

Closes #2362

Summary

Created a dynamic_detachable_joint system as an extension of the detachable joint system where the childModelName and childLinkName can now be passed through a gz service or a ROS2 service (a bridge has also been added for this). A command: attach or detach can be specified in the service call. I have also added an attachDistance option that allows you to specify how far the child should be to be considered attached.

Most of the code is based on the already existing DetachableJoint Plugin logic

Test it

Adding the plugin to robot model URDF/SDF

    <gazebo>
      <plugin filename="gz-sim-dynamic-detachable-joint-system"
            name="gz::sim::systems::DynamicDetachableJoint">
        <parent_link>robotiq_85_left_finger_tip_link</parent_link>
        <service_name>/payload/attach_detach</service_name>
        <output_topic>/child_state</output_topic>
        <attach_distance>0.25</attach_distance>
      </plugin>
    </gazebo>

GZ Service call command

# Attach
gz service -s /payload/attach_detach \
    --reqtype gz.msgs.AttachDetachRequest \
    --reptype gz.msgs.AttachDetachResponse \
    --timeout 3000 \
    --req 'child_model_name: "cube", 
           child_link_name: "link", 
           command: "attach"'

# Detach
gz service -s /payload/attach_detach \
    --reqtype gz.msgs.AttachDetachRequest \
    --reptype gz.msgs.AttachDetachResponse \
    --timeout 2000 \
    --req 'child_model_name: "cube", 
           child_link_name: "link", 
           command: "detach"'

ROS2 Service call command

# Attach
ros2 service call /payload/attach_detach \
    ros_gz_interfaces/srv/AttachDetach \
    "{child_model_name: 'cube', 
      child_link_name: 'link', 
      command: 'attach'}"

# Detach  
ros2 service call /payload/attach_detach \
    ros_gz_interfaces/srv/AttachDetach \
    "{child_model_name: 'cube', 
      child_link_name: 'link', 
      command: 'detach'}"

Checklist

  • Signed all commits for DCO
  • Added tests
  • Added example and/or tutorial
  • Updated documentation (as needed)
  • Updated migration guide (as needed)
  • Consider updating Python bindings (if the library has them)
  • codecheck passed (See contributing)
  • All tests passed (See test coverage)
  • While waiting for a review on your PR, please help review another open pull request to support the maintainers
  • Was GenAI used to generate this PR? If so, make sure to add "Generated-by" to your commits. (See this policy for more info.) Yes. Chat GPT in copilot was used partly for help with formatting and for the first draft of some of the comments.

Generated-by: Remove this if GenAI was not used.

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by and Generated-by messages.

Copy link
Contributor

@ahcorde ahcorde left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It will be great to add some details in the header file as documentation

using namespace systems;

/////////////////////////////////////////////////
void DynamicDetachableJoint::Configure(const Entity &_entity,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

include ""

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

hi! could you please clarify what header is missing here?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

https://github.com/gazebosim/gz-sim/blob/361af45888b96fd860f56c9c2c785fe61e339113/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh#L40C1-L64C64

I have added these comments in the header file similar to what was done in the existing DetachableJoint.hh plugin. If you could clarify if i need to add more detailed comments with usage example in the .cc source file above the Configure function then I shall do that.

Comment on lines +47 to +48
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

include header for EntityComponentManager and EventManager


if (_sdf->HasElement("parent_link"))
{
auto parentLinkName = _sdf->Get<std::string>("parent_link");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

include <string>

Comment on lines 22 to 27
#include <gz/plugin/Register.hh>
#include <gz/transport/Node.hh>

#include <gz/common/Profiler.hh>

#include <sdf/Element.hh>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

alphabetize

Comment on lines 112 to 113
gzdbg << "using service: "
<< this->serviceName << std::endl;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
gzdbg << "using service: "
<< this->serviceName << std::endl;
gzdbg << "Using service: " << this->serviceName << std::endl;

Comment on lines 240 to 241
gzwarn << "Child Model " << this->childModelName
<< " could not be found.\n";
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
gzwarn << "Child Model " << this->childModelName
<< " could not be found.\n";
gzwarn << "Child Model " << this->childModelName
<< " could not be found.\n";

Comment on lines 302 to 303
_res.set_message("Attached to child model " + this->childModelName +
" at link " + this->childLinkName + ".");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
_res.set_message("Attached to child model " + this->childModelName +
" at link " + this->childLinkName + ".");
_res.set_message("Attached to child model " + this->childModelName +
" at link " + this->childLinkName + ".");

Comment on lines +77 to +78
EntityComponentManager &_ecm,
EventManager &_eventMgr) final;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

include headers for EntityComponentManager and EventManager

Comment on lines 95 to 96
private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req,
gz::msgs::AttachDetachResponse &_res);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req,
gz::msgs::AttachDetachResponse &_res);
private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req,
gz::msgs::AttachDetachResponse &_res);

private: Entity detachableJointEntity{kNullEntity};

/// \brief Whether detachment has been requested
private: std::atomic<bool> detachRequested{false};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

include <atomic>

@github-project-automation github-project-automation bot moved this from Inbox to In review in Core development Sep 12, 2025
@arjo129
Copy link
Contributor

arjo129 commented Sep 13, 2025

Thanks for this contribution @AdarshKaran! Please sign off your commits. You can see how here: https://github.com/gazebosim/gz-sim/pull/3086/checks?check_run_id=50152759632

Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
…, added clearer loggin

Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
@AdarshKaran AdarshKaran force-pushed the plugin/dynamic_detachable_joint branch from 361af45 to 33c1fd6 Compare September 17, 2025 07:54
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
@AdarshKaran
Copy link
Author

I have addressed the comments. Once my doubt regarding the documentation comment is clarified, I'll add that commit.

Copy link
Contributor

@arjo129 arjo129 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the contribution! This can be super useful.

I'm being a bit picky, but:
(1) We should at least have an integration test for this system.
(2) Check indentation + Address the comments I've left for you.
(3) You may need to use a lock instead of atomics as we are configuring multiple parameters at once.

<< std::endl;
}
else
{
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Knit: I recommend applying the error-first pattern (also called "happy path" programming) here. By checking for and returning early on error conditions, we can significantly reduce code nesting and enhance the overall readability.

Reference: https://preslav.me/2023/09/22/ditch-that-else/

<< " could not be found.\n";
}
}
else if (!this->suppressChildWarning)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Knit: Why do we need this. Can't we just rely on gazebo's verbosity level?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I actually followed the same pattern as the existing Detachable Joint cc for this.
https://github.com/gazebosim/gz-sim/blob/5dfe0ad1e6e01b8ddb915981e6b3da2efd3cf673/src/systems/detachable_joint/DetachableJoint.cc#L306C1-L311C2
Do we still keep it or remove it for now?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, that plugin does need a refactor.

Copy link
Author

@AdarshKaran AdarshKaran Sep 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So, do you suggest to remove suppressChildWarning and instead just have

    // if child model is not found
    if (kNullEntity == modelEntity)
    {
      gzdbg << "Child Model " << this->childModelName
             << " could not be found.\n";
      return;
    }

gzdbg instead of gzwarn?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No keep it as gzwarn just dont bother with the suppress child option.

if (this->isAttached)
{
_res.set_success(false);
_res.set_message("Already attached to child model " + this->attachedChildModelName +
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Knit: Indentation issue

gz::msgs::StringMsg detachedStateMsg;
if (attached)
{
detachedStateMsg.set_data("attached to " +
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why are we publishing a custom message with custom semantics? I'm not sure we should publish this at all.

Copy link
Author

@AdarshKaran AdarshKaran Sep 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Contributor

@arjo129 arjo129 Sep 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

DetachableJoint.cc publishes only 2 types of outputs: "attached" or "detached" (if I were redesigning this I'd just publish a bool), in your case you publish "attached to ..." . If I were writing a parser to interact with your software I'd have to look up this magic string. Its much better to either publish an empty string or just the linked name so end users dont have to write custom parsers. Alternatively publish the entity/knullentity that the object is attached to.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's a good point. I did not think from that angle. But how exactly would we be able to differentiate bw attached and detached state in the case of an empty string or which model it is attached to in case of link name(when different models have same link names)?
So maybe in that case, using the entity/knullentity like you said is a better option?

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);
}

and in the topic echo

id: 29
type: LINK


id: 25
type: LINK


when i do

ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_2', child_link_name: 'body', command: 'attach'}"
ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_2', child_link_name: 'body', command: 'detach'}"
ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_1', child_link_name: 'body', command: 'attach'}"
ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_1', child_link_name: 'body', command: 'detach'}"

so basically we could extract the model name from the link entity and in detach state we publish a knullentity so it's empty.
does this change seem good?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

#include <gz/msgs/entity.pb.h>

Just this extra dependency though

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah that seems reasonable to avoid any sense of ambiguity.

}

//////////////////////////////////////////////////
void DynamicDetachableJoint::OnAttachRequest(const msgs::Empty &)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why are we passing an empty message for no reason? We don't actually need this method.

Copy link
Author

@AdarshKaran AdarshKaran Sep 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I actually kept the two function just to like separate concerns and to also keep it extensible in the future. But, if its better to simplify it then I'll remove it for now. Like you said it actually makes more sense to remove since there is an unwanted dependency on the empty proto msg.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suggest removing it for now

this->childModelName = _req.child_model_name();
this->childLinkName = _req.child_link_name();
this->OnAttachRequest(msgs::Empty());
_res.set_success(true);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

knit: Indentation

// set the child model and link names from the request
this->childModelName = _req.child_model_name();
this->childLinkName = _req.child_link_name();
this->OnAttachRequest(msgs::Empty());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

directly assign the this->attachRequested = true; without calling the method,

}

// set the child model and link names from the request
this->childModelName = _req.child_model_name();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Knit: I don't think atomics are sufficient here. You do need to use a lock to guarantee data consistency between
this->childModelName, this->childModelName and this->attachRequested.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Regarding mutex locks, I haven't worked much with them before so please guide me here.

@@ -158,6 +159,8 @@ void DynamicDetachableJoint::PreUpdate(
   EntityComponentManager &_ecm)
 {
   GZ_PROFILE("DynamicDetachableJoint::PreUpdate");
+  std::lock_guard<std::mutex> lock(this->mutex);
+
   // only allow attaching if child entity is detached
   if (this->validConfig && !this->isAttached)
   {
@@ -265,6 +268,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques
                                               gz::msgs::AttachDetachResponse &_res)
 {
   GZ_PROFILE("DynamicDetachableJoint::OnServiceRequest");
+  std::lock_guard<std::mutex> lock(this->mutex);
 
   // Check if the request is valid
   if (_req.child_model_name().empty() || _req.child_link_name().empty() )

is it right to add them to the top of PreUpdate loop?

and for the header file, I have removed atomic

 
     /// \brief Whether detachment has been requested
-    private: std::atomic<bool> detachRequested{false};
+    private: bool detachRequested{false};
 
     /// \brief Whether attachment has been requested
-    private: std::atomic<bool> attachRequested{false};
+    private: bool attachRequested{false};
 
     /// \brief Whether child entity is attached
-    private: std::atomic<bool> isAttached{false};
+    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;
   };
   }
 }

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah this approach will work. Although it may be a bit heavy as ideally you'd only lock for the time period which you need access to the variable for. One option is to create a scope within preupdate, lock that scope then copy out the necessary details. For our purposes, I don't think it matters.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

alright then. Thank you! I'll push these changes

Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
@arjo129
Copy link
Contributor

arjo129 commented Sep 25, 2025

I have one more ask that falls outside of the integration test. Please add a demo world of some form so I/users can test it.

Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
_res.set_success(false);
_res.set_message("Already attached to child model " + this->attachedChildModelName +
" at link " + this->attachedChildLinkName + ".");
gzdbg << "Already attached" << std::endl;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Might be worth being a bit more descriptive with the error message. Mention the linkName.

}

// set the child model and link names from the request
this->childModelName = _req.child_model_name();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah this approach will work. Although it may be a bit heavy as ideally you'd only lock for the time period which you need access to the variable for. One option is to create a scope within preupdate, lock that scope then copy out the necessary details. For our purposes, I don't think it matters.

Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
@AdarshKaran
Copy link
Author

I have one more ask that falls outside of the integration test. Please add a demo world of some form so I/users can test it.

Yes. I will look into it

@AdarshKaran
Copy link
Author

There is actually one issue. When i give attach request and specify a child_model_name that doesn't exist in the scene, we get a true response from the service. The main reason is because we can't access EntityComponentManager within the OnServiceRequest function. Do you think there is a better way to do it? or shall we just update the response message to something like "Attach request accepted" instead of the current "Attached to child model..."?

@arjo129
Copy link
Contributor

arjo129 commented Sep 25, 2025

There is actually one issue. When i give attach request and specify a child_model_name that doesn't exist in the scene, we get a true response from the service. The main reason is because we can't access EntityComponentManager within the OnServiceRequest function. Do you think there is a better way to do it? or shall we just update the response message to something like "Attach request accepted" instead of the current "Attached to child model..."?

Yes, this is one of the less-than-ideal side effects of the way we mix pub/sub with ECM in Gazebo. I think "Attach request accepted" may be a reasonable thing and then log an error. If need be we could publish to a seperate topic, but I'd leave that as "future work".

Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
@AdarshKaran
Copy link
Author

There is actually one issue. When i give attach request and specify a child_model_name that doesn't exist in the scene, we get a true response from the service. The main reason is because we can't access EntityComponentManager within the OnServiceRequest function. Do you think there is a better way to do it? or shall we just update the response message to something like "Attach request accepted" instead of the current "Attached to child model..."?

Yes, this is one of the less-than-ideal side effects of the way we mix pub/sub with ECM in Gazebo. I think "Attach request accepted" may be a reasonable thing and then log an error. If need be we could publish to a seperate topic, but I'd leave that as "future work".

Done! Please let me know if it looks good. I have also added some additional logs and errors for more clarity

Copy link
Contributor

@arjo129 arjo129 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have a quick question here. It seems like I forgot to update the review.

In general the code looks ok but I cannot approve it without examples/tests.

{
GZ_PROFILE("DynamicDetachableJoint::PreUpdate");
std::lock_guard<std::mutex> lock(this->mutex);

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A quick question : is it intended that the plugin continues to work while paused?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think it should work when the sim is paused. Should we track if it is paused and return in that case (following the error first approach)?

…tests

Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
@AdarshKaran
Copy link
Author

I was working on the tests and example world, please let me know if it looks good after testing

Example World.

I have added clean comments in the sdf for the commands to test it with.
Screenshot from 2025-09-26 19-54-25

Test

As for the integration test, I added both a test world and the test cpp file.
I tested it with the following command after building, run from /build/gz-sim9/ directory
ctest -V -R INTEGRATION_dynamic_detachable_joint

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
🪵 jetty Gazebo Jetty
Projects
Status: In review
Development

Successfully merging this pull request may close these issues.

Make child model and link configurable in DetachableJoint System
3 participants