Handle TCP frame in FK solver and controller methods

[PeterBowman]

As a follow up to #121, we now may use the diffInvKin and diffInvKinEE methods from ICartesianSolver to issue a few commands in the cartesian space referring to the end-effector/TCP frame. However, some other depend on FK/IK transformations (i.e. non-differential kinematics), which do not support TCP frames yet.

That being said, we'll probably need the end-effector counterparts of invKin and fwdKinError (and invDyn?).

[PeterBowman]

Still WIP, but diffInvKin and diffInvKinEE have been already merged into one method, which now accepts ICartesianSolver::reference_frame as a parameter. No further changes here. Regarding the other two methods:

• invKin: similarly to diffInvKin, users may pass the reference frame in which the input pose should be expressed.
• fwdKinError: I dismissed my original idea since the input cartesian pose should always be a static reference, i.e. it doesn't make sense to express it in terms of the TCP frame as it would keep changing during the execution of the trajectory.

To bypass the latter, I propose yet another solver method: changeReferenceFrame (name may change).

kinematics-dynamics/libraries/YarpPlugins/ICartesianSolver.h

Lines 64 to 80 in 9643744

	/**
	* @brief Express given pose in other reference frame
	*
	* @param x_in 6-element vector describing a pose in cartesian space, expressed in @p currentFrame;
	* first three elements denote translation (meters), last three denote rotation in scaled
	* axis-angle representation (radians).
	* @param currentQ Vector describing current robot position in joint space (degrees).
	* @param x_out 6-element vector describing a pose in cartesian space, expressed in @p newFrame;
	* first three elements denote translation (meters), last three denote rotation in scaled
	* axis-angle representation (radians).
	* @param currentFrame Points at the @ref reference_frame the input pose is expressed in.
	* @param newFrame Points at the @ref reference_frame the output pose should be expressed in.
	*
	* @return true on success, false otherwise
	*/
	virtual bool changeReferenceFrame(const std::vector<double> &x_in, const std::vector<double> &currentQ,
	std::vector<double> &x_out, reference_frame currentFrame, reference_frame newFrame) = 0;

[PeterBowman]

BTW I'd probably need to update ICartesianSolver's docs so that the reference frame of input/output pose vectors is explicitly stated.

[PeterBowman]

Both available implementations of changeReferenceFrame are prepared to handle a world frame, besides the currently available base and TCP frames (ICartesianSolver::BASE_FRAME and ICartesianSolver::TCP_FRAME, respectively).

Regarding world and TCP frames, let's discuss a reasonable way of handling the H0 and HN .ini config parameters (currently used in KdlSolver, still pending for AsibotSolver at #112).

[jgvictores]

Ref: "una imagen vale más que mil palabras"