Head-Eye Coordinated Control. More...
Head-Eye Coordinated Control.
OBJECTIVES:
ControlGaze2 is a module that controls in a coordinate way the head and the eyes of the iCub. It has been made for two main purposes:
The module allows changing the gaze direction by:
OUTLOOK:
The module controls simultaneously head gaze and vergence. Head gaze control has two modalities:
Saccades are controlled in two phases:
A new saccade is accepted only when the previous one has finished. This is the typical operation in humans where saccades are used to change the object of interest.
Smooth pursuit only operates in the slow phase, but it accepts a continuous stream of commands. It is meant to emulate the human behavior when tracking an object.
A single set of gains suits both the saccade and smooth pursuit modes. These gains specify both the speed of the motions and the amount of motion that is performed by the eyes and by the neck.
Vergence also operates in a single phase. There is an independent gain for the vergence controller.
OPERATION:
The module has an internal loop at 50Hz. At this rate it controls the angular velocities of each joint in order to reach a certain provided reference gaze value, in azimuth and elevation angles. This reference can be provided in 4 different coordinate systems.
Usually there are no stability problems when controlling the module with saccade motions. The module does not accept new commands while the gaze has not reached the reference values.
In pursuit mode, the heading direction can be specified at any time step and the controller must recompute the controls. This puts some stability issues because the trajectories become dependent of the rate commands are sent to the controller. The default gains have been tuned for internal control loop rate of 20 ms (50 Hz) and a pursuit command refresh rate (visual loop) of 50ms (20Hz).
CURRENT STATUS:
The following sections describe the current state of controlGaze2.
STARTING THE MODULE
To start controlGaze2 type execute the following command:
The following parameters are accepted:
configCamera
--configCamera iCubEyes.ini
A file with camera parameters. To convert from pixel coordinates to azimuth and elevation angles we need the intrinsic parameters of the cameras. These are the parameters that result from camCalibConf. The file should be something like this: ------------------------------------------------
----------------------------------------------------
If the images come from camCalib modules, the optical distortion parameters (k1, k2, p1, p2) should be put to 0.
appPath
--appPath c:/icub/app/myapp
The folder name where to find the configuration files. This is subject to change in the new ResourceFinder specification.
imageSize
--imageSize 160x120
If the image size is different from the specified in the calibration file, this allows rescaling the intrinsic parameters. If this is not specified, the module will use the image dimension specified in the calibration file.
motorBoard
--motorBoard /icub/head
This specifies the prefix of the port names made available by the control board to the head device. If you use ICubInterface, this is usually /icub/head.
--velocity_gain
Let you tune the speed of the controller. Play with care: this may lead to unstability. Never deviate too much from the defaults.
--fake_velocity_control
This specifies that the internal rate control loop will use relative position control rather than velocity control. This is useful to use with the simulator.
--fake_velocity_gain
Velocity gain for the fake velocity controller. Play with care: this may lead to unstability. Never deviate too much from the defaults.
Other Parameters and Defaults
These could go to a initialization file (e.g. controlgaze2.ini)and invoked by:
This is probably going to change in future version due to the new ResourceFinder's methods.
TESTING THE MODULE FROM THE CONSOLE
The module has a console interface where you can issue several commands. If you have access to the console window, you can just type the commands there. Otherwise open a command window and type:
Then just type the required commands in the console.
ACTUATION COMMANDS
STATUS COMMANDS
CONFIGURATION COMMANDS
For instance, if you issue the command:
The head-eye system will move to 10 degrees azimuth and 10 degrees elevation.
SENDING COMMANDS FROM OTHER MODULES
You can issue the previous actuation commands from a module sending appropriate bottles to the /controlGaze2/conf port. For instance the following code will send a bottle instructing controlGaze2 to move to 10 degrees azimuth and 10 degrees elevation:
However, these commands use the RPC protocols and thus cannot be issued very fast (there is a delay due to the acknowledge/reply protocol). If you want to do send high frequency and low latency commands to the module, please use the streaming ports, described below.
STREAMING PORTS
Beyond /controlGaze2/conf, that receives the console commands, the other ports opened by controlGaze2 are of the streaming type. They do not provide replies or acknowledges.
/controlGaze2/pos
This port accepts gaze position commands. The iCub head will perform a fast redirection of gaze (saccade), first moving the eyes quickly to the final heading. Then it will move the head, while the eyes counter-rotate to keep the image stabilized. The module will ignore any position commands issued while performing the saccade. When the saccade finishes, other position commands can will be accepted. To check if the module is accepting position commands, please see the documentation of port /status:o. States 3,4,5 and 6 will accept saccades.
Each command is a vector of 5 floats:
/controlGaze2/vel
This port accepts commands that implement the smooth pursuit behavior. The behavior is similar to the position commands but the eye does not saccade faster: both eyes and head move slower. The port accepts a fast stream of commands. Each command is a Vector of 3 floats:
/controlGaze/dis
This port accepts gaze vergence commands. Each command is a vector of 1 float:
/controlGaze/imgcoord
This port was made purposefully to connect directly viewer clicks. Will send the gaze direction to the clicked position in the viewer.
/controlGaze2/imu
Connects to the output of the inertial sensor a compensates for external body motions.
/controlGaze2/trackersignal/bot:o
Not working. Used once to indicate the end of a saccade, meaning there is a new object to track. Please advice if interested in this feature.
/controlGaze2/possibledirections/vec:o
Experimental. Tells external modules about direction that cannot be reached due to head-eye mechanical limits. Please advice if interested in this feature.
/controlGaze2/status:o
Outputs the state of the controller and the current azimuth and elevation gaze direction coordinates (degrees). The port sends bottles with three elements:
SUMMARY:
Receives position / velocity comands either in angles (azimuth/elevation/vergence)[degrees] or in image coordinates (horizontal, vertical, disparity)[check this] and executes the comands by appropriately controlling the iCub head's joint angles.
Position commands are controlled in a saccadic manner:
Velocity comands are executed in a smooth-pursuit mode:
Notifies other modules of important status data and events:
A mode for the integration of vestibular information is under development.
A mode for the integration of motion prediction, either using integral control of model based predictors, is under development.
Parameters:
Absolute path to the application folder.
Configuration file with camera parameters.
Actual image size (not calibration size)
Port name of the remote (server) control board.
Time from start of saccade until looking back to center if saccade reaches a joint limit.
Minimum time between saccades.
Expected rate (samples per second) of input data in velocity mode (smooth pursuit)
Rate of the control thread. Limited by the motor control board rates (50Hz is about the maximum).
Gains of the rate controller for head-eye coordination.
Gain to tune the speed of the velocity controller.
Replaces velocity control by position increment control.
Gain to tune the speed of the fake velocity controller.
Threshold to check when a saccade reaches the end. When the gaze error squared norm (azimuth_error^2+elevation_error^2) is below this value, a message is sent to notify the end of the saccade.
If true, a log file is created with several important status variables.
Name of the file where to log information.
EXPERIMENTAL. Turns on integral control.
EXPERIMENTAL. Sets the gain for integral control.
EXPERIMENTAL. Turns on the vestibular (inertial) system.
EXPERIMENTAL. Configuration file for model based predictors.
Commands:
-- ACTUATION COMMANDS
-- STATUS COMMANDS
-- CONFIGURATION COMMANDS
1.7.1
