Implement actuator control

[PeterBowman]

The ICartesianControl interface could provide a method to control an end-effector actuator (e.g. a gripper tool). To not interfere with tool (see #95), a new act/VOCAB_CC_ACT method/RPC vocab pair is proposed.

[PeterBowman]

This is easier to accomplish on AmorControlBoard since we have dedicated API methods for controlling the gripper. On the other hand, BasicCartesianControl connects (as of its current implementation) to a remote controlboard and use all of its joints, regardless whether there are fake ones (i.e. actuators such as the LacqueyFetch or the TextilesHand, see yarp-devices/libraries/BodyYarp) or not. Didn't try it, but I guess that the robot controlboard plugin was launched without these additional "joints" so far.

To sum up, BasicCartesianControl should be able to distinguish motor and actuator joints via explicit --param value options, or fetch this information from the remote controlboard in case it implements the appropriate interface. I was thinking more about yarp::dev::IAxisInfo (VOCAB_JOINTTYPE_UNKNOWN for actuator joints?) or yarp::dev::IRemoteVariables (retrieve the list of motor/actuator joints with getRemoteVariable?). That said, the remote fetch solution would entail changes in the yarp-devices repository.

[jgvictores]

Sounds interesting. If implemented on yarp-devices, should also go in simulator, as in https://github.com/roboticslab-uc3m/openrave-yarp-plugins

[PeterBowman]

My current concept for the future actuator-related method in the ICartesianControl interface is the following signature:

bool act(int command);

Where command would evaluate to one vocab of the following new set:

• VOCAB_CC_ACTUATOR_NONE: no actuator available or just do nothing
• VOCAB_CC_ACTUATOR_CLOSE_GRIPPER
• VOCAB_CC_ACTUATOR_OPEN_GRIPPER
• VOCAB_CC_ACTUATOR_STOP_GRIPPER
• VOCAB_CC_ACTUATOR_GENERIC
• ...

Still wondering how would this cope with hypothetical implementations of generic (non-gripper) actuators of any kind. Should the controller/remote controlboard/both be aware of the actuator type?

[PeterBowman]

WIP at the amor-cc-actuator branch (locally) to get a first insight of this interface working on AMOR. As noted before, broader changes are required to accomplish this in the generic controller with custom actuators.

ICartesianControl vocabs:

kinematics-dynamics/libraries/TeoYarp/ICartesianControl.h

Lines 86 to 91 in a9b6ecd

	// Actuator control
	#define VOCAB_CC_ACTUATOR_NONE VOCAB3('a','c','n') ///< No actuator or no action
	#define VOCAB_CC_ACTUATOR_CLOSE_GRIPPER VOCAB4('a','c','c','g') ///< Close gripper
	#define VOCAB_CC_ACTUATOR_OPEN_GRIPPER VOCAB4('a','c','o','g') ///< Open gripper
	#define VOCAB_CC_ACTUATOR_STOP_GRIPPER VOCAB4('a','c','s','g') ///< Stop gripper
	#define VOCAB_CC_ACTUATOR_GENERIC VOCAB3('a','c','g') ///< Generic actuator

New method signature:

kinematics-dynamics/libraries/TeoYarp/ICartesianControl.h

Lines 245 to 254 in a9b6ecd

	/**
	* @brief Actuate tool
	*
	* Send control command to actuate the robot's tool, if available.
	*
	* @param command One of available @ref ICartesianControl_actuator_vocabs "actuator vocabs".
	*
	* @return true on success, false otherwise
	*/
	virtual bool act(int command) = 0;

Implementation at AmorCartesianControl:

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

Lines 224 to 251 in a9b6ecd

	bool roboticslab::AmorCartesianControl::act(int command)
	{
	AMOR_RESULT (*amor_command)(AMOR_HANDLE);
	switch (command)
	{
	case VOCAB_CC_ACTUATOR_CLOSE_GRIPPER:
	amor_command = amor_close_hand;
	break;
	case VOCAB_CC_ACTUATOR_OPEN_GRIPPER:
	amor_command = amor_open_hand;
	break;
	case VOCAB_CC_ACTUATOR_STOP_GRIPPER:
	amor_command = amor_stop_hand;
	break;
	default:
	CD_ERROR("Unrecognized command with code %d (%s).\n", command, yarp::os::Vocab::decode(command));
	return false;
	}
	if (amor_command(handle) != AMOR_SUCCESS)
	{
	CD_ERROR("%s\n", amor_error());
	return false;
	}
	return true;
	}

Used in several apps: keyboardController, streamingDeviceController, haarDetectionController.

[PeterBowman]

That said, the remote fetch solution would entail changes in the yarp-devices repository.

Marking as blocked.

[PeterBowman]

Update: I'm not convinced anymore about the IRemoteVariables interface, see roboticslab-uc3m/yarp-devices#198 (comment). Also, the upstream work on yarp-devices would imply roboticslab-uc3m/yarp-devices#211.

@jgvictores OK to merge amor-cc-actuator branch into develop as a first approach for tackling this problem (at least) on AMOR? We'll most probably rewrite that part sooner or later, but those changes would not be lost (and it will spare me doing a lot of merge commits on the already-too-long run).