How to get joint velocity from Moveit 2 Robot State?
Is it possible to get the velocity of each joint using Moveit 2 Robot State? Is there any better approach?
I tried the following:
robot_model_loader::RobotModelLoader robot_model_loader(node);
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("my_arm");
const moveit::core::RobotState current_robot_state = moveit::core::RobotState(kinematic_model);
auto velocities = current_robot_state.getVariableVelocities();
// velocities = garbage
std::vector<double> actualVelocity;
current_robot_state.copyJointGroupVelocities(joint_model_group, actualVelocity);
// actualVelocity = garbage also
I can get the joints positions without any problem. The velocity is published to /joint_states
topic correctly.
The only solution is subscribe to /joint_states
?