[gazebo_ros_control] robot moves towards 0 position without any command (custom effort controller)

[mikelasa]

Hello, I wrote an impedance controller for my 6 DOF robot using based in EffortJointInterface and I'm strugglin while simulating it in gazebo. So far because of the sake keeping simplify I'm trying to just compensate gravity but the robot behaviour during the simulation is always the same, to go to the 0 position for every joint acting as spring, instead of just keeping the iniital position as it should be. Here my code for the controller, I know some term are based in personal classes but I will try to explain the problem:

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>
#include <panda_controller/StaubliDynModel.h>
#include <panda_controller/StaubliJacobians.h>
#include <panda_controller/StaubliModel.h>
#include <panda_controller/TypeDefinitions.h>
#include <panda_controller/MathHelper.h>
#include <Eigen/Dense>
#include <thread>

using namespace ourTypeDefinitions;

namespace tx90_torque_control {

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

private:
  std::vector<hardware_interface::JointHandle> joints_;
  ros::Subscriber sub_command_;
  std::vector<double> tauCommand;
  std::vector<double> joint_positions;
  std::vector<double> joint_velocities;
  std::vector<double> initial_pose_;
  std::vector<double> torque_command;

  QVector6 xs;
  QVector6 xd;
  QVector6 dxs;
  QVector6 dxd;
  QVector6 ddxd;
  QVector6 qd;
  QVector6 dqd;
  QVector6 ddqd;
  QVector6 qs;
  QVector6 dqs;
  QVector6 ddqs;
  QVector6 predqs;
  QVector6 qe;
  QVector6 xe;
  QVector6 dxe;
  QVector6 spring;
  QVector6 damper;
  QVector6 imp;
  QVector6 tau;

  Eigen::Matrix<double, 6, 6> Km;
  Eigen::Matrix<double, 6, 6> Dm;
  Eigen::Matrix<double, 6, 6> Mm;
  Eigen::Matrix<double, 6, 6> KmMap;
  Eigen::Matrix<double, 6, 6> DmMap;

  std::vector<double> kp_;
  std::vector<double> kd_;

  double nAxis;
    
  Eigen::MatrixXd gMat;
  Eigen::MatrixXd CentrCor;
  Eigen::MatrixXd massMat;

  Eigen::MatrixXd J;
  Eigen::MatrixXd JD;
  
  StaubliDynModel gravMat;
  StaubliJacobians JMat;

  StaubliModel tx90;
  
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;
    }

    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;
  }

  void update(const ros::Time &time, const ros::Duration &period) 
  {
  
    joint_positions.clear();
    joint_velocities.clear();

    for (size_t i = 0; i < joints_.size(); ++i) 
    {
      joint_positions.push_back(joints_[i].getPosition());
      joint_velocities.push_back(joints_[i].getVelocity());
    }	

    qs = MathHelper::stdV2qv6(joint_positions);
    dqs = MathHelper::stdV2qv6(joint_velocities);

    xs = tx90.fkAxisAngle(qs, nAxis);
    xd = tx90.fkAxisAngle(qd, nAxis);

    dxs= JMat.getJ(qs) * dqs;
    dxd= JMat.getJ(qd) * dqd;

    ddqs = (dqs - predqs) / period.toSec();
    //ddxs = JMat.getJ(qs) * ddqs + JMat.getJD(qs, dqs) * dqs;
    ddxd = JMat.getJ(qd) * ddqd + JMat.getJD(qd, dqd) * dqd;

    gMat = gravMat.getGravity(qs);
    CentrCor = gravMat.getCentrCor(qs, dqs);
    massMat = gravMat.getMass(qs);
 

    Mm = massMat;

    //OK
    qe = qd - qs;
    xe = (xd - xs);
    dxe = (dxd - dxs);

    KmMap = Eigen::VectorXd::Map(kp_.data(), kp_.size()).asDiagonal();
    DmMap = Eigen::VectorXd::Map(kd_.data(), kd_.size()).asDiagonal();

    // OK
    spring = KmMap * xe;
    damper = DmMap * dxe;

    std::cout << "Error Vector (xe): " << xe.transpose() << std::endl;
    std::cout << "Proportional Term: " << spring.transpose() << std::endl;
    std::cout << "Derivative Error (dxe): " << dxe.transpose() << std::endl;
    std::cout << "Derivative Term: " << damper.transpose() << std::endl;


    imp = JMat.getJ(qs).completeOrthogonalDecomposition().pseudoInverse() * (ddxd - JMat.getJD(qs, dqs) * dqs + Mm.completeOrthogonalDecomposition().pseudoInverse() * (spring + damper));
    std::cout << "ddxd: " << ddxd.transpose() << std::endl;
    std::cout << "dqs: " << dqs.transpose() << std::endl;
    std::cout << "Control Action: " << imp.transpose() << std::endl;

   //tau = Mm * imp + CentrCor + gMat;
    tau = gMat;

    std::vector<double> gVector(gMat.data(), gMat.data() + gMat.rows() * gMat.cols());
    //std::cout << "CentrCor" << std::endl<< std::endl;

    for (size_t i = 0; i < joints_.size(); ++i) 
     {
          torque_command[i]=(tau[i] + tauCommand[i]);
          joints_[i].setCommand(torque_command[i]);
          //std::cout << torque_command[i] << std::endl;
     }

     predqs = dqs;
    
   }

void setCommandCB(const std_msgs::Float64MultiArrayConstPtr& 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) 
  {
    nAxis = joints_.size();
    dqd = {0,0,0,0,0,0};
    ddqd =  {0,0,0,0,0,0};
    predqs =  {0,0,0,0,0,0};

    for (size_t i = 0; i < joints_.size(); ++i) 
    {
      initial_pose_.push_back(joints_[i].getPosition());
      //std::cout << initial_pose_[i] << std::endl;
    }

    qd << initial_pose_[0], initial_pose_[1], initial_pose_[2], initial_pose_[3], initial_pose_[4], initial_pose_[5];
    std::cout << "Initial position for joint: " << std::endl;
    std::cout << qd << std::endl;

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

};


PLUGINLIB_EXPORT_CLASS(tx90_torque_control::ImpedanceControllerTx90, controller_interface::ControllerBase);
} // namespace tx90_torque_control

This very same code (the mathematics I mean) has been tested in a real robot and its working so I was trying to simulate the robot now, but as I said the robot behavior is wrong. I'm just trying to compensate the gravity term through the tau = gMat;. So far I checked this:

• initial position is ok
• g term values are ok
• position and velocity lectures are ok.
• kp and kd initialization are ok
• error calculations are ok

I printed this values while running the simulation in gazebo and for the first iteration seems to be ok, though the robot is always going upwards to the 0 position for every joint with a "spring" behavior (seems to be like a bad tuned PD).
As I checked my mathematics and tested in a real robot, I dont know what can it and I'm honestly lost here. my yaml file is the following, the idea is to set Kp and Kd gains and apply the m to the impedance matrixes, but as I'm trying to just compensate gravity I dont need them yet.

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
    kp:
        - 250
        - 250
        - 200
        - 200
        - 50
        - 50
    kd:
        - 10
        - 10
        - 5
        - 5
        - 2
        - 2