Dynamically create/remove joint

[waytry]

Problem description

I am trying to develop a robot arm that can grab an object. The way I implement the "grab" is dynamically creating a fixed joint between the robot arm and the object. This was done in two steps. First, I used the components::ContactSensorData to detect when the robot contacts the object. Second, at the moment of contact, I used the sdfEntityCreator to dynamically create a fixed joint as below.

auto joint = sdf::Joint();
joint.SetName( pad_link_name_ + "_fixed_joint");
joint.SetType(sdf::JointType::FIXED);
joint.SetParentLinkName( pad_link_name_ );
joint.SetChildLinkName(target_link_name_);

auto joint_entity = creator_->CreateEntities(&joint);
creator_->SetParent(joint_entity, model_entity_);

Error message

I was able to detect the moment of contact. However, creating the joint was failed and the error message was "[Err] [SDFFeatures.cc:700] Asked to create a joint from link [pad_0] to link [object] in the model [robot], but the child link could not be found in that model!"

It seems like the error is because the link [object] is in a model that different than the robot. My question is how to create a joint that connects two links from two models? Or is there another way to implement the "grab"?

Thanks in advance.

[diegoferigo]

For another use-case I also faced this problem without finding a proper solution. I am interested to follow this issue and I will give my 2 cents.

Creating / removing a joint could be generalized to the idea of creating / removing a constraint. In your case this constraint is between a manipulator EE and an object, but it could be also used to take a model e.g. a floating base humanoid and fix the position of its base link.

There's a difference in these two cases. In the manipulator case the two objects are already in the desired position, and they get attached there with a new constraint. In the humanoid case, while this could be done, it's not ideal. In fact, there are many cases where you want to fix the base in another point. Doing it would involve:

1. In t=k reset the pose of the model
2. Run the simulator
3. In t=k+1 create the new joint

With #61 maybe the extra step could be performed as paused, so that the simulation is not affected. But in general, it could be useful allowing to create a constraint not only point-to-point.

In my opinion here pybullet does a very good job. See constraint.py. It would be amazing being able to do the same also with gazebo.

Another improvement is using a generic joint type, not only a fixed joint. Using either a revolute or a ball joint could be a use case. Related to https://github.com/robotology/gazebo-yarp-plugins/pull/461.

[waytry]

Hi @diegoferigo, thanks for the reply. You're right that the more general case would be creating/removing constraints directly. As far as I know, in the current ignition gazebo, the constraints are added through different types of joints (I guess it would be sufficient in many cases). I didn't see API functions that could impose constraints directly.

[azeey]

There is support for creating fixed joints between links of different models. The Detachable Joint system makes use of that. In theory, other types of joints can be created as well, but that has not been tested.

FYI, we're running a little behind on forward porting PRs to the master branch. That's why the Detachable Joint system is not on master yet. This should be resolved soon.

[waytry]

Hi @azeey, thank you for providing me the guidance. I looked at the DetachableJoint system and the DetachableJoint component and they are exactly what I am looking for. I am using the Ignition Citadel and it doesn't have the DetachableJoint component. So I copied the DetachableJoint.hh component to the directory /usr/include/ignition/gazebo3/ignition/gazebo/components. And did the following

#include <ignition/gazebo/components/DetachableJoint.hh>
....
....
auto joint_entity = _ecm.CreateEntity();
_ecm.CreateComponent( joint_entity,
ignition::gazebo::components::DetachableJoint({parent_link_entity_, target_link_entity_, "fixed"}));

However, creating the fixed joint failed and I get the warning message "[GUI] [Wrn] [EntityComponentManager.cc:1093] Component type [285520019272511910] has not been registered in this process, so it can't be deserialized".

I guess it's because I simply copy the DetachableJoint component file to the include path. If I copy the file to my project folder, how should I write the CMakeLists? I think this question is equivalent to the scenario that someone wants to develop their own component. How should the CMakeLists be written?

Thanks.

[azeey]

