Make solver implementations more aware of their internal state

[PeterBowman]

Solver devices could store more information about their internal state, such as the angle representation (removed at roboticslab-uc3m/kinematics-dynamics#113 so that a single copy is managed by the server device and passed down the chain, i.e. controller & trajectory generator & solver), thus rendering more useful as standalone components not necessarily linked to a controller device at all times (cf. transCoords app).

One potential benefit of said awareness is related to the fact that users are now fully responsible for managing the right angle representation and its conversions between ICartesianSolver's standard format and whatever their app handles. In other words, spare them several annoying calls to KinRepresentation::encodePose and KinRepresentation::decodePose every time they want to interface with a solver.

As proposed by @jgvictores, there could be an intermediate layer between the actual ICartesianSolver interface and the implementation. In that vein, the former accepts the angle representation of choice, as specified by the user. The latter works with KDL standard (scaled axis-angle) or whatever suits it better, and in fact would behave as a ICartesianSolverRaw or such. The new layer may now hold the angle representation and use it to manage all conversions needed between interface-implementation. We already work with something similar in the server device, check the RpcTransformResponder conversion methods.

Several solutions were laid out at roboticslab-uc3m/kinematics-dynamics#113 and could be now considered again. My main concern, and one of the reasons behind how things are currently done, is that angleRepr will be supervised by several components, perhaps introducing duplicate routines and time overheads. Also, consistency must be kept when working with the full chain (server + controller + solver).

[PeterBowman]

Further ideas regarding internal state in solvers (per https://github.com/roboticslab-uc3m/teo-main/issues/15#issuecomment-367320582):

@PeterBowman I'd like to discuss the possibility of removing joint limits from the kinematics .ini used from the solver:

Those .ini values are not used at all, so IMO it's totally reasonable to remove them. Speaking of current cartesian controllers, I'd consider opening the robot device before the solver one, pick actual robot joint limits via IControlLimits interface, and pass them later to the solver upon configuration (e.g. solverOptions.put("min", "whatever")).

One thing to sort out is: should joint limits be constrained as read-only properties in ICartesianSolver?

• Yes, we set them upon device initialization and never modify their values again. ICartesianSolver::setLimits could be either removed or rewritten to hold the corresponding class invariants (= report failure if called more than once, which is perhaps a bit against the purpose of said method).
• No, keep ICartesianSolver::setLimits so that users may call it at will on runtime, but take some care to ensure it'll never fall into race conditions. Similarly, variables holding the kinematic chain have been guarded against such circumstances, recently.

[jgvictores]

1. I agree with the general statement Make solver implementations more aware of their internal state.
2. We have allowed tool changing during runtime, and this is good because this is frequent in robotics. Regarding joint limits, I believe initial setup would be enough, as in solverOptions.put("mins", "(val1 ... valn)"). The implementation side is easier as we avoid race conditions, and it is justified because we have no use case or potential use case with varying joint limits. The only end-user inconvenience is that the solver cannot be initialized until after connecting to a robot and getting joint limits. Not bad, and reasonable practice, IMHO. Regarding the treatment, two options:
   1. Draconian. Force user to establish limits, else fail.
   2. Fall back to +-180 degree defaults or similar, simply CD_WARNING (may pass unperceived anyway).

[jgvictores]

Blocks roboticslab-uc3m/kinematics-dynamics#118.

[PeterBowman]

may pass unperceived anyway

Indeed, I forgot to call setLimits in TransCoords::configure. Ouch.

[jgvictores]

+1 to draconian.

[PeterBowman]

With roboticslab-uc3m/kinematics-dynamics#146, I started using yarp::os::Value::makeList(const char *) to populate a bottle of minimum or maximum joint values, thus simplifying the process of working with bottles and values (yarp::os::Property::put requires a pointer to a Value). Example:

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/7ea960b810161ae0d398b3c668b3544683cac49f/programs/transCoords/TransCoords.cpp#L98-L99

However, I didn't realize that this kind of creator funcion uses dynamic allocation, and returns a pointer to an object that needs to be destroyed further on. In other words, without a delete instruction, memory leaks are likely to happen. Check docs and source code.

[jgvictores]

I didn't realize that this kind of creator function uses dynamic allocation, and returns a pointer to an object that needs to be destroyed further on. In other words

Ok, yes, that is important!

[PeterBowman]

However, I didn't realize that this kind of creator funcion uses dynamic allocation, and returns a pointer to an object that needs to be destroyed further on. In other words, without a delete instruction, memory leaks are likely to happen. Check docs and source code.

It's fine. Per yarp::os::Property::put(const ConstString &, yarp::os::Value *):

After the association find(key) will return that value. The Property object is responsible for deallocating the value. If key is already associated, the value will be replaced with the new one.

[PeterBowman]

[jgvictores]

I've been thinking... Perhaps the CCRH wouldn't even need an additional interface class, since there is only going to be one, right? (would inherit from DeviceDriver, but then be view()ed directly)

[jgvictores]

There are actually 3 possiblites:

1. What I just proposed, which is an intermediate proposal.
2. Not a YARP device at all, just a helper class. Would provide the cleanest syntax, but would require the end-user most code changes to switch between RH and non-RH.
3. What @PeterBowman proposed, ICCRH : public ICC would perhaps provide the interface with an API in line with the rest, and only add that single line of code. For now, I think I'd go with this one.

[PeterBowman]

91fec270146b28def7635443666f64b02c8d6fc7 has been partially reverted in 4557220c504b9d5f535d4c7b5c5b7295ca8cb560 because of AMOR returning 0.0 as minimum and maximum joint limits (check roboticslab-uc3m/amor-yarp-devices#2).

[PeterBowman]

@jgvictores after having taken another look at my fix-145-solver-state branch, I think it could do as a starting point for implementing the concepts behind https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/145#issuecomment-370009336. So far, an abstraction layer (following the proxy pattern) has been put on top of current solver code (AsibotSolver + KdlSolver), but there is some duplication that should be refactored. Once done, there it goes: ICSRH + ICS.

Sorry for the mess, let's discuss the next steps face to face.

[jgvictores]

Cool. IMHO a representation layer over the controller was a slight higher priority than over the solver,, but they are not redundant nor mutually exclusive. PS: IMHO they are not ultra-high priority tasks, but it's also nice to see some progress. Code can help see design decisions we are making and prove if they are in a right/wrong direction.

[PeterBowman]

There is much more work to do. I started from the bottom of the chain (cartesian solver), but all components should be made representation-aware before merging anything. Also, we need to discuss the main shortcoming of the current solution (#113), which went largely unnoticed: transformations of velocities and accelerations.

[PeterBowman]

Let me sum this up.

Problem: historically, we used to mix different geometrical representations up. Orientations were inconsistently represented in the axis-angle, scaled axis-angle and eulerXY conventions, among others (https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/113). This got sorted out and explicitly documented - our controller/solver interfaces want scaled axis-angle no matter what. However, clients (mostly existing code) may request a specific representation that differs from the standard. Also, new codebase (namely the soft-neck project) demanded more flexibility and additional geometric transformations.

Current situation: the NWS, CartesianControlServer, accepts specific parameters regarding coordinate system, angle representation and angular units (ref). Besides the standard /rpc:s port, another port /rpc_transform:s is opened if any of these parameters is requested. All incoming and outgoing data is processed so that clients always get what they demanded, despite of solvers still consuming scaled axis-angle under the hood. Conversion is undergone via KinematicRepresentationLib. The CartesianControlClient NWC may be instructed to connect to this non-standard port for full flexibiilty. Moar context: https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/143#issuecomment-393621545.

Now, the question is: if consumers demand a different geometrical representation, i.e. scaled axis-angle does not fit their needs, who must be placed the burden on in order to perform the necesssary conversions?

1. The consumers (clients of our code). Let them suffer cope with the intricacies of turning whatever they came up with intro scaled axis-angle on their own end. We already provide the KinematicRepresentationLib for whoever wants to use it.
2. The NWS (CartesianControlServer), i.e. stick to the current solution. A single point is responsible of managing the conversion details at the cost of clients having to know that a different RPC port is used and that the NWC needs to be configured with --transform. Also, currently there is no conversion enabled in the streaming port!
3. The wrapped cartesian controllers (e.g. BasicCartesianControl). They already need to handle several details and interface with the wrapped solver device.
4. The solver devices (e.g. KdlSolver). This is current WIP on an experimental branch.

I liked 2 since nothing else needs to change, whereas 3 and 4 are prone to code duplication (easy to solve with a refactorization and additional layers, but it doesn't make me feel good...). I'm considering either droping 2 in favor of 1, or improving 2 so that representation shenanigans are also spanned to streaming commands. In that latter case, perhaps /rpc_transform:s should be dropped altogether so that both /rpc:s and /command:i permeate custom representation data.

Another idea: configure a protocol check on NWC/NWS. It might be possible that an unaware client connects to a specific server that expects a non-standard representation. To prevent havoc from ensuing, consumers may be forced to configure the NWC in the exact same way, and this information could be compared against the server in a similar way that a protocol check works.

[PeterBowman]

ASWJ @jgvictores we have decided to remove the hackish --transform from the NWC (https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/92dfaed1592a1c6e2ce451f6249397bf5fc67047) and leave the NWS unchanged. In this way, there is no violation of the public interface (we'll always want a specific, standard representation to be used as input/output). Still, we allow users to make the cartesian server open an additional, optional /rpc_transform:s port configured as they wish. This is intentionally left to facilitate an easy access to cartesian commads via terminal. Our C++/Python examples have been updated accordingly (https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/cb2b185d8d3b08f40f5f8c203f79ebc4a039c351).

The legacy "fix-145-solver-state" WIP branch will be deleted now: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/6d252ce41c96f8eea602dd238e0e07b7e2c2685d, https://github.com/roboticslab-uc3m/kinematics-dynamics/compare/821234cc2779781f47e54f6d711c3fa1c5def83a...6d252ce41c96f8eea602dd238e0e07b7e2c2685d. Closing the issue as solved, but WONTFIX.