Cartesian-related interfaces should be representation-agnostic

[PeterBowman]

Currently, the class relationship tree for cartesian control-related interfaces may force its intermediate actors to make assumptions about angle representation up to four times, that is, from the perspective of:

1. the client (application) code, say a module that needs to interface with our cartesian controller (e.g. the CartesianControlClient), or pipe directly to a solver device (KdlSolver, AsibotSolver)

2. the cartesian controller (ICartesianControl)

3. the cartesian solver, called by 2. or as a stand-alone device as in 1. (ICartesianSolver)

4. the trajectory generator, so far only used by 2. (Trajectory)

All of these make use of generic containers in their public interface, e.g.:

// ICartesianControl
bool movj(const std::vector<double> &xd);

// ICartesianSolver
bool appendLink(const std::vector<double>& x);

// Trajectory
bool getX(const double movementTime, std::vector<double>& x);

Since the std::vector<double> parameters carry no information about the angle representation, clients may not encode said vectors at their convenience; instead, they must conform to the angleRepr .ini parameter used to consistently instantiate the whole chain (usually passed to an implementor of ICartesianControl, like BasicCartesianControl). Note that it didn't work that way prior to #101 as axisAngle was virtually hardcoded. This is equally true for intermediate classes, e.g. BasicCartesianControl::inv talks to a ICartesianSolver::invKin and the vector passed between them (ref) must be kept coherent with the chosen representation.

Another point of grief is the lack of uniformity across different methods within each of the aforementioned interfaces. Compare:

• KdlSolver::diffInvKin - one may not notice this at first, but the KDL implementation forces callers to pass a std::vector encoded as a velocity twist (i.e. scaled axis-angle notation, ref). Similarly, the return parameter of KdlSolver::fwdKinError is another twist-encoded container (ref); however, its first input parameter is a "partially" constrained vector, in a manner such that callers are not free to decide on which representation to work with, yet it's not fully hardcoded - here, it depends on the device initialization via CLI/.ini file.

• LineTrajectory::getX vs LineTrajectory::getXdot - the latter forces clients to adhere to velocity twists since this is the angle representation used in the CMC thread of BasicCartesianControl (ref). Now, we may develop a full picture of this mess:

  trajectory.getX(movementTime, desiredX); // returns 'desiredX', angle-repr as specified in .ini
  trajectory.getXdot(movementTime, desiredXdot); // returns 'desiredXdot' - this is always a velocity twist
  // ...
  iCartesianSolver->fwdKinError(desiredX, currentQ, commandXdot); // pass .ini-controlled 'desiredX', returns (infinitiesimal) displacement twist in 'commandXdot'
  // ... finishes performing numerical differentiation on 'commandXdot'
  if (! iCartesianSolver->diffInvKin(currentQ, commandXdot, commandQdot) ) {} // accepts velocity twist

Method calls may seem quite clean by now in BasicCartesianControl/ICartesianControlImpl.cpp, but clashes between representations already arised while working on #105 and #86, and also motivated #107.

At this point, we may want to distinguish what kind of representations are considered here:

• Linear vs angular:

  • linear: ATM only cartesian, at some point we may want to explore cylindrical and spherical coordinates
  • angular: currently the ones handled by KdlVectorConverter with more to come (see velocity below)

• Position vs velocity (vs acceleration?):

  • position: as in KdlVectorConverter, used to describe a pose
  • velocity: integrating factors used to express rotational velocity in terms of an angle set and vice-versa

---

Proposed solutions:

1. Force all interfaces to accept the same representation, ideally axisAngle (scaled) as it is the one used by KDL internals.
2. Improve KdlVectorConverter to handle all cases of linear/angular position/velocity(/acceleration) representations, make it store positions/angles and pass it as a parameter type to all interface methods listed above (instead of std::vector). Callers and callees will act as encoders and decoders of those representations, respectively (e.g. store our data as eulerZYZ, pass it through an interface, unpack it as axisAngle on the callee's side). Probably, this class would have to implement yarp::os::Portable in order to make itself readable and writable on the YARP network.

[PeterBowman]

I'll give the second option a try. @jgvictores I'd greatly appreciate any idea or objection you could come up with, I'm a bit in a hurry to have this closed and move on to the next point. Perhaps you already run into any of this; for instance, I noticed that transCoordsUsingJoints is ignoring orientation values at PremultPorts.cpp#L54.

[jgvictores]

The second option looks interesting, it would be great if you could come up with some implementation and see how it works (not too sure about inheriting from any yarp class, leaving that to your best decision).

[PeterBowman]

After some thought, I've changed my mind. Let's start by asking ourselves why client applications should ever know which angle representation is being used by the cartesian controller (which boils down to the question: what value of the --angleRepr parameter has been passed to BasicCartesianControl upon initialization?).

I've prepared a poor drawing of two valid use cases, please bare with me:

We distinguish between C++ client applications (e.g. a yarp::os::RFModule) and direct RPC communication established via terminal. The former will talk to the server part through the CartesianControlClient (CCC), then both applications connect to the server ports (CCS: CartesianControlServer) and interact with the controller (BCC: BasicCartesianControl).

Now, let's focus on the data flowing through each of these channels. To conform with the ICartesianControl interface (same will apply to ICartesianSolver and Trajectory), generic containers of the class std::vector<double> are currently used, and hold (this should be noted in the documentation) two packs of values: cartesian coordinates (3 elements) + orientation in the selected angular representation (size varies, supported conversions store between 2 and 4 elements).

My revisited approach targets the distinction in client usage: since beforehand knowledge of the angle representation a BCC has been started with is only relevant for manual interaction via terminal, don't enforce hardcoding such information in C++ client applications (which would clearly fail, should the BCC start with an --angleRepr value they don't support). To solve this, some kind of utility class could handle data conversion between the C++ client and the YARP network, and ignore it when talking directly to the server port via RPC (see green annotations on my drawing). The easiest way I found to implement such distinction comprises the creation of a new server port /rpc_transform:s opened on demand (i.e. if --angleRepr was passed to the cartesian controller plugin), independent from /rpc:s, which in turn would stick to a standard representation used by the controller itself and downstreams (solver, trajectory generator and so on).

To summarize, human users can talk to /rpc_transform:s via terminal in an angle representation they understand best; on the other hand, C++ applications should use /rpc:s and apply any conversion needed to conform to their inner algorithms (note that same applies to cartesian solvers). Additionally, I'd make the CartesianControlClient accept a --transform parameter to connect to /rpc_transform:s instead of /rpc:s for backwards compatibility (or should we ever need this feature).

To implement previous ideas, one may conclude that the C++-apps-way force communication channels to either share information about the angle representation, or stick to one and use it everywhere.

// IFancyInterface
bool myFancyMethod(???); // what should we pass here?

To answer the question in the inlined comment, three alternatives are proposed:

1. Generic std::vector<double> container, holds cartesian coordinates and orientation (implicitly assumes always the same angle representation, to be noted in docs).

2. Generic std::vector<double> container, holds cartesian coordinates, orientation and choice of angle representation.

3. Some kind of new structure class, holds cartesian coordinates, orientation and choice of angle representation. Consider inheriting from yarp::os::Portable to handle conversion of stored data to bottles within CCC/CCS.

I'd pick the first solution as a first approach, due to its simplicity and use of STL classes (instead of something that may depend publicly on KDL/YARP). Also, no angleRepr data means that we don't need to worry about passing an identifier to incoming/outgoing bottles in CCC/CCS. The obvious consequence is that implementors of ICartesianControl et al. must know which angle representation to use; I'd choose 6-element twists (scaled axis-angle) as it's the standard embraced by KDL (and participates in most geometric calculations, e.g. in twist Jacobians).

[PeterBowman]

@jgvictores please check #114.

[jgvictores]

@PeterBowman 👍

[PeterBowman]

Follow-up announcement: @roboticslab-uc3m/teo developers should note that the /rpc:s port is no longer handling vectors in the representation given by --angleRepr; besides, this option is not recognized anymore by cartesian solvers (e.g. KdlSolver) and can be safely deleted from .ini files (teo-configuration-files/share/kinematics/). Now, users should explicitly launch the cartesian controller plugin with an angle representation of their choice, for example:

yarpdev --device BasicCartesianControl --name /teoSim/rightArm/CartesianControl --from /usr/local/share/teo-configuration-files/contexts/kinematics/rightArmKinematics.ini --robot remote_controlboard --local /BasicCartesianControl/teoSim/rightArm --remote /teoSim/rightArm --angleRepr axisAngle

...and connect via RPC to the new /rpc_transform:s port:

yarp rpc /teoSim/rightArm/CartesianControl/rpc_transform:s

In case angleRepr is missing or not recognized, no /rpc_transform:s port will be opened. C++ applications may talk through this new port instead of the standard /rpc:s if --transform is provided (see example app in /example/cpp/cartesianControlExample/).

[jgvictores]

Edited out duplicate --angleRepr axisAngle above at #113 (comment)