Let users access and modify low-level controller configuration

[PeterBowman]

As discussed with @jgvictores, we'd like to stop and avoid an increasing expansion of the list of commands at ICartesianControl (totalling 17 at the time of filing this issue). See background at #105.

Influenced by yarp::dev::ICartesianControl::tweakSet and yarp::dev::ICartesianControl::tweakGet, consider adding a pair of setter/getter RPC methods that would allow users to change a set of controller parameters on runtime, such as:

• gain
• maximum joint velocity
• trajectory duration
• switch between velocity and duration as a limiter for movl and the like ([BasicCartesianControl] needs improvements in movl #46)
• trajectory generator? (Design and run more complex trajectories in BasicCartesianControl #135)
• switch between RPC/streaming modes? (mind return values!)
• switch reference frame (intertial vs end-effector) -> palliates command duplication
• position vs velocity control? (not for now)

Iin parallel to this, new configuration options should be implemented in the corresponding YARP devices (e.g. yarpdev --device BasicCartesianControl --gain 0.15 ...).

Example signature:

// for doubles...
bool setParameter(int vocab, double value);
bool getParameter(int vocab, double *value);

// for bools...
bool setParameter(int vocab, bool value);
bool getParameter(int vocab, bool *value);

// other types?

Multiple overloads may seem like overkill and go against the initial movation of this issue, but C++ lacks a standard container similar to yarp::os::Value (at least not until C++17, see std::any).

[PeterBowman]

ros::NodeHandle implements a similar solution for accessing the parameter server (ref1, ref2). ROS devs didn't hesitate to create overloads: ros::NodeHandle::getParam x 16, ros::NodeHandle::setParam x 16, plus their cached counterparts, and more, including generic methods (which I think we can't use in an abstract base class).

A remark on that: ROS does not place constraints on the parameter identifier (the key part, as opposed to the value). It's meant to be a plain std::string which may point at any type of data, hence such a high number of overloads. Should we make them accept vocabs instead, a decision should be taken whether to define a narrow, basic set of properties in ICartesianControl.h (e.g. VOCAB_CC_GAIN), or delegate this responsibility to the implementors (e.g. BasicCartesianControl and AmorCartesianControl might accept different sets of vocabs). The latter imposes a serious restriction: client code must know beforehand how is the vocab encoded and construct it with yarp::os::Vocab::encode prior to handling the vocab parameter. This is obviously true for stringified parameter keys as well, but what is the point of enforcing a four-character limit for these descriptors, then?

[PeterBowman]

Idea: publish some or all of these configuration parameters along with controller state and FK solution in stat() (#102, #129). Discuss it here or at #118.

[jgvictores]

Maybe just double and string for now?

[PeterBowman]

I'm going to forget about strings for now... I can't see any valid use case for the time being.

[PeterBowman]

Done at b0ccbf2. Currently enabled parameters:

• controller gain
• maximum joint velocity
• trajectory duration
• reference frame (implemented as vocabs):
  • base frame (inertial)
  • TCP frame (end effector)

No longer blocking #46.

[PeterBowman]

Small follow-up at 944ec2a. Relevant changes:

• new configuration parameter: cmcRateMs (only BasicCartesianControl)
• encoded frame vocabs have been moved to ICartesianControl::reference_frame
  

  kinematics-dynamics/libraries/YarpPlugins/ICartesianControl.h

  Lines 100 to 105 in 944ec2a

  	//! Lists supported reference frames.
  	enum reference_frame
  	{
  	BASE_FRAME = VOCAB4('c','p','f','b'), //!< Base frame
  	TCP_FRAME = VOCAB4('c','p','f','t') //!< End-effector frame (TCP)
  	};

• a few RPC commands now support the TCP frame, notably movl (we can issue linear offsets) and movv
  • remaining commands will be tracked in Handle TCP frame in FK solver and controller methods #136
  • important: movl and pose will crash unless controllerGain is set to zero

[jgvictores]

kinematics-dynamics/libraries/YarpPlugins/ICartesianControl.h

Lines 100 to 105 in 944ec2a

	//! Lists supported reference frames.
	enum reference_frame
	{
	BASE_FRAME = VOCAB4('c','p','f','b'), //!< Base frame
	TCP_FRAME = VOCAB4('c','p','f','t') //!< End-effector frame (TCP)
	};

Classy! Smooth! Nice!!

• important: movl and pose will crash unless controllerGain is set to zero

Should that be happening? In simulation, zero gain should work, but a non-zero gain should be needed with real motors.

[PeterBowman]

Classy! Smooth! Nice!!

Just a shameless copypasta from YARP 😄

Should that be happening? In simulation, zero gain should work, but a non-zero gain should be needed with real motors.

In fact, this is closely related to #136 because of a fwdKinError call that does not support (yet) the TCP frame. See:

kinematics-dynamics/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp

Lines 63 to 69 in 944ec2a

	iCartesianSolver->fwdKinError(desiredX, currentQ, commandXdot);
	for (int i = 0; i < 6; i++)
	{
	commandXdot[i] *= gain * (1000.0 / cmcRateMs);
	commandXdot[i] += desiredXdot[i];
	}

As a workaround, I suggest setting gain to zero so that the buggy command does not influence subsequent operations.

[jgvictores]

Okay, understood. Thanks!

[PeterBowman]

Another follow-up: commit 5ad5eea added two new methods for setting and retrieving multiple parameters at once. Also, set/get vocabs have been standardized. Example invocations via yarp rpc:

• get cpjv: retrieve maximum joint velocity
• set cpjv 15: set maximum joint velocity to 15
• get prms: retrieve all configuration parameters
• set prms (cpjv 15) (cpcr 60): set maximum joint velocity to 15 and CMC rate to 60 ms

Vocabs may be passed either as plain strings or in brackets: cpjv or [cpjv].

kinematics-dynamics/libraries/YarpPlugins/ICartesianControl.h

Lines 341 to 361 in 5ad5eea

	/**
	* @brief Set multiple configuration parameters.
	*
	* Ask the controller to store or update multiple parameters at once.
	*
	* @param params Dictionary of YARP-encoded vocabs as keys and their values.
	*
	* @return true on success, false otherwise
	*/
	virtual bool setParameters(const std::map<int, double> & params) = 0;
	/**
	* @brief Retrieve multiple configuration parameters.
	*
	* Ask the controller to retrieve all available parameters at once.
	*
	* @param params Dictionary of YARP-encoded vocabs as keys and their values.
	*
	* @return true on success, false otherwise
	*/
	virtual bool getParameters(std::map<int, double> & params) = 0;

[PeterBowman]

Yet another follow-up (YAFU, which vaguely resembles YAFUD): 1f62c00. Now, we can configure the check period within the implementors of ICartesianControl::wait (#106):

kinematics-dynamics/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp

Line 104 in 1f62c00

addUsage("... [cpwp] value", "(config param) check period of 'wait' command [ms]");

[PeterBowman]

#121 (comment)

The cpjv (maximum joint velocity) parameter shall be removed after roboticslab-uc3m/yarp-devices#188, and convenient calls to getVelLimits performed on controller init (similarly to getLimits).

Edit: see #159.