Controlling joints with velocity commands
In most cases, position controllers are the best solution to control a robot by just setting the joints to specified angle positions. However, in some cases it is better to use velocity controllers instead, and control the robot by sending velocity commands to the joints.
A good example is when executing a joint trajcectory. Of course it is possible to use position controllers and just set the joint positions of the subsequent trajectory points. However in this case the joints will move in-between trajectory points at any velocity determined by the PID controllers, the drivers, etc. So this will affect the overall duration of the trajectory, unless the execution is synchronized at the key frames (which in turn will affect smoothness of execution). If the trajectory was computed along with other moving targets, it is important to stick to the exact timings.
Not only the duration of the trajectory may be affected by altering joint velocities,
but also the accuracy: altering the velocities in-between key frames can lead to changes
in intermediate robot poses assumed at trajectory computation time.
Even if the changes are small, errors amplify down the kinematic chain, and
this can be crucial if the trajectory gets close to obstacles.
For example, one joint may have to move faster than another in order to reach
the target at the same time. When using position
controllers only, there is no direct control over which velocity the individual
joints will have - one joint may arrive at the next trajectory point before others.
This can change the expected robot pose in-between trajectory points quite dramatically.
Also, the importance to synchronize at trajectory points is very obvious in this example.
The synchronization can lead to a jerky trajectory execution if some
joints are faster or slower than the pre-computed velocities.
Controlling the arm with velocities instead does not eliminate the need to synchronize, but
it will make the overall execution much smoother.
Another drawback to position controllers in regards to trajectory execution is that for continuous joints, mostly the shortest distance to the target position is considered the direction to move the joint towards. But with collision-sensitive trajectory execution, sometimes the joints may actually be required to "go the long way round" in order to avoid obstacles. This can only be done by taking the target velocity directly specified in the joint trajectory into account.
Velocity controlled trajectory execution can also be much smoother because pre-computed trajectory velocity values often are to be interpolated between points (e.g. spline interpolation). This requires a fine-grained control of velocities in-between trajectory points (interpolation between positions can only be done linear for 1DOF joints). Execution of such interpolated trajectories is usually a lot smoother.
Cases when position controllers could still be OK
- Robot drivers which take simultaneous position commands for several joints and then determine that all targets have to be reached at the same time, and set the individual joint velocities accordingly, can support a more or less accurate trajectory execution with position commands. But that is very robot-specific.
- Very highly resolved joint trajectories can avoid the problem with large deviations from intermediate robot poses, as long as the execution is synchronized at trajectory points. In this case the individual joint velocities will just alter the total duration of the execution, unless a "wait" is incurred at key frames to play the trajectory in the given time. If the pre-determined trajectory timings are not met, collisions with other moving targets (e.g. a second moving arm) cannot be avoided.
To control an arm via velocity commands, using velocity controllers alone is not necessarily a viable solution. It may work on some robots, but on others (and from Gazebo 5 (?)) this may not work well at all. The reason why is best explained with an example.
Say we want to control the arm with velocities only. And we want it to maintain the current position by setting all velocities to 0.
This does not work well with PID controllers if setting the desired velocity value is not accurate and stable in the robot driver or Gazebo. Take the example:
- We want the arm to maintain a position in which it is stretched out, while it is also pulled down by gravity, so that it would fall down unless an "upwards" force (or velocity) is applied. This is shown in situation (1) in the image. We start with velocity v=0 in the horizontal position. Gravity G will now pull down the arm a tiny bit. Lets assume for this example the gravity will cause a small negative velocity v for the joint j.
- In the next PID update step, the arm will now have moved a tiny bit down due to gravity. This is displayed in situation (2) in the image, which exaggerates the amount of movement for illustration purposes. The joint sensors read a small negative velocity, so the PID controller now wants to adjust v with a positive "upwards" force in order to reach a velocity of 0 again. This is fine, because we want the arm to move up again. The amount of positive velocity is then determined, but in reality gravity will decrease the actual velocity just a tad, compared to the target velocity: the PID controller has to adjust for the effect of gravity over time. It cannot do this in the current step already, because the error is just being caused in the coming movement.
- So with the velocity positive again, the joint did cause an upwards movement, but the target velocity was not met exactly (the link has been unpredictably "dragged down" during movement). Therefore, it does not make it all the way back up to the 0 position - it would need at least one more time step to get back up. This would be fine in general, but: now that v is positive while the arm is moving back up, the PID demands a negative ("downwards") force for j again, to reach v=0. Now the trouble begins.
- We now even gave the joint an additional "downwards" spin with the negative force. Then we additionally have the gravity effect, which amplifies the downwards spin, just as it did in step (1). Even if this effect is very small, and tiny update steps are chosen, the effect will amplify over several iterations, eventually causing the arm to collapse.
And here we get the problem: The arm will slowly collapse, because it never gets "upwards" force applied for long enough in order for to the joints to counter the effects of gravity. The PID commands even reinforce the collapse movement. This is an unstable solution.
What is missing, is a reference at which pose to maintain the arm. This is where position controllers are good, because the PID commands are determined by the position which we want to maintain. Velocity controllers have no concept of a position, therefore they are not predestined at maintaining one. They are only good for maintaining a velocity which is continuously positive or negative and does not fluctuate around 0.
This issue can be alleviated by adding position controllers in addition to velocity controllers:
- Use position controllers to maintain the current position when there is either no velocity command for the joint, or the velocity command is 0 (or very close to 0).
- If there are non-zero velocity commands, set the effort/velocity based on the velocity command.
It is still possible for an external controller to send only velocity commands. The additional position controller can be maintained internally by the driver / Gazebo plugin (some robot arms may actually do this, I believe for example the Jaco arm drivers must be considering this, because I did not have this problem while working on the real arm). The last known position for each joint has to be remembered and held for as long as 0 velocity commands are arriving for that joint.
Note for Gazebo:
The ideal solution would be to re-implement (or extend) gazebo_ros_control::gazebo::DefaultRobotHWSim
to support the above functionanlity. Then, we could also use ros_control with
gazebo_ros_control for trajectory execution
and velocity control in Gazebo.
Mabye this can be combined with adding support in physics::JointController as well,
and making DefaultRobotHWSim use the model JointController.
Background
The reason behind the issue is that when setting a joint velocity, with some robots/drivers it will not be exaclty met - there will be some small deviations from it, which are large enough to cause the problem.
For example, in Gazebo, when calling Joint::SetVelocity() with physics/gravity enabled, the joint velocity is rarely met exactly. Also using Joint::SetForce() will not lead to stable velocity values.
Some robot drivers may not have that problem, and when setting a certain velocity, it may be extremely close to the velocity specified. However this is not the case with Gazebo 5.0, and probably not with some robot drivers as well.
Note:
Referring to Gazebo 5.0 at the time of writing. If I remember correctly,
in Gazebo 2.0 Joint::SetVelocity() set the joint velocity to exactly the given speed, so the issue
would not appear. However I will have to verify this again which I will hopefully get around to soon.
If that is the case, it is a good thing that Gazebo is now more realistic in its simulation.
However this also requires new (more realistic) joint controller implementations.