Torque sensing of programmatically created joints

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

stnc199 gravatar image

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

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 http://gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors

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.

bot.SetFromString(
    "<sdf version='1.4'>\
        <model name='"+bot_name+"'>\
            <pose>"+strpose+"</pose>\
            <link name='L_"+bot_name+"'>\
                <inertial>\
                    <inertia>\
                        <ixx>"+ixx+"</ixx>\
                        <ixy>"+ixy+"</ixy>\
                        <ixz>"+ixz+"</ixz>\
                        <iyy>"+iyy+"</iyy>\
                        <iyz>"+iyz+"</iyz>\
                        <izz>"+izz+"</izz>\
                    </inertia>\
                    <mass>"+mass+"</mass>\
                </inertial>\
                <collision name='collision'>\
                    <geometry>\
                        <mesh><uri>"+mesh_location+"</uri></mesh>\
                    </geometry>\
                </collision>\
                <visual name='visual'>\
                    <geometry>\
                        <mesh><uri>"+mesh_location+"</uri></mesh>\
                    </geometry>\
                </visual>\
            </link>\
      </model>\
  </sdf>"
);
this->world->InsertModelSDF(bot);

// 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->SetName(parent+"_to_"+child);
        joint->Attach(this->world->ModelByName(parent)->GetLink("L_"+parent),this->world->ModelByName(child)->GetLink("L_"+child));
        joint->Load(this->world->ModelByName(parent)->GetLink("L_"+parent),this->world->ModelByName(child)->GetLink("L_"+child), JointPose);
        joint->SetModel(this->world->ModelByName(child));

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

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

        if(type=="revolute"){
            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));
}
CreateJoint::CreateJoint(){}

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