Extending `servo` commands

[adeguet1]

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 CartesianSetpoint with:
  • pose and pose_defined
  • twist and twist_defined
  • wrench and wrench_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.

[adeguet1]

• JointSetpoint is a bad name, can be confused with result from setpoint_js (same for c). Could use JointServo.
• pose_error as double?
• For cartesian state, which Jacobian? And for servo_cx, do we add a identifier (string, enum, char) in message to determine the frame of reference (body vs spatial/world) or do we body/servo_cx? In ROS, header.frame_id could be used?