I'm trying to control the toyota HSR with a higher level whole-body controller that computes velocities, but the robot only offers JointPositionInterfaces in it's ros_control HardwareInterface implementation.
As none of the standard ROS controllers accept a velocity and output a position my approach so far included to write an integrator as an ROS controller plugin that continuously integrates the velocity command into an position set point.
This works more or less, but some of the joints react with shaky movements to this, and none of the joints reach the desired velocity.
I would assume that i have to tune the integrator gain for each joint to track the desired velocities, but i'm wondering if there exist other methods to achieve this?
As the robot offers position and velocity feedback i was thinking about using the velocity error to improve the position set point calculation, but somehow do not think that a classical PID loop will work. Has anyone encountered a similar situation or knows some literature on that topic?