iKinGazeCtrl

Gaze controller based on iKin.

Collaboration diagram for iKinGazeCtrl:

Gaze controller based on iKin.

Copyright (C) 2010 RobotCub Consortium

Authors: Ugo Pattacini, Alessandro Roncone

CopyPolicy: Released under the terms of the GNU GPL v2.0.

Description

This module provides a controller for the iCub gaze capable of steering the neck and the eyes independently performing saccades, pursuit, vergence, OCR (oculo-collic reflex) and VOR (vestibulo-ocular reflex relying on inertial data).

The controller can be seen as cartesian gaze controller since it receives as input a 3D position in the task space. Nonetheless, further command modalities are available: 1) the coordinates (u,v) of just one pixel in the image plane along with a guessed component z in the eye's reference frame can be provided, or alternatively the vergence angle; 2) the position of the target within the two image planes can be converted in the 3D task space using the monocular approach coupled with a pid on the component z; 3) the head-centered azimuth and elevation angles along with the vergence angle can be given to the module both in absolute and relative mode.

Moreover, this module also implements the server part of the Gaze Control Interface. For a tutorial on how to use the interface, please go here.

Note

If the torso is not detected alive then the module will try to keep on working with just the head part.

If you're going to use this controller for your work, please quote it within any resulting publication: Roncone A., Pattacini U., Metta G. & Natale L., "A Cartesian 6-DoF Gaze Controller for Humanoid Robots", Proceedings of Robotics: Science and Systems, Ann Arbor, MI, June 18-22, 2016.

Reminder
If you experience a slow speed motion, please check the shift factors settings within your low-level configuration file of the head part: they should be properly tuned. Usually a value of 8 is enough.

Rule: a lower shift factor allows to yield an higher joint speed and at the same time it increases the value of minimum speed that can be executed.

Example: look in the file icub_head_torso.ini of your robot setup; you should find something similar to:

[VELOCITY]
Shifts 8 8 8 8 8 8 ...

Note

A video on iCub gazing can be seen here.

Libraries

• YARP libraries.
• iKin library (it requires IPOPT).

Parameters

–context dir

• Resource finder default searching directory for configuration files; if not specified, iKinGazeCtrl is assumed. Beware of the context search policy.

–from file

• Resource finder default configuration file; if not specified, config.ini is assumed.

–name ctrlName

• The parameter ctrlName identifies the controller's name; all the open ports will be tagged with the prefix /<ctrlName>. If not specified, iKinGazeCtrl is assumed.

–robot name

• The parameter name selects the robot name to connect to; if not specified, icub is assumed.

–head name

• The parameter name selects the robot's head port to connect to; if not specified, head is assumed.

–torso name

• The parameter name selects the robot's torso port to connect to; if not specified, torso is assumed. The special string off can be used to skip opening the torso device.

–trajectory_time::neck time

• Specify the neck trajectory execution time in point-to-point movements [expressed in seconds]; by default time is 0.75 seconds. (Tneck cannot be set equal or lower than Teyes).

–trajectory_time::eyes time

• Specify the eyes trajectory execution time in point-to-point movements [expressed in seconds]; by default time is 0.25 seconds.

–cameras::context dir

• The parameter dir specifies the context used to locate the cameras parameters file (see below).

–cameras::file file

• The parameter file specifies the file name used to read cameras parameters.

–saccades switch

• Enable/disable saccadic movements; the parameter switch can be therefore ["on"|"off"], being "on" by default.

–neck_position_control switch

• Enable/disable low-level position control of the neck; the parameter switch can be therefore ["on"|"off"], being "on" by default.

–imu::mode switch

• Enable/disable stabilization using IMU data; the parameter switch can be therefore ["on"|"off"], being "on" by default.

–imu::source_port_name name

• Allow specifying a different source port for the IMU data (see IMU filtering tools such as e.g. imuFilter).

–imu::stabilization_gain gain

• Specify the integral gain (in [1/s]) used for gaze stabilization; the gain is 11.0 [1/s] by default.

–imu::gyro_noise_threshold thres

• Specify a different threshold thres given in [deg/s] to filter out the residual bias in gyro readouts.

–imu::vor gain

• Specify the contribution of the vestibulo-ocular reflex (VOR) in computing the final counter-rotation of the eyes due to neck rotation. To turn off the VOR just set the gain equal to 0.0. By default gain is 1.0, that means "full contribution". If imu::mode is "off", then the gain is 0.0 by default. Values of the gain greater than 1.0 mean "contribution amplified". –imu::timeout
• Specify the read timeout (in seconds) for the imu data. The default is 0.04 seconds.

–ocr gain

• Specify the contribution of the oculo-collic reflex (OCR) in computing the counter-rotation of the eyes due to neck rotation. To turn off the OCR just set the gain equal to 0.0. Default values are 0.0 if imu::mode is "off", 1.0 otherwise.

