0
$\begingroup$

I'm writing my own effort controller based in a simple example from gazebo webpage to test it in a robot and I'm having an issue when writing void setCommandCB function.

So far I managed myself to write the code for a 7DOF robot but I'm stuck in this fucntion as I dont quite understand what does this function exactly. I guess that is the callback that is called whenever receives the messages from the command topic right?

maybe my code approach is wrong, I just want to control each of the joints of the robot sending a torque command to them.

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64.h>

namespace panda_torque_control {


class ImpedanceController : public controller_interface::Controller<hardware_interface::EffortJointInterface> {

private:
  std::vector<hardware_interface::JointHandle> joints_;
  ros::Subscriber sub_command_;
  std::vector<double> tauCommand;
  

public:
  bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n) {
    std::vector<std::string> joint_names;
    if (!n.getParam("joint_names", joint_names)) 
    {
        ROS_ERROR("Could not find joint names");
        return false;

    }

    // Initialize joint handles for each joint
    for (const auto &joint_name : joint_names) {
      joints_.push_back(hw->getHandle(joint_name));
    } // throws on failure
    
    tauCommand = {0,0,0,0,0,0,0};


    // Start command subscriber
    sub_command_ = n.subscribe<std_msgs::Float64>("tauCommand", 1, &ImpedanceController::setCommandCB, this);
    
    return true;
  }

  void update(const ros::Time &time, const ros::Duration &period) 
  {

    // Send a torque command of 2 Nm to each joint
    std::vector<double> torque_command(joints_.size(), 0.0);
    for (size_t i = 0; i < joints_.size(); ++i) 
    {
        torque_command[i] = tauCommand[i];
        joints_[i].setCommand(torque_command[i]);
    }
   }


void setCommandCB(const std_msgs::Float64ConstPtr& msg)
  {
    if (msg->data.size() == tauCommand.size()) 
    {
          tauCommand = msg->data;
        } else {
          ROS_WARN("Received tauCommand message with incorrect size");
        }
  }

  void starting(const ros::Time &time) {}
  void stopping(const ros::Time &time) {}

};

PLUGINLIB_EXPORT_CLASS(panda_torque_control::ImpedanceController, controller_interface::ControllerBase);
} // namespace panda_torque_control
$\endgroup$
3
  • $\begingroup$ This already exists as the JointEffortController class. Please edit your question to explain what you need beyond the existing class. If you need something more, why are you not using the existing ros code as a starting point? The class doc is here: docs.ros.org/en/noetic/api/effort_controllers/html/… $\endgroup$
    – Mike973
    Nov 10, 2023 at 13:23
  • $\begingroup$ As I'm quite new in Ros_control and gazebo, I'm going step by step but I have implemented in a real robot a torque control algorithm and I want to set up a simulation in gazebo using this same control that command a torque to every joint of the robot. Does JointEffortController allow me to control all joints together rather that one by one? $\endgroup$
    – mikel lasa
    Nov 13, 2023 at 9:23
  • $\begingroup$ The link I provided already answers your question. Hint: in this context, "group" means multiple joints. $\endgroup$
    – Mike973
    Nov 14, 2023 at 14:33

1 Answer 1

0
$\begingroup$

Regarding your implementation question, you can look at the existing effort_controllers/ JointEffortController to see what they did.

More generally, the functionality you want is already implemented by the ros_control subsystem. If you configure ros_control correctly, it will create a ".../command" topic that you can then use to move the arm.

Note that there is an inherent assumption that your particular arm already has a hardware_interface that accepts effort input (or none of these effort_controllers is going to work.)

$\endgroup$
1
  • $\begingroup$ thanks for the tip, yeah, I know that the functionality is already implemented already. I'm doing this because looking to the future, I will be writing an impedance control for two complete different robots that work together (a 7DOF panda and 6 DOF staubli). I'm not sure though if this is a good approach or should I use some existing code for that like panda_ros library, but thats another question I guess. $\endgroup$
    – mikel lasa
    Nov 16, 2023 at 7:15

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.