Monitor internal state in BasicCartesianControl/CartesianControlServer

[PeterBowman]

Some thoughts arised while working on the BCC/CCS at #117:

• Forbid simultaneous execution of RPC and streaming commands. This is monitored via command presets (d2e79f5).
• Allow only one active connection for streaming commands (maybe RPC too)? Might be necessary to enforce this in the CartesianControlClient plugin. Not doing, this should be handled in the NWS/NWC layer and flexibility would be at risk if implemented, we could actually benefit from multiple CCS accessing stat or others. We do have some safety mechanisms in place, though, to guard against ugly race conditions.
• Keep track of run/stop states to avoid frequent mode changes and reduce amount of messages sent to the real robot. (a597ef9)
• Stop control if erroring at streaming commands (remember that these return no success/failure flag)? (82e87b6)
• Handle blocking/non-blocking operations (proposal: new wait() method, see Issue non-blocking movements and create new wait() command #106).
• Quit if connection with remote robot/simulator is lost? The CMC is aborted in case 20 consecutive encoder reads are not received. (1ab0339)
• Make linear trajectories depend on velocity instead of time (Make linear trajectories depend on velocity instead of time #157). Not done as of 2022/07/13, see that issue for follow-ups.
• Refuse setting controller parameters (VOCAB_CC_CONFIG_XXX) if not in non-controlling state (stopControl()). (6e78a53, ac4e607)
• Warn or stop control when limits are hit (Watch joint configuration in CMC thread #161).
• Allow concurrent requests for new movv commands, especially for the keyboardController app (BasicCartesianControl::movv should follow a velocity-driven trajectory #166). (eae5bd1)
• Move mode change out of each command? Example: movi performs that switch on its very first execution, but it takes quite long due to hardware limitations. Idea: preset control modes via controller configuration parameters iff using streaming commands. (852453d)
• Relax hard requirements on controlboard subdevices. For instance, AmorControlboard has no current implementation of IPositionDirect and this is only needed for issuing movi commands, but BCC would report an error on instantiation anyway. Not doing and not caring of AmorControlboard as of 2022 anymore. We have AmorCartesianControl instead. Besides, AmorControlboard should be repurposed as a collection of raw device instances in the vein of TechnosoftIpos, but again, it might be never done due to lack of interest.

[PeterBowman]

This issue was repurposed to focus solely on internal state considerations regarding the BasicCartesianControl implementation and its network wrapper (possibly spanning to AmorCartesianControl, too). Initial and on-runtime controller configuration will be discussed at #121.

[PeterBowman]

Regarding internal state and concurrency issues, check #129 (comment). Conveniently pasted excerpt:

• multi-threading within the controller itself (e.g. RateThread): current locks cover the state identifier (getCurrentState, setCurrentState), but some other members are shared between threads without mutex guards (e.g. desired cartesian velocities and forces); by now, we could assume that issuing multiple commands simultaneously is not allowed and re-think the internal behavior in Monitor internal state in BasicCartesianControl/CartesianControlServer #118 ❓

[PeterBowman]

Idea (from #121 (comment)): stream configuration parameters (#121) in the recently created state:o port.

[PeterBowman]

Idea (from #121 (comment)): stream configuration parameters (#121) in the recently created state:o port.

Not sure about this, we'll rarely want to access config params in a high frequency manner. RPC set/get calls should be enough. The only advantage I see here is that yarp rpc access to the stat command will retrieve a comprehensive list of these parameters and their current value, but this would obviously render no benefits when querying the data programatically (i.e. via CartesianControlClient).

Another idea: provide a bonus RPC vocab to retrieve all config parameters with their value. For both interfaces:

• CLI (yarp rpc): simply list the values, similarly to [help] [more]
• C++ code: store keys and values in a std::map

[jgvictores]

Not sure about this, we'll rarely want to access config params in a high frequency manner. RPC set/get calls should be enough. The only advantage I see here is that yarp rpc access to the stat command will retrieve a comprehensive list of these parameters and their current value, but this would obviously render no benefits when querying the data programatically (i.e. via CartesianControlClient).

I agree.

CLI (yarp rpc): simply list the values, similarly to [help] [more]
C++ code: store keys and values in a std::map

Cool. For yarp rpc, something like [get] [prms] sounds good to me!

[PeterBowman]

Added reference to related issue in description: #106.

[PeterBowman]

In spite of 7d2e073, I'd like to decouple our internal vocabs from those used by YARP controlboard interfaces (which have nothing to do with cartesian control). @jgvictores OK to create VOCAB_CC_OK, VOCAB_CC_FAILED, VOCAB_CC_SET and VOCAB_CC_GET? They would resolve to the values users are already familiar with ([ok], [fail], [set], [get], respectively).

[jgvictores]

OK to create VOCAB_CC_OK, VOCAB_CC_FAILED, VOCAB_CC_SET and VOCAB_CC_GET? They would resolve to the values users are already familiar with ([ok], [fail], [set], [get], respectively).

Sure!

[PeterBowman]

New vocab definitions and group parameter accessors: #121 (comment).

[PeterBowman]

Keyword: state machines (FSM). Several checks should be performed against internal state variables upon entering most of the control commands (e.g. current control mode, whether the trajectory instance is cleared and ready for use, etc.). The CMC thread would take care of any necessary cleanup before and after execution.

[PeterBowman]

For now, marking as blocked by #145.

[PeterBowman]

Added to list: refuse setting controller parameters (VOCAB_CC_CONFIG_XXX) if not in non-controlling state (stopControl()).

[PeterBowman]

Moving this away from the issue description regarding control modes:

See the following excerpt from BasicCartesianControl::stopControl:

kinematics-dynamics/libraries/TeoYarp/BasicCartesianControl/ICartesianControlImpl.cpp

Lines 243 to 248 in 37571fa

	for (unsigned int joint = 0; joint < numRobotJoints; joint++)
	{
	iControlMode->setPositionMode(joint);
	}
	iPositionControl->stop();
	setCurrentState( VOCAB_CC_NOT_CONTROLLING );

Perhaps we can solve it upstream (that is, in the robot controller), see roboticslab-uc3m/yarp-devices#120).

[PeterBowman]

Regarding locks and race conditions because of port callbacks being invoked at the same time, see yarp::os::Contactable::lockCallback and other lock-related methods. Also: robotology/community#188 and the ex0300_port_callback example.