–ping_robot_tmo tmo

• The parameter tmo is the timeout (in seconds) that allows starting up the robot before connecting to it; by default we have a timeout of 40.0 [s].

–eye_tilt::min min

• The parameter min specifies the minimum eye tilt angle [deg] in order to prevent the eye from being covered by the eyelid (when they're wide open) while moving; default value is -12 [deg].

–eye_tilt::max max

• The parameter max specifies the maximum eye tilt angle [deg] in order to prevent the eye from being covered by the eyelid (when they're wide open) while moving; default value is 15 [deg].

–min_abs_vel vel

• The parameter vel specifies the minimum absolute velocity that can be achieved by the robot [deg/s] due to the approximation performed while delivering data over the network. By default this value is 0.0 having no result on the controller's operations. In case it is different from 0.0, the controller will implement a bang-bang approach whenever the velocity to be delivered goes under the minimum threshold.

–head_version ver

• This option specifies the kinematic structure of the head; the value ver is a string (e.g., "v1.0", "v2.0"), being "v1.0" the default.

–verbose

• Enable some output print-out.

–tweak::file file

• The parameter file specifies the file name (located in module context) used to read/write options that are tweakable by the user; if not provided, tweak.ini is assumed.

–tweak::overwrite switch

• If "on", at startup default values and cameras values retrieved from file will be overwritten by those values contained in the tweak file. The switch is "on" by default.

Ports Accessed

The ports the module is connected to: e.g. /icub/head/command:i and so on.

Ports Created

There are different ways of commanding a new target fixation point:

• by sending the absolute 3D position to gaze at in the task space through /<ctrlName>/xd:i port.
• by localizing the target in just one image plane and then sending its coordinates together with a guessed component z in the eye's reference frame to the /<ctrlName>/mono:i port. Alternatively, the z component can be replaced by the vergence angle. In this mode the cameras intrinsic parameters are required.
• by localizing the target in the two image planes and thus sending its coordinates to the /<ctrlName>/stereo:i port. This strategy employs the monocular approach along with a pid that varies the component z incrementally according to the actual error; to achieve that it's required to feed continuosly the port with new feedback while converging to the target. In this mode the cameras intrinsic parameters are required.
• by sending the head-centered azimuth/elevation couple in degrees wrt either to the current head position or to the absolute head position (computed with the robot looking straight ahead and all neck and eyes encoders zeroed). Vergence is also to be given either in relative mode or absolute mode.
  In this mode a final conversion to 3D points is always performed wrt to the absolute head position since the controller is intrinsically cartesian. Moreover, as the center of the head varies over time because of the motion induced by the neck, the ultimate target vergence given for instance as delta wrt the current one may differ from what expected beacuse what is actually achieved is the corresponding 3D point.

The module creates the usual ports required for the communication with the robot (through interfaces) and the following ports:

• / <ctrlName>/xd:i receives the target fixation point. It accepts 3 double (also as a Bottle object) for xyz coordinates.
• / <ctrlName>/mono:i receives the current target position expressed in one image plane. The input data format is the Bottle [type u v z], where type can be "left" or "right", (u,v) is the pixel coordinates and z is the guessed z-component in the eye's reference frame. An alternative command modality employs the vergence given in degrees in place of the z-component and its format is: [type u v "ver" ver].
• / <ctrlName>/stereo:i receives the current target position expressed in image planes. It accepts 4 double (also as a Bottle object) in this order: [ul vl ur vr].
• / <ctrlName>/angles:i receives the current target position expressed as azimuth/elevation/vergence triplet in degrees. It accepts 1 string and 3 doubles (also as a Bottle object) in this order: [mode azi ele ver], where mode can be rel or abs. A positive azimuth will turn the gaze to the right, whereas a positive elevation will move the gaze upward.
• / <ctrlName>/x:o returns the actual fixation point (Vector of 3 double). Units in meters.
• / <ctrlName>/q:o returns the actual joints configuration during movement (Vector of 9 double). The order for torso angles is the one defined by kinematic chain. Units in degrees.
• / <ctrlName>/angles:o returns the current azimuth/elevation couple wrt to the absolute head position, together with the current vergence (Vector of 3 double). Units in degrees.
• / <ctrlName>/events:o streams out the event associated to the controller's state.
  Available events are:
  • "motion-onset" <time>: sent out at the beginning of the motion; comprise the time instant of the source when the event took place.
  • "motion-done" <time>: sent out at the end of the motion; comprise the time instant of the source when the event took place.
  • "motion-ongoing" <time> <checkpoint>: sent out when the portion of path given in [0,1] by the checkpoint parameter has been attained; comprise the time instant of the source when the event took place as well as the checkpoint.
  • "saccade-onset" <time>: sent out at the beginning of the saccade; comprise the time instant of the source when the event took place.
  • "saccade-done" <time>: sent out at the end of the saccade; comprise the time instant of the source when the event took place.
  • "stabilization-on" <time>: notify that gaze stabilization has been turned on; comprise the time instant of the source when the event took place.
  • "stabilization-off" <time>: notify that gaze stabilization has been turned off; comprise the time instant of the source when the event took place.
  • "closing" <time>: sent out when the controller is being shut down; comprise the time instant of the source when the event took place.
  • "suspended" <time>: sent out when the controller is suspended; comprise the time instant of the source when the event took place.
  • "resumed" <time>: sent out when the controller is resumed; comprise the time instant of the source when the event took place.
  • "comm-timeout" <time>: sent out when the controller gets suspended because of a communication timeout; comprise the time instant of the source when the event took place.
• / <ctrlName>/rpc remote procedure call.
  Recognized remote commands (be careful, commands dealing with geometric projections will only work if the cameras intrinsic parameters are provided):
  • [stop]: stop the motion immediately.
  • [bind] [pitch] <min> <max>: bind the neck pitch within a range given in degrees.
  • [bind] [roll] <min> <max>: bind the neck roll within a range given in degrees.
  • [bind] [yaw] <min> <max>: bind the neck yaw within a range given in degrees.
  • [bind] [eyes] <ver>: bind the eyes to look always straight ahead (the tilt can vary) with a specified vergence given in degrees.
  • [clear] [pitch]: restore the neck pitch range.
  • [clear] [roll]: restore the neck roll range.
  • [clear] [yaw]: restore the neck yaw range.
  • [clear] [eyes]: restore the eyes movements.
  • [get] [Tneck]: returns the neck movements execution time.
  • [get] [Teyes]: returns the eyes movements execution time.
  • [get] [vor]: returns the vor gain.
  • [get] [ocr]: returns the ocr gain.
  • [get] [sacc]: returns the saccades control status [0/1].
  • [get] [sinh]: returns the saccades inhibition period [s].
  • [get] [sact]: returns the saccades activation angle [deg].
  • [get] [track]: returns the current controller's tracking mode [0/1].
  • [get] [stab]: returns 1 iff gaze stabilization is active, 0 otherwise.
  • [get] [done]: returns 1 iff motion is done, 0 otherwise.
  • [get] [sdon]: returns 1 iff saccade is done, 0 if still underway.
  • [get] [pitch]: returns in degrees the current range of neck pitch joint.
  • [get] [roll]: returns in degrees the current range of neck roll joint.
  • [get] [yaw]: returns in degrees the current range of neck yaw joint.
  • [get] [eyes]: returns in degrees the vergence set when eyes are bound to look straight ahead; a negative values means no constraint.
  • [get] [ntol]: returns in degrees the current user tolerance for gazing with the neck.
  • [get] [des]: returns the desired head joints angles that achieve the target [deg].
  • [get] [vel]: returns the head joints velocities commanded by the controller [deg/s].
  • [get] [pose] <type>: returns (enclosed in a list) the left eye pose if type=="left", the right eye pose if type=="right" and the head-centered pose if type=="head". The pose is given in axis/angle representation (i.e. 7-componenets vector). Additionally, a stamp is appended as further list accounting for the time (second element of the list) relative to the encoders positions used to compute the pose.
  • [get] [2D] (<type> <x> <y> <z>): returns the 2D pixel point whose cartesian coordinates (x,y,z) are given wrt the root reference frame as the result of its projection into the image plane <type> ["left"|"right"].
  • [get] [3D] [mono] (<type> < u> <v> <z>): returns the 3D point whose projected pixel coordinates (u,v) in the image plane <type> ["left"|"right"] along with third component <z> in the eye's reference frame are given.
  • [get] [3D] [stereo] (< ul> <vl> <ur> <vr>): returns the 3D point whose projected pixels coordinates (ul,vl) and (ur,vr) in the image planes are provided as the result of the triangulation.

    Note

    The triangulation is deeply affected by uncertainties in the cameras extrinsic parameters and cameras alignment.

  • [get] [3D] [proj] (<type> < u> <v> < a> < b> < c> <d>): returns the 3D point with projected pixel coordinates (u,v) in the image plane <type> ["left"|"right"] that results from the intersection with the plane expressed with its implicit equation ax+by+cz+d=0 in the root reference frame.
  • [get] [3D] [ang] (<type> <azi> <ele> <ver>): transforms angular coordinates into cartesian coordinates. The option <type> can be ["abs"|"rel"].
  • [get] [ang] (<x> <y> <z>): transforms cartesian coordinates into absolute angular coordinates.
  • [get] [pid]: returns (enclosed in a list) a property-like bottle containing the pid values used to converge to the target with stereo input.
  • [get] [info]: returns (enclosed in a list) a property-like bottle containing useful information, such as the "head_version" (e.g. "1.0", "2.0", ...), the "min_allowed_vergence" (in degrees), a list of the available "events", the intrinsic and extrinsic camera parameters used.
  • [get] [tweak]: returns (enclosed in a list) a property-like bottle containing low-level information on the current controller's configuration.
  • [set] [Tneck] <val>: sets a new movements execution time for neck movements.
  • [set] [Teyes] <val>: sets a new movements execution time for eyes movements.
  • [set] [vor] <val>: sets a new gain for vor.
  • [set] [ocr] <val>: sets a new gain for ocr.
  • [set] [sacc] <val>: enables/disables saccades; val can be [0/1].
  • [set] [sinh] <val>: sets the saccades inhibition period [s].
  • [set] [sact] <val>: sets the saccades activation angle [deg].
  • [set] [ntol] <val>: sets in degrees the new user tolerance for gazing with the neck.
  • [set] [track] <val>: sets the controller's tracking mode; val can be [0/1].
  • [set] [stab] <val>: turns on/off the gaze stabilization (if enabled at start-up); val can be [0/1].
  • [set] [pid] ((prop0 (<val> <val> ...)) (prop1) (<val> <val> ...)): sets the pid values used to converge to the target with stereo input. The pid is implemented in parallel form (PIDs). Aside from the usual pid parameters a further option "dominantEye" is available that enables the user to chose the dominant eye employed for the monocular approach.
  • [set] [tweak] ((prop0 (<val> <val> ...)) (prop1) (<val> <val> ...)): sets parameters for the low-level controller's configuration.
  • [look] [3D] (<x> <y> <z>): yields gazing at target specified as 3D point. Distances are in meters.
  • [look] [mono] (<type> < u> <v> <z>): yields gazing at target specified in terms of pixel coordinates (u,v) in the image plane <type> ["left"|"right"] along with third component <z> in the eye's reference frame. Distances are in meters.
  • [look] [mono] (<type> < u> <v> "ver" <ver>): yields gazing at target specified in terms of pixel coordinates (u,v) in the image plane <type> ["left"|"right"] along with the vergence <ver>. Angles are in degrees.
  • [look] [stereo] (< ul> <vl> <ur> <vr>): yields gazing at target specified in terms of projected pixels coordinates (ul,vl) and (ur,vr) in the image planes.
  • [look] [ang] (<type> <azi> <ele> <ver>): yields gazing at target specified in angular coordinates. The option <type> can be ["abs"|"rel"]. Angles are in degrees.
  • [store]: store the controller context returning an integer identifier. The context comprises the values of internal controller variables, such as the tracking mode, the trajectory time and so on.
  • [restore] <id>: restore a previously stored controller context referred by the identifier id.
  • [del] (<id0> <id1> ...): delete all the contexts whose ids are contained in the list.
  • [register] [ongoing] <checkpoint>: register the "motion-ongoing" event with the given checkpoint in [0,1].
  • [unregister] [ongoing] <checkpoint>: unregister the "motion-ongoing" event with the given checkpoint in [0,1].
  • [list] [ongoing]: return the list of registered "motion-ongoing" events.
  • [susp]: suspend the module.
  • [run]: resume the module.
  • [status]: returns "running" or "suspended".
  • [quit]: quit the module.

Note

When the tracking mode is active and the controller has reached the target, it keeps on sending velocities to the head in order to compensate for any movements induced by the torso. If tracking mode is switched off, the controller automatically disconnects once the target is attained and reconnects at the next requested target. The controller starts by default in non-tracking mode.

Coordinate System

Positions (meters) refer to the root reference frame attached to the waist as in the official documentation.

Input Data Files

None.

Output Data Files

None.

Configuration Files

A configuration file passed through --cameras::file contains the fields required to specify the cameras intrinsic parameters along with a roto-translation matrix appended to the eye kinematic (see the iKinChain::setHN method) in order to achieve the alignment with the optical axes compensating for possible unknown offsets.

The final roto-translation matrix is meaningful only as result of the calibration of the cameras extrinsic parameters that can be obtained for instance through the stereoCalib module.

Example:

[CAMERA_CALIBRATION_RIGHT]
fx 225.904
fy 227.041
cx 157.858
cy 113.51
 
[CAMERA_CALIBRATION_LEFT]
fx 219.057
fy 219.028
cx 174.742
cy 102.874
 
[ALIGN_KIN_LEFT]
HN (0.0 1.0 2.0 ... 15.0)  // list of 4x4 doubles (per rows)
 
[ALIGN_KIN_RIGHT]
HN (0.0 1.0 2.0 ... 15.0)  // list of 4x4 doubles (per rows)

Tested OS

Windows, Linux

Author

Ugo Pattacini, Alessandro Roncone