0
$\begingroup$

I am trying to simulate the control of a caught heavy object in gazebo garden. To do so I have a plugin that creates a detachable fixed joint between the object and the end-effector of my robot once collision occurs, the issue is that when the joint is created the velocity of the object dramatically increases, while also changing direction. I originally believed that is was due to incorrect inertia parameters, however most other tests / collisions work as expected so I assume something is going wrong with the joint creation.

A synopsis of how my system works is that there is a collision sensor on the end effector, which triggers with contact with a specific object, it sends a boolean value to a topic that this plugin subscribes to, which then creates the joint between the 2 links, so long as the plugin had first been enabled.

this plugin takes heavy inspiration from the detachable joint system inside the gz_sim repo

gazebo Versions:

  • sim: 7.5.0
  • plugin: 2.0.1
  • transport 12.2.0
#include <detachable_joint_creator.hpp>


#include <gz/plugin/Register.hh>
#include <gz/transport/Node.hh>
#include <functional>
#include <gz/sim/components/DetachableJoint.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/Joint.hh>
#include <gz/sim/Link.hh>
using namespace monke_plugins;


JointCreator::JointCreator():
dataPtr(new JointCreatorPrivate)
{

}
JointCreator::~JointCreator(){

}
void JointCreator::Configure(const gz::sim::Entity &_entity,
    const std::shared_ptr<const sdf::Element> & _sdf,
    gz::sim::EntityComponentManager &_ecm,
    gz::sim::EventManager &/*_eventMgr*/
){

    // Initialise the Plugin via sdf elements

    gzmsg << "loading JointCreator plugin\n";
    if(_sdf->HasElement("parent")){

        this->dataPtr->end_effector_link_name = _sdf->Get<std::string>("parent");
    }
    else{
        gzerr << "[parent] link name not specified in sdf \n";
        return;
    }

    gz::sim::Model model(_entity);
    this->dataPtr->end_effector_entity = model.LinkByName(_ecm, this->dataPtr->end_effector_link_name);
    if (this->dataPtr->end_effector_entity == gz::sim::kNullEntity)
        {
        gzerr << "Could not find link named "<< this->dataPtr->end_effector_link_name << std::endl;
        return;
        }

    if(_sdf->HasElement("contact_sensor_topic")){
        this->dataPtr->contact_sensor_topic = _sdf->Get<std::string>("contact_sensor_topic");
        std::function<void(const gz::msgs::Contacts &)> callback = [this](gz::msgs::Contacts msg){this->ContactSensorCallback(msg);};
        if(!this->dataPtr->transport_node->Subscribe<gz::msgs::Contacts>(this->dataPtr->contact_sensor_topic,callback)){
            gzerr << "Failed to subscribe to topic [" << this->dataPtr->contact_sensor_topic << "] \n";
            return;
        }
        //? idk if advertising a subscription works
        this->dataPtr->transport_node->Advertise<gz::msgs::Contacts>(this->dataPtr->contact_sensor_topic);

    }
    else{
        gzerr << "detachable_joint plugin has no SDF Element contact_sensor_topic";
        return;
    }
    
    if(_sdf->HasElement("command_topic")){
        this->dataPtr->command_topic = _sdf->Get<std::string>("command_topic");
        std::function<void(const gz::msgs::Boolean &)> callback = [this](gz::msgs::Boolean msg){this->CommandCallback(msg);};
        if(!this->dataPtr->transport_node->Subscribe<gz::msgs::Boolean>(this->dataPtr->command_topic,callback)){
            gzerr << "Failed to subscribe to topic [" << this->dataPtr->command_topic << "] \n";
            return;
        }
        //? idk if advertising a subscription works 
        this->dataPtr->transport_node->Advertise<gz::msgs::Boolean>(this->dataPtr->command_topic); 
        
    }
    else{
        gzerr << "detachable_joint plugin has no SDF Element command_topic";
        return;
    }

    if(_sdf->HasElement("triggered_topic")){
        this->dataPtr->triggered_topic = _sdf->Get<std::string>("triggered_topic");
        
         this->dataPtr->publisher = std::make_unique<gz::transport::Node::Publisher>(this->dataPtr->transport_node->Advertise<gz::msgs::Boolean>(this->dataPtr->triggered_topic));  
        
    }
    else{
        gzerr << "detachable_joint plugin has no SDF Element triggered_topic";
        return;
    }







}

