Cartesian velocity control of robot arm

edit

I actually found a good solution to this in the code for pr2_teleop_general. The basic idea is to:

1. Grab the current robot state using RobotCommander.get_current_state()
2. Call the GetPositionFK service of your move_group to get the the end effector position using this state as input.
3. Scale your Cartesian velocity by a constant and add it onto this position. Call the GetPositionIK service of your move_group to get an inverse kinematics solution for this.
4. Subtract your current joint angles from the IK joint angles, and multiply by some constant to get a velocity for every joint.