The data about the current position of the robot (UR5) changes with a lower frequency than that with which the pose is commanded

edit

I want to evaluate how good the UR5 follows the commanded trajectory. The cartesian motion controller: link text is used in this specific task. I created a .txt file in which I defined the exact poses that the UR5 must have to describe a square. Each pose is given as an input to the controller with a frequency of 120 Hz. To evaluate the effective robot pose I created a TransformListener object and, with a frequency of 120 Hz, I queried the listener for a specific transformation, as follow:

geometry_msgs::TransformStamped transformStamped;
try{
    transformStamped = tfBuffer.lookupTransform("base", "tool0", ros::Time(0));
}
catch (tf2::TransformException &ex) {
    ROS_WARN("%s",ex.what());
    ros::Duration(1.0).sleep();
    continue;
}
reached_pose.publish(transformStamped);

This information is published on the topic /reached_pose (created for this scope) with a frequency of 120 Hz.

I use the rosbag package to record two pieces of information:

• The commanded poses from the /my_cartesian_motion_controller/target_frame topic
• The reached poses from the /reached_pose topic

Plotting the results, I noticed that the poses commanded to the UR5 and the poses reached by the robot are published on the topics with a correct frequency of 120 Hz. Hovewer, the information about the present state of the robot remains the same for 5/6 samples despite the commanded pose changes. Seems that the listener returns the latest available transform in the buffer, but the buffer is updated with a frequency of 20 Hz.

In order to publish a tf for each of the received joint_states, I set the parameter "ignore_timestamp" of "robot_state_publisher" to "true"; in this way the "publish_frequency" and the timestamp of joint_states are ignored. The situation is the same. joint_state works at 125 Hz.

What could be the problem?

Thanks!