hardware_interface::JointHandle getPosition() not returning right values

[mikelasa]

I'm testing my own custom controller for my panda but I have a small problem, when I start the simulation in gazebo, the robot is loaded in the right joint configuration. In my launch file I specified an initial position:

<?xml version="1.0"?>
<launch>

    <arg name="initial_joint_positions" default=" -J panda_joint1 0 -J panda_joint2 0 -J panda_joint3 0 -J panda_joint4 -1.5708 -J panda_joint5 0 -J panda_joint6 1.59 -J panda_joint7 0" doc="Initial joint configuration of the robot"/>

    <param name="robot_description" command="$(find xacro)/xacro '$(find panda_description)/robots/panda/panda_custom_gazebo.urdf'" />

    <!-- GAZEBO arguments -->
    <arg name="paused" default="true"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!--launch GAZEBO with own world configuration -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="$(arg debug)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda $(arg initial_joint_positions)"/>

    <!-- Load joint controller configurations from YAML file to parameter server -->
      <rosparam file="$(find panda_controller)/config/controllers.yaml" command="load"/>

    <!-- load the controllers -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/panda" args="joint_state_controller panda_impedance_controller" />

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/panda/joint_states" />
  </node>

</launch>

When I start the simulation I have a print in the controller code so I can see the values that the function getPosition() right after I initialize the handles. It should be the same as the launch file but is returning a different position. This is the controller code until the print:

#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/gravity.h>
#include <Eigen/Dense>
#include <thread>

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;
  std::vector<double> joint_positions;
  std::vector<double> joint_velocities;
  std::vector<double> torque_command;

  //Eigen::VectorXd gMat {{0, -8, 0, 7, 0, 0, 0}};
  Eigen::VectorXd gMat;

  Gravity gravMat;

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;

    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1));

    // 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};
    torque_command = {0, 0, 0, 0, 0, 0, 0};

    for (size_t i = 0; i < joints_.size(); ++i) 
    {
      std::cout << "Initial position for joint " << joints_[i].getName() << ": " << joints_[i].getPosition() << std::endl;
    }

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

    return true;
  }

I guess that the issue comes from a gazebo initialization problem of the robot or maybe its a controller loading issue... Can someone help me?