Guide on how to implement pure torque control (like Gazebo).

[xgamingman]

Check the second comment.

In the documentation: " Moreover, two control modes are implemented: position and velocity. The mode is automatically detected by checking the size of the position and velocity vectors, defined in the message, i.e., if the user wants to use position mode, only the position vector should be filled." but i want to use torque control with the servo.

I know this is possible using the cpp syntax but i have no idea how to implement it with ROS2 package.

also, would it help if i change line 2444 in /ScenarioParser.cpp ?

[xgamingman]

Finally, I was able to achieve very satisfactory results following these steps:

In STONEFISH PACKAGE

-file: "Servo.h" --> line 117 add: Scalar DesTor; -file: "Servo.h" --> line 87 add: void setTorque(Scalar tau); -file: "servo.cpp" --> line 82 add new void:

                               void Servo::setTorque(Scalar tau)
                               {
                                   DesTor = tau;
                               } 

-file: "servo.cpp" --> update function (comment all the function): just add this:

                         fe->DriveJoint(jId, DesTor);
                         //fe->ApplyDampingMod(jId);

-file: "FeatherstoneEntity.cpp": at start add: (can be used for debugging)

    #include <iostream>
    #include <chrono>

-file: "FeatherstoneEntity.cpp" line 422 and 430 --> add: (this method is used to apply damping to bt_rigid_body but i didn't need it

            //multiBody->setLinearDamping(50);
            //multiBody->setAngularDamping(50);
            //std::cout << "linear damp = " << multiBody->getAngularDamping() << "angular damp = " << multiBody->getAngularDamping() << " for link " << links.size() << std::endl;

-file: "FeatherstoneEntity.cpp": add ApplyDampingMod() in line 660 :

void FeatherstoneEntity::ApplyDampingMod(unsigned int iT)
{
        if(joints[iT].sigDamping >= SIMD_EPSILON || joints[iT].velDamping >= SIMD_EPSILON) //If damping factors not equal zero
        {
            Scalar velocity = multiBody->getJointVel(joints[iT].child - 1);

            if(btFabs(velocity) >= SIMD_EPSILON) //If velocity higher than zero
            {
                Scalar damping = - velocity/btFabs(velocity) * joints[iT].sigDamping - velocity * joints[iT].velDamping;
                multiBody->addJointTorque(joints[iT].child - 1, damping);
            }
        }
}

-file: "FeatherstoneEntity.h": line 185: void ApplyDampingMod(unsigned int iT);

-file: "FeatherStoneRobot.cpp": line 209 add: (using this line will change all revolute joint dampings to only one variable, check next section). dynamics->setJointDamping(i, 7, 7);

INSTALL STONEFISH PACKAGE AGAIN $ cd stonefish/build/ ; cmake .. ; make; sudo make install ;

In Stonefish_ROS2 package

-file: "ROS2simulationmanager.cpp" --> comment out from 423 to 432 --> add in 423,424:

                                         ((Servo*)actuator)->setControlMode(ServoControlMode::TORQUE);
                                         ((Servo*)actuator)->setTorque(it->second.second);

-file: "ROS2simulationmanager.cpp" --> comment out from 844 to 875 --> add in 844:

    if(msg->effort.size() > 0)
    {
        for(size_t i=0; i<msg->effort.size(); ++i)
        {
            try
            {
                robot->servoSetpoints_.at(msg->name[i]) = std::make_pair(ServoControlMode::TORQUE, (Scalar)msg->effort[i]);
            }
            catch(const std::out_of_range& e)
            {
                RCLCPP_WARN_STREAM(nh_->get_logger(), "Invalid joint name in desired joint state message: " << msg->name[i]);
            }
        }
    }

use colcon build in workspace, and send the message to ROS with empty arrays of position and velocity but add values in effort.

$ source /opt/ros/humble/setup.bash; cd ~/ros2_ws; clear; colcon build

// add damping in XML file \

the following section will help you add the damping titles to the XML file of the robot

-file "ScenarioParser.cpp": lines 2156, 2157

        Scalar sigDmp(0);
        Scalar velDmp(0);

-file "ScenarioParser.cpp": lines 2176

        if((item = element->FirstChildElement("JointDmp")) != nullptr) //Optional
        {
            if((item->QueryAttribute("sigDmp", &sigDmp) != XML_SUCCESS) || (item->QueryAttribute("velDmp", &velDmp) != XML_SUCCESS))
                log.Print(MessageType::WARNING, "Joint '%s' Modified Damping Values not properly defined - using 0 ", jointName.c_str());
        }

-file "ScenarioParser.cpp": line 2185 robot->DefineRevoluteJoint(jointName, parentName, childName, origin, axis, std::make_pair(posMin, posMax), damping, std::make_pair(sigDmp, velDmp));

-file "Robot.cpp": line 133: add in brackets of void (revolutejoint) the following: std::pair<Scalar,Scalar> DmpingValues line 144: add: jd.dmpVals = DmpingValues;

-file "Robot.h": line 83: add before end of brackets: std::pair<Scalar, Scalar> DmpingValues = std::make_pair(Scalar(0), Scalar(0)) line 245: add new line: std::pair<Scalar, Scalar> dmpVals;

-file: "FeatherStoneRobot.cpp": line 209 (custom line from before) change to: dynamics->setJointDamping(i, jointsData[i].dmpVals.first, jointsData[i].dmpVals.second);

NOTES: 0- this method assumed your robot has only revolute joints 1- It's better to use a high simulation rate of 800Hz (I am using MATLAB to send commands to ROS2). 2- doing this will remove position and velocity control modes. but any person with simple knowledge can make it function along with torque control. 3- (not sure about this) maximum torque in joints definition should be 0. since higher values didn't allow the joint to receive torque unless it exceeded the maximum torque value.