@waytry, There are changes in the Physics system as well that you'd need for this to work. https://github.com/ignitionrobotics/ign-gazebo/blob/4569d7f8ed98d94ded3074c0bc88c006f16f4110/src/systems/physics/Physics.cc#L798-L874 https://github.com/ignitionrobotics/ign-gazebo/blob/4569d7f8ed98d94ded3074c0bc88c006f16f4110/src/systems/physics/Physics.cc#L924-L949 The warning message about serialization can be ignored for now as that component is not needed by the GUI.

[waytry]

Hi @azeey, thanks for the reply. I tried to add the changes you provided into my current Physics system. However, it needs the support of physics::DetachJointFeature in the physics engine. Does it mean that the only way to get the newly developed DetachableJoint component and feature is to reinstall everything from source?

[azeey]

Support for DetachJointFeature was merged into the ign-physics2 branch and released in ign-physics 2.1 just last week, so you should be able to update your system to get it. You'd still need to build ign-gazebo from source until #86 is merged and released.

[azeey]

86 is merged. You should be able to create/remove fixed joints in Citadel now.

[waytry]

Hi @azeey, thanks for the update. Very glad to hear that the improvements had been merged, since I had been waiting for it. I am actually a little confused on how to obtain these merged improvements. Do I need to reinstall from source as this instruction? Or I need to reinstall from the binary as here?

[azeey]

You're welcome. You should be able to do apt upgrade and get the latest releases. For future reference, the ign-gazebo and ign-physics changelogs are probably the best places to see what PRs made it into the release.

[waytry]

Hi @azeey, thanks a lot for the instruction. It was a great help. I am now able to create/remove fixed joints using the DetachableJoint.

For those who are interested, I basically followed the detachable_joint system. More specifically, the lines below

this->detachableJointEntity = _ecm.CreateEntity();

 _ecm.CreateComponent(
            this->detachableJointEntity,
            components::DetachableJoint({this->parentLinkEntity,
                                         this->childLinkEntity, "fixed"}));

[waytry]

Hi @azeey, I'd like to report one potential issue related to the DetachableJoint component. As I mentioned previously, I was developing a robot arm that can grab an object. Thanks for your instructions, I managed to implementing the "grab" by creating a DetachableJoint component at the moment of contact.

The robot has two "fingers". And both of the fingers will be in contact with the object. I tried to create DetachableJoint between each of the fingers with the object. However, the DetachableJoint will only be successfully created for one of the fingers, which is the one that firstly contacts the object. I guess it's a problem related to the situation that two links in a model have the same third link as child link when create joint. My question is, using the DetachableJoint, is it allowed to connect two links in a model to the same third link?

[azeey]

Hi @waytry, yeah, I think the issue is that creating a joint for both fingers creates a kinematic loop, which is not supported with the DetachableJoint system. Does it work okay if you only create a joint with just one finger?

FYI: https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo3/tutorials/detachable_joints.md

[waytry]

Hi @azeey, thanks for the quick reply. Now I see the reason. It is exactly that kinematic loop will be created if I connect two fingers to the same object. It works perfectly if I only create a joint with one finger.

Is there a way that I can connect both of the fingers to the same object (I actually have to implement this functionality)? Or is there a plan in the development team to improve the DetachableJoint such that the kinematic loop will be supported?

BTW, to get the magnitude of the contact force, I will have to wait until this PR is merged, right?

[waytry]

Hi @azeey, just want to follow up with you to see if there is a plan in the development team to support the kinematic loop with the DetachableJoint system. Thanks.

[azeey]

Hi @waytry, I don't think this is in our short-term plan, so I can't give you any timeline, but I believe Gazebo-classic supported kinematic loops, so we'll probably add it to ignition at some point.

[waytry]

Hi @azeey, thanks for the reply. I guess I will have to switch back to the Gazebo-classic. Let me know when there is any update.

[waytry]

Hi @azeey, just want to check back to see if the kinematic loop is supported by the ignition gazebo now? Thanks.

[azeey]

Hi @waytry, this is still not supported yet.

[Dagu12s]

Hi @azeey, thanks a lot for the instruction. It was a great help. I am now able to create/remove fixed joints using the DetachableJoint.

For those who are interested, I basically followed the detachable_joint system. More specifically, the lines below

this->detachableJointEntity = _ecm.CreateEntity();

 _ecm.CreateComponent(
            this->detachableJointEntity,
            components::DetachableJoint({this->parentLinkEntity,
                                         this->childLinkEntity, "fixed"}));

Hello, im trying to develp a modular robot with ROS2 and Ignition. Can u share more details about how u did it?. Did you build a plugin for this or is it possible to do it whith in a ROS node?

[chapulina]

Here's an example world using the detachable joint, you don't need to build a new plugin, you can use the one that we ship:

https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo5/examples/worlds/detachable_joint.sdf

To trigger it with ROS, you can bridge the detach message using ros_ign.

[Dagu12s]

The thing is that i need to create the conexion dinamically and destroy it aswell. For example i destroy the link between the body and a leg and later on the sumulation y need to create it again. So the point is to launch a node or something to conect the two models whenever i want during the simulation.

First i tryied to build a node and now im trying to develop a plugin to do so but i had some problems and idont know if its the right thing to do or is it better do something else.

[Dagu12s]

Following the DetachableJoint i created a plugin that attaches 2 differents models and detaches them like the DetacherJoint. It actually creates a detachableJoint but it does not need to be in the parents model.

My issue now is that if run a .sdf world and then i run the plugin from other .sdf, it does not found the models with: this->parentLinkEntity = _ecm.EntityByComponents( ignition::gazebo::components::Link(), ignition::gazebo::components::ParentEntity(modelEntity), ignition::gazebo::components::Name(this->parentLinkName));

Any idea why i can't access to the components in the simulation created by other .sdf?

[Thanish-Kumar]

Hi @azeey, I'd like to report one potential issue related to the DetachableJoint component. As I mentioned previously, I was developing a robot arm that can grab an object. Thanks for your instructions, I managed to implementing the "grab" by creating a DetachableJoint component at the moment of contact.

The robot has two "fingers". And both of the fingers will be in contact with the object. I tried to create DetachableJoint between each of the fingers with the object. However, the DetachableJoint will only be successfully created for one of the fingers, which is the one that firstly contacts the object. I guess it's a problem related to the situation that two links in a model have the same third link as child link when create joint. My question is, using the DetachableJoint, is it allowed to connect two links in a model to the same third link?

Hii @waytry could you guide me on how to create a dynamic detachable joint on runtime. Currently, I could only detach the joint configured on sdf. cc @azeey

[alexlt97]

I was wondering if there is some continuation about the attached part of this plugin? Any example for attaching two models together during simulation?

[AbiyyuMufti]

can I use detachable_joint in gazebo classic? or is there similar solution like detachable joint for gazebo classic?

[solid-sinusoid]

Hi @Dagu12s I also need to create and destroy a joint in the simulator dynamically. Have you solved your problem? Enough time has passed

[Dagu12s]

Hello, i have crated a plugin based on dettachableJoint to create and destroy links dinamically, maybe it could be useful for you as well. Later today i will share it, i cant accees to the computer right now

[Dagu12s]

I opened the repo, there are more things that im currently working on. To create and destroy the links dinamically i modified the DettachableJoint. You can check the code https://github.com/Dagu12s/AttachablePlugin/tree/main/AttachableJoint. I compiled the plugin and acreate a "Builds" directory if you don't have ignition from source. Maybe you can use it @solid-sinusoid

[mitsui29]

@azeey Hi,Thanks for your answers. It's very nice to see here a lot discussions about what i am doing righnt now! I wanna ask for cooperative manipulation for dual franka robot. Will i meet the kinematic loop problem in gazebo? I wanna to dynamically create the fixed joint to finish the grab, kinda like rigid grasping. Thanks in advance!

[VincentTUHH]

Is there finally a solution that solves kinematic loops in gazebo