0
$\begingroup$

I finished coding my impedance control for a 6 DOF robot but now I'm struggling on how to initialize and edit the spring and damping matrixes. So far, first I tried hardcoding them in the controller cpp but its kind of bothering me cause everytime I want to change values I have to compile the workspace again.

I was wondering how could I write this 6x6 matrixes in a way so I could change the values while the simulation is executing. I have an sort of an idea about how to do it but I'm getting errors, in my controller.cpp I used the same kp and kd idea of the PID control loading to the parameter server:

public:

  ImpedanceControllerTx90(): tx90(StaubliModel::ModelID::TX90){}

  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
    
    /*
    // Load gain using gains set on parameter server
     if (!n.getParam("kp", kp_))
    {
        ROS_ERROR("Could not find the gain parameter value");
        return false;
    }

    if (!n.getParam("kd", kd_))
    {
        ROS_ERROR("Could not find the gain parameter value");
        return false;
    }*/
    if (!n.getParam("Km", Km))
    {
      ROS_ERROR("Could not find the Km parameter value");
      return false;
    }

    if (!n.getParam("Dm", Dm))
    {
      ROS_ERROR("Could not find the Dm parameter value");
      return false;
    }


    tauCommand = {0,0,0,0,0,0};
    torque_command = {0,0,0,0,0,0};

    Mm << 1, 0, 0, 0, 0, 0,
          0, 1, 0, 0, 0, 0,
          0, 0, 1, 0, 0, 0,
          0, 0, 0, 1, 0, 0,
          0, 0, 0, 0, 1, 0,
          0, 0, 0, 0, 0, 1;

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

and my yaml looks like this:

tx90:

  # Creates the /joint_states topic necessary in ROS
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 1000
    
  tx90_impedance_controller:
    controlPeriod: 0.001
    type: impedance_controller_staubli/ImpedanceControllerTx90
    joint_names:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6

    Km << 500, 0, 0, 0, 0, 0,
          0, 500, 0, 0, 0, 0,
          0, 0, 400, 0, 0, 0,
          0, 0, 0, 0, 0, 0,
          0, 0, 0, 0, 100, 0,
          0, 0, 0, 0, 0, 100;

    Dm << 10, 0, 0, 0, 0, 0,
          0, 10, 0, 0, 0, 0,
          0, 0, 5, 0, 0, 0,
          0, 0, 0, 0, 0, 0,
          0, 0, 0, 0, 2, 0,
          0, 0, 0, 0, 0, 2;

of course, the Pid gain values goes to each of the joints and here I have a matrix... so instead of looking for a specific solution I would appreciate if somebody can give a hint about how to approach this issue

$\endgroup$
2
  • $\begingroup$ Is the << operator in the C++ code the Eigen << operator for matrix initialization? There's a merge operator in some YAML implementations that uses that syntax but it's not at all the same thing. $\endgroup$ Nov 22, 2023 at 1:57
  • $\begingroup$ yep, it is! so far I tested this code in the real robot and works $\endgroup$
    – mikel lasa
    Nov 30, 2023 at 15:11

1 Answer 1

0
$\begingroup$

As far as I know, ROS parameters are restricted to single lists of numbers. So you could flatten and reshape the matrix, passing it in from the YAML as a single list of floating point numbers concatenating all the rows.

If all you need to pass are strictly diagonal matrices, you could pass only the six diagonal elements in as the YAML array and populate the diagonal of a 6x6 zero matrix from those.

$\endgroup$
1
  • $\begingroup$ thank you! you are right, I managed to load this gains as a normal kp and kd vector and then create a matrix inserting values in the diagonal $\endgroup$
    – mikel lasa
    Nov 30, 2023 at 15:12

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.