Description
The current servo_jp and servo_cp commands only use a position setpoint. Since these commands are often sent directly to a PID controller (low-level), it would make sense to also add a velocity setpoint along the position one. This would greatly improve trajectory following. By extension, we could also consider sending an effort setpoint as a feed-forward.
The user would then be allowed to send p, v, f (or effort) to the low-level controller. The output would be f = P * (setpoint_p - measured_p) + D (setpoint_v - measured_v) + setpoint_f.
Depending on which values are provided, the behavior can be summarized in the following table.
| p | v | e | Meanning |
|---|---|---|---|
| 0 | 0 | 1 | servo_{j,c}f |
| 0 | 1 | 0 | servo_{j,c}v |
| 0 | 1 | 1 | |
| 1 | 0 | 0 | servo_{j,c}p |
| 1 | 0 | 1 | |
| 1 | 1 | 0 | servo_{j,c}pv new |
| 1 | 1 | 1 | servo_{j,c}pvf new |
To extend the current CRTK convention, we can either add more information for the existing servo_{j,c}p commands or add one or more new commands with new payloads. Changing the payload would not be backward compatible so it is not the preferred solution.
This being said, for the ROS implementation of servo_jp, we use the JointState message which does contain 3 vectors, position, velocity and effort so it is possible to add this feature without breaking the API (we can use the vectors size to determine which setpoints are provided).
For servo_cp, since we use the PoseStamped, there is no way to add the feature without a new command. For the cartesian command, we propose to add:
- Command
servo_cpvf - Message
CartesianSetpointwith:poseandpose_definedtwistandtwist_definedwrenchandwrench_defined
For consistency's sake, we might also add the command servo_jpvf and the message type JointSetpoint. We could also add a CartesianState to mirror the JointState.