Real time joint-speed control

edit

Hi everyone.

I wanted to know how I can control a robot only with joint speeds by following some trajectory. I want to control a UR3, so I checked out ur_driver, ur_modern_driver, moveit.

So the work-flow I want to accomplish:

1. From a point in Cartesian space, calculate a goal configuration (with spatial similarity, so with little movement in the operational space)
2. Create some PID controller that takes care of calculating a trajectory and minimizing the error of goal to current position
3. At high frequency (125Hz) send out velocity commands to robot by following the trajectory and making sure that it actually minimizes the positional error, e.g. moves to the desired position.
4. If, at any point I want to change the goal position, recalculate 1-2 and start approaching the new goal in 3.

The driver already gives me a topic I can advertise joint velocities to. So ideally I would want to send out something like this

..., joints=[0,1,2,3,4,5], velocity = [my_traj(current_joint_pos[0], 0), ..., my_traj(current_joint_pos[5], 5]), ...

Where current_joint_pos is an array of the current position of the joints and my_traj gives out a smooth velocity curve (as fast as some threshold or max_vel). I don't want to specify a time for the execution like I would if I would write a cubic polynomial. Because depending on the motion this could yield to unwanted velocities. I rather want to control the velocity and have it execute as fast as some set threshold or as fast as physically possible (although I know that time optimal control is not trivial).

Now, I have worked with ROS and mobile robots a lot before but real-time arm control is something new and I am a little bit confused by the possibility and ways to do it. It seems that there are several packages that do at least part of what I want but it is not apparent what is better and what is just old junk lying around. And also what can still give me real time control. E.g. moveit looks good, but seems to be more of a high-level-"I give control away to something I don't know and don't directly control" although I at this point don't necessarily want that. I rather want some high level concepts like PID/traj calculation, IK and then put everything together myself and send it out.

Furthermore when I calculated the IK geometrically by following the work of Hawkins, the at most 8 joint configurations don't always make sense for me, because for a very small displacement in operational space, the joint movements can be very high although a "closer" joint position is possible. (So the robot would move its whole base and shoulder and everything just to do a small motion with its end effector). So maybe a smarter or robust jacobian-based approach would be better here?