void JointCreator::PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm){

    if(_info.paused){return;} //if sim is paused.

    // gzmsg << "JointCreator::PostUpdate - " << _info.simTime.count() << std::endl;
    
}

void JointCreator::PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm){
    if(_info.paused){return;} //if sim is paused.


    auto Contact = [this](){return(this->dataPtr->contacted_entity != gz::sim::kNullEntity);};



    if(this->dataPtr->PluginActive && !this->dataPtr->JointCreated && Contact()){ //! CREATE JOINT
        // if the joint is active, it has not been created, and contact is made, then create the joint
        this->dataPtr->joint = _ecm.CreateEntity();
        auto parentLink = _ecm.ParentEntity(this->dataPtr->contacted_entity); //create a new link as the parent entity of the obejct we're trying to grab
        _ecm.CreateComponent(
            this->dataPtr->joint,
            gz::sim::components::DetachableJoint({this->dataPtr->end_effector_entity,parentLink,"fixed"})
            );
        gzdbg << "created detachable joint between end effector & [" << this->dataPtr->contacted_entity<< "] @ " << _info.simTime.count() <<"\n";
        gz::msgs::Boolean msg;
        msg.set_data(true);
        this->dataPtr->publisher->Publish(msg);


        this->dataPtr->JointCreated = true;

    }
    if(!this->dataPtr->PluginActive && this->dataPtr->JointCreated){//! DESTROY JOINT
        // if the joint is not active and the joint has been created, then destroy the joint.
        _ecm.RequestRemoveEntity(this->dataPtr->joint,true);
        this->dataPtr->joint = gz::sim::kNullEntity;

        
        gzdbg << "deleted detachable joint between end effector & ["<< this->dataPtr->child_entity << "] @ " << _info.simTime.count() <<"\n";
        gz::msgs::Boolean msg;
        msg.set_data(false);
        this->dataPtr->publisher->Publish(msg);
        this->dataPtr->JointCreated = false;
    }
 
    
}


void JointCreator::CommandCallback(const gz::msgs::Boolean & _msg){
    gzmsg << "JointCreator received command msg [" << _msg.data() << "]\n";
    this->dataPtr->PluginActive = _msg.data();    
}

void JointCreator::ContactSensorCallback(const gz::msgs::Contacts & _msg){
    if(_msg.contact_size()){
        gzmsg << "JointCreator received contact msg with object: [" << _msg.contact(0).collision2().id() << "]\n";
        auto contact = _msg.contact(0);
        this->dataPtr->contacted_entity = contact.collision2().id();

    }
    else{
        this->dataPtr->contacted_entity = gz::sim::kNullEntity;
    }
}

GZ_ADD_PLUGIN(
    monke_plugins::JointCreator,
    gz::sim::System,
    monke_plugins::JointCreator::ISystemPreUpdate,
    // sample_system::JointCreator::ISystemUpdate,
    monke_plugins::JointCreator::ISystemPostUpdate,
    monke_plugins::JointCreator::ISystemConfigure
)
// GZ_ADD_PLUGIN_ALIAS(sample_system::JointCreator,"gz::sim:systems::JointCreator")
```
$\endgroup$

1 Answer 1

1
$\begingroup$

There's a remark in the tutorial that models should not be colliding when the detachable joint is made. See also this discussion in the pull request.

So I think the behavior you experience is due to this.

An alternative, instead of checking for contact, you could check for distance and create the joint when the object is close enough.

Or, in a similar previous question I suggested to use a PerformerDetector. This reports when an object enters a specific volume. I have not tried this yet, but the idea is that you could place a small detection volume at the center of your gripper and get notified if (and which) object overlaps with that region. And then you can create the joint with that object.

Note that there is a remark to that answer that the "PerformerDetector uses axis aligned boxes". Not sure what that means and why it would be an issue.

$\endgroup$
1
  • $\begingroup$ Ah thank you, I missed that when going through the documents, I'll implement this asap. Another thing of note I found was that this only occured when the end-effector is at the edge of its reach, so a short-tern solution to minimise its impact was to change the initial position of the arm away from this point. $\endgroup$ Commented Oct 5, 2023 at 7:17

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.