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).

[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.

Note: roboticslab-uc3m/yarp-devices#20 will enable arbitrary CAN bus + YARP wrapper configurations, and roboticslab-uc3m/yarp-devices#171 is going to wrap raw joints involved in the actuation. Two remarks:

• BCC needs to know how to treat/map ports (remote connections, i.e. we opened a controlboardwrapper2 exclusively dedicated for the gripper joints) and joints (local devices, i.e. we instantianted the CanBusControlboard instead of a remote_controlboard).
• In addition, BCC should be aware of the gripper type and its behavior on open/close commands. Changing my mind once again: the IRemoteVariables interface might do...
• Loose idea: migrate actuator vocabs to yarp-devices, expose them via installed header, implement IRemoteVariables on Dextra/TextilesHand/LacqueyFetch to perform the desired action, either include that header in user apps or call vocabs via RPC to command the tool.

[PeterBowman]

Per #162 (comment):

I plan to merge numRobotJoints and numSolverJoints into one entity that represents motor joints, only. That is, the (until and for now) only additional YARP-handled joint, which corresponds to the LacqueyFetch single-motor gripper, will be exposed via a dedicated YARP port to which BasicCartesianControl would have to connect via a separate remote_controlboard device in case it needs to access actuator commands (#115). See rationale at roboticslab-uc3m/yarp-devices#228 (comment).

We might want to rethink the whole act command stuff. See also roboticslab-uc3m/yarp-devices#228.