CartesianControlServer should handle streaming commands

[PeterBowman]

This was implemented on CartesianServerLib at asibot-main (see xCallbackPort.cpp), but never made its way to CCS. The vmos command - velocity control in cartesian space - seems the most interesting option to start working at.

[PeterBowman]

Started with some refactoring at the streaming-cartesian-server branch.

[jgvictores]

Great to see improvements in CartesianControlServer. In #102 we're working on another improvement, I'll comment there.

[PeterBowman]

vmos is mostly implemented. I've created the streamingSpnav program (109ec2c at streaming-spnav branch) to give it a try - it works fine on simulator with ASIBOT (default) and TEO, we are about to test it on real ASIBOT (brace yourselves). You'll need a SpaceNavigator 3D mouse or similar (current implementation launches SpaceNavigator from yarp-devices, but creating your own yarp::dev::AnalogSensor device may be trivial).

@jgvictores the current WIP list for old xCallbackPort commands is: fwd, bkwd, rot, pose (and the first one, left without a name). Is there any other streaming command you are especially keen on having brought to kin-dyn?

[jgvictores]

@jgvictores the current WIP list for old xCallbackPort commands is: fwd, bkwd, rot, pose (and the first one, left without a name). Is there any other streaming command you are especially keen on having brought to kin-dyn?

I think the current WIP is great!

[PeterBowman]

The current WIP list is:

// move forward (relative to end-effector)
void fwd(const std::vector<double> &rot, double step);

// move backwards (relative to end-effector)
void bkwd(const std::vector<double> &rot, double step);

// rotate in end-effector frame
void rot(const std::vector<double> &rot);

// pan in end-effector frame
void pan(const std::vector<double> &transl);

// instantaneous velocity steps (inertial frame)
void vmos(const std::vector<double> &xdot);

// instantaneous velocity steps (end-effector frame)
void eff(const std::vector<double> &xdotee);

// achieve pose in inertial frame
void pose(const std::vector<double> &x, double interval);

See also full documentation here:

kinematics-dynamics/libraries/TeoYarp/ICartesianControl.h

Lines 239 to 339 in 2fd0b37

	/**
	* @brief Move forward (relative to end-effector)
	*
	* Move along the Z (positive) axis in velocity increments, applying desired angular
	* velocities. All coordinates are expressed in terms of the end-effector frame.
	* Negative step values will be ignored.
	*
	* @param rot 3-element vector describing desired angular velocity increments in
	* cartesian space, expressed in radians/second.
	* @param step Velocity step corresponding to Z axis, expressed in meters/second.
	*
	* @see bkwd (move backwards)
	* @see eff (rotations + translations)
	*/
	virtual void fwd(const std::vector<double> &rot, double step) = 0;
	/**
	* @brief Move backwards (relative to end-effector)
	*
	* Move along the Z (negative) axis in velocity increments, applying desired angular
	* velocities. All coordinates are expressed in terms of the end-effector frame.
	* Negative step values will be ignored.
	*
	* @param 3-element vector describing desired angular velocity increments in
	* cartesian space, expressed in radians/second.
	* @param step Velocity step corresponding to Z axis, expressed in meters/second.
	*
	* @see fwd (move forward)
	* @see eff (rotations + translations)
	*/
	virtual void bkwd(const std::vector<double> &rot, double step) = 0;
	/**
	* @brief Rotate in end-effector frame
	*
	* Apply desired angular velocities, but avoid translating, that is, only change the
	* orientation of the TCP. All coordinates are expressed in terms of the end-effector frame.
	*
	* @param 3-element vector describing desired angular velocity increments in
	* cartesian space, expressed in radians/second.
	*
	* @see pan (only translations)
	* @see eff (rotations + translations)
	*/
	virtual void rot(const std::vector<double> &rot) = 0;
	/**
	* @brief Pan in end-effector frame
	*
	* Apply desired linear velocities, but avoid rotating, that is, only change the position
	* of the TCP. All coordinates are expressed in terms of the end-effector frame.
	*
	* @param transl 3-element vector describing desired translational velocity
	* increments in cartesian space, expressed in meters/second.
	*
	* @see pan (only rotations)
	* @see eff (rotations + translations)
	*/
	virtual void pan(const std::vector<double> &transl) = 0;
	/**
	* @brief Instantaneous velocity steps (inertial frame)
	*
	* Move in instantaneous velocity increments using the fixed (inertial) frame as the
	* reference coordinate system.
	*
	* @param xdot 6-element vector describing velocity increments in cartesian space;
	* first three elements denote translational velocity (meters/second), last three
	* denote angular velocity (radians/second).
	*
	* @see eff (end-effector frame)
	*/
	virtual void vmos(const std::vector<double> &xdot) = 0;
	/**
	* @brief Instantaneous velocity steps (end-effector frame)
	*
	* Move in instantaneous velocity increments using the end-effector frame as the
	* reference coordinate system.
	*
	* @param xdotee 6-element vector describing velocity increments in cartesian space;
	* first three elements denote translational velocity (meters/second), last three
	* denote angular velocity (radians/second).
	*
	* @see vmos (inertial frame)
	* @see fwd, bkwd, rot, pan (only translations/rotations)
	*/
	virtual void eff(const std::vector<double> &xdotee) = 0;
	/**
	* @brief Achieve pose in inertial frame
	*
	* Move to desired position, computing the error with respect to the current pose. Then,
	* perform numerical differentiation and obtain the final velocity increment (as in @ref vmos).
	*
	* @param x 6-element vector describing desired instantaneous pose in cartesian space;
	* first three elements denote translation (meters), last three denote rotation (radians).
	* @param interval Time interval between successive command invocations, expressed in seconds
	* and used for numerical differentiation with desired/current poses.
	*/
	virtual void pose(const std::vector<double> &x, double interval) = 0;

As you can see, I added pan and eff to the initial list. The former acts just like rot, but only with linear translations (while rot changes orientation about the same point in space). The latter is exactly the same as vmos, but it moves in the end-effector frame.

Additionally, I've introduced the step parameter at fwd and bkwd to provide a means of controlling the step velocity at which the robot arm moves forward or backwards. Furthermore, pose is a bit different from the original command at asibot-main: it overrides the trajectory generator and applies inverse differential kinematics on the velocity obtained by numerical differentiation between current and target poses, using interval as the time step (see BasicCartesianControl/ICartesianControlImpl.cpp).

The unnamed command has been set aside by now - I'd like to improve control with the IPositionDirect interface, although I'm a bit worried about computational power (the controller would perform inverse kinematics every 10 milliseconds).

For completeness, note that end-effector frame commands require a new solver method, ICartesianSolver::diffInvKinEE:

kinematics-dynamics/libraries/TeoYarp/ICartesianSolver.h

Lines 130 to 144 in 2fd0b37

	/**
	* @brief Perform differential inverse kinematics on end effector
	*
	* Same as @ref diffInvKin, but the input velocity is expressed in terms of the
	* end effector frame.
	*
	* @param q Vector describing current position in joint space (degrees).
	* @param xdotee 6-element vector describing desired velocity in cartesian space,
	* expressed in the frame of the end effector; first three elements denote translational
	* velocity (meters/second), last three denote angular velocity (radians/second).
	* @param qdot Vector describing target velocity in joint space (degrees/second).
	*
	* @return true on success, false otherwise
	*/
	virtual bool diffInvKinEE(const std::vector<double> &q, const std::vector<double> &xdotee, std::vector<double> &qdot) = 0;

[PeterBowman]

@jgvictores I'm a bit worried about the amount of commands resulting from this, some of them apparently redundant. There are three main variants: RPC vs streaming commands, position vs velocity control (vs torque), and inertial vs end-effector frame. And there is more to come: I'd find quite useful a movv RPC command using end-effector coordinates for my keyboardController application, but I'm running out of ideas for choosing the best name (move? movve?).

Are you OK with adding new commands as they render convenient for our applications, or is it preferable to keep the lowest number of them and parameterize if possible?

I was thinking about a new interface (inheritable by BasicCartesianControl and the like) for managing controller configuration during runtime. In fact, it would be nice to have the ability to select a different gain, tweak the maximum joint velocity and trajectory time (along the lines of #46), change the trajectory generator, enable RPC/streaming mode, or even choose the reference frame for input coordinates. Do you think it's feasible?

[PeterBowman]

Resolved at #117, improvements will be tracked in several follow-up issues:

• Monitor internal state in BasicCartesianControl/CartesianControlServer #118 - simultaneous execution of RPC and streaming commands
• Implement streaming command in position-direct mode w/ fast solvers #119 - implement last streaming command inherited from asibot-main
• Use UDP carrier in streaming contexts #120 - use UDP carrier instead of TCP
• Let users access and modify low-level controller configuration #121 - parameterize reference frames to shrink the list of commands and avoid duplicity