Description
Description
Joint velocities published to ROS are always 0, even when joints are moving.
Your environment
- ROS Distro: Foxy
- OS Version: Ubuntu 20.04
- Source or Binary build? moveit2_tutorials @69a86aca , ros2 foxy installed from
apt
Steps to reproduce
[Terminal 1] Launch move_group: ros2 launch moveit2_tutorials move_group.launch.py
[Terminal 2] Start monitoring: ros2 topic echo /joint_states
[Terminal 3] Start monitoring: ros2 topic echo /dynamic_joint_states
[Rviz or any other method] Move the manipulator: e.g., by setting Goal State to "random valid" in the MotionPlanning panel of rviz, and then commanding "Plan & Execute".
Expected behaviour
The /joint_states and /dynamic_joint_states topics get messages with valid position and non-zero velocity while the arm is moving.
Backtrace or Console output
This rosbag file includes all messages published to those two topics.
We can see that position values change properly, but all velocities are always 0.
$ ros2 topic echo /joint_states
...
header:
stamp:
sec: 1645146995
nanosec: 32057561
frame_id: ''
name:
- panda_finger_joint1
- panda_joint1
- panda_joint2
- panda_finger_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
position:
- 0.0
- -2.217043304508542
- 0.029448564111580405
- 0.0
- 1.4100945255919148
- -0.9100914599177757
- -1.8063640432500931
- 1.9718506888883238
- -1.1409078693760133
velocity:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
effort:
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
---
$ ros2 topic echo /dynamic_joint_states
header:
stamp:
sec: 1645147397
nanosec: 20005934
frame_id: ''
joint_names:
- panda_finger_joint1
- panda_joint1
- panda_joint2
- panda_finger_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
interface_values:
- interface_names:
- position
- velocity
values:
- 0.0
- 0.0
- interface_names:
- position
- velocity
values:
- -2.7295728030378457
- 0.0
- interface_names:
- position
- velocity
values:
- 0.2177303686632308
- 0.0
- interface_names:
- position
- velocity
values:
- 0.0
- 0.0
- interface_names:
- position
- velocity
values:
- 1.7360759976772124
- 0.0
- interface_names:
- position
- velocity
values:
- -0.5758306101361987
- 0.0
- interface_names:
- position
- velocity
values:
- -2.2239539276540743
- 0.0
- interface_names:
- position
- velocity
values:
- 2.064518162790732
- 0.0
- interface_names:
- position
- velocity
values:
- -1.5861335411062927
- 0.0
---
The joint controller (panda_arm_controller) does recognize both position and velocity state interfaces:
$ ros2 param get /panda_arm_controller state_interfaces
String values are: ['position', 'velocity']
so that in the code, has_velocity_state_interface_ is true, and the velocity values are populated with the current velocities read from the hardware.
By back-tracing, I found that JointTrajectoryController::read_state_from_hardware retrieves the current states including the velocities from joint_state_interface_[1], but I'm lost here as to where this reference object is being populated. Anyway, it's getting zeros for the velocities, which appears to be a bug.
Could you have a look at this? Thanks!