Torque sensing of programmatically created joints

asked 2021-12-24 03:05:50 -0600

stnc199 gravatar image

updated 2021-12-31 04:30:14 -0600

Hi everyone!

I am working with a robot that is successfully dynamically generated in gazebo (it is not created from a urdf file). I would like to introduce a joint sensor but because of the way the robot is spawned, I cannot apply the classical method of inserting a sensor in the description file (since there is none), as in the tutorial

Let me give some context. Essentially, the robot is constituted of smaller polygonal units, each of which is spawned programmatically by creating a temporary sdf.

    "<sdf version='1.4'>\
        <model name='"+bot_name+"'>\
            <link name='L_"+bot_name+"'>\
                <collision name='collision'>\
                <visual name='visual'>\

// parent_edge and bot_edge are the edges by which the two polygons are connected
CJ->JointCreation(parent_name, bot_name, parent_edge, bot_edge, joint_type, angle);

The joint between a parent and child polygon is programmatically created with JointCreation()

void CreateJoint::JointCreation(std::string parent, std::string child, int parent_edge, int child_edge, std::string type, double angle){
        //here are geometrical computations to correctly set the axis of rotation, based on the parent edge, child edge and joint angle

        gazebo::physics::JointPtr joint = this->world->Physics()->CreateJoint(type, this->world->ModelByName(parent));

        joint->Load(this->world->ModelByName(parent)->GetLink("L_"+parent),this->world->ModelByName(child)->GetLink("L_"+child), JointPose);

        if(type == "revolute"){
            joint->SetAxis(0, joint_axis);

        std::cout<< "Joint created : " << joint->GetName() << std::endl; 
        this->MStruct->add_joint(joint->GetName(), joint, angle);

            JC->JointMotorPosition(joint, joint->Position(0) + angle, 0); 

And the constructor of the joint

CreateJoint::CreateJoint(gazebo::physics::WorldPtr &world, ros::NodeHandle* rosnode,
                        sdf::ElementPtr sdf, Bot_Structure *botStruct, Joint_control_srv* JC){
    this->rosnode = rosnode;
    this->sdf = sdf;
    this->world = world;
    this->MStruct = botStruct;
    this->JC = JC;
    this->serviceThread1 = std::thread(std::bind(&CreateJoint::init_Service_Creation, this));
    this->serviceThread2 = std::thread(std::bind(&CreateJoint::init_Service_Destruction, this));

I searched the forums for similar questions but unfortunately found nothing. I believe I could use the ForceTorqueSensor class in the JointCreation() function, but so far I only got segfaults when creating the joints. Could anyone please point me in the right direction?

Thanks a lot and happy holidays!

edit retag flag offensive close merge delete