JHU BRP Robot protocol description

System Diagram

Source code:
svn co http://www.na-mic.org/svn/NAMICSandBox/trunk/OpenIGTjhu

Workflow

Scanner: Scout image acquisition with the landmark close to the prostate
Navigation soft and scanner: Calculate Z-frame position, scan a slice through the Z-frame
Repeat previous step if the Z-frame is not visible enough to register the robot
Scanner: 3D volume acquisition of the prostate
Navigation soft: Select (or import) the targets, show needle path, allow modification
Navigation soft: Calculate the target and path, send it to the robot
Robot: Execute command
[Optional] Navigation soft: send current robot coordinates to the Real Time scanner IO for needle tracking, display image
Navigation soft: Display robot position and status
Scanner: Confirm robot location before biopsy
Manual: Insert biopsy needle
[Optional] Navigation soft: show Real time images of the needle insertion
Manual: Fire biopsy gun

Open IGT Link Protocol

Some commands / queries are expecting parameters in the Body section which follows the Header Structure. The parameter can be:

8 bit Byte
32 bit Integer
32 bit Float

For example the Position and Orientation parameters in SET_Z_FRAME command will be the following.

 Bytes (Body)
 58      62      66      70      74      78      82
 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 |   X   |   Y   |   Z   |  Ox   |  Oy   |  Oz   |   W   |
 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+

Set Workphase (Slicer -> Robot)

There are no parameters for this commands.

START_UP

Slicer: Send "START_UP" Workphase command
Robot: Initialize robot (INITIALIZE command) and respond with status message

PLANNING

Robot: Do nothing, respond with status

CALIBRATION

Robot: Do nothing, respond with status. Waiting for SET_Z_FRAME command (Z-Frame transformation).

TARGETING

Robot: Fail if Z-Frame was not set. Respond with status. Waiting for targets (MOVE command).

MANUAL

Robot: Engage breaks, responds with status.
Slicer sends "GetCoordinate" query periodically to update the display.

EMERGENCY

Robot: Responds with status

Command / Query (Slicer -> Robot)

Command: INITIALIZE

Parameters: none
Robot: Initialize, set the encoders, remember this position as "HOME". Respond with status.

Command: SET_Z_FRAME

Parameters (28 byte total):

Vector of three 32 bit floats (3x4 byte): position X, Y, Z
Vector of four 32 bit floats (4x4 byte): orientation quaternion - normalized!

Robot: Store Z-frame position and orientation (global RAS coordinate system) - respond with status.

Command: MOVE_TO

Parameters (12+16 = 28 byte total):

Vector of three 32 bit floats (3x4): position X, Y, Z
Vector of four 32 bit floats (4x4): orientation quaternion

Robot: Go to this coordinate - position, orientation (later: speed?)

Global RAS coordinate system).

Robot responds with status.

Command: INSERT

Parameters (4 byte): 32 bit float
Robot: Insert the needle to this depth - float (in mm)

Command: BIOPSY

Parameters: none
Robot: Take biopsy (Retract Cannula)

Command: HOME

Parameters: none
Robot: Go to Home (collapses the robot to remove/replace needle)

Command: STOP

Parameters: none
Robot: Stop moving

Command: E_STOP

Parameters: none
Robot: Emergency Stop (stop + purges the air lines)

Command: SET_JOINT

Parameters: a set of 32bit floats. The number of floats is BODY_SIZE/4.
Robot: Set Joint Positions - float[max.16] - move joint here

Command: SET_VELOCITY

Parameters: a set of 32bit floats. The number of floats is BODY_SIZE/4.
Robot: Set Joint Velocities - float[max.16] - move joint with this speed

Command: SET_VOLTAGE_OUTPUT

Parameters: a set of 32bit floats. The number of floats is BODY_SIZE/4.
Robot: sets the air pressure / voltage output

Command: LOCK

Parameters: none
Robot: Engage brakes - all of them

Command: UNLOCK

Parameters: none
Robot: Unlock - disengage all the brakes so the robot can be moved manually

Command: LOCK_AXIS

Parameters: one or more 32bit integers (BODY_SIZE/4)
Robot: Engage brakes on a give axis

Command: UNLOCK_AXIS

Parameters: one or more 32bit integers (BODY_SIZE/4)
Robot: Unlock - disengage the brakes on a give axis, so it can be moved manually

Command: LIMIT_VELOCITY

Parameters: one 32 bit float
Robot: Set entire robot velocity limit in Robot Coords. - float (like 10 mm/s)

Query: GET_COORDINATE

Parameters: none
Robot: Respond with current coordinate (position and orientation in global RAS coordinate system)

Query: GET_STATUS

This was moved to the generic header implemented by every device!
Parameters: none
Robot: Respond with STATUS packet

Query: GET_BIOPSY

Parameters: none
Robot: Respond with Actual Insertion Depth

Query: GET_NR_JOINT

Parameters: none
Robot: Get Number of Joints - integer, see the description below

Query: GET_JOINT

Parameters: one or more 32bit integers (BODY_SIZE/4)
Robot: Get Actual Joint Positions

Query: GET_VELOCITY

Parameters: one or more 32bit integers (BODY_SIZE/4)
Robot: Get Actual Joint Velocities

Query: GET_VOLTAGE_OUTPUT

Parameters: one or more 32bit integers (BODY_SIZE/4)
Robot: Get Actual Air Pressures

Query: GET_JOINT_TRACK

Parameters: one or more 32bit integers (BODY_SIZE/4)
Robot: Get Actual Joint Tracking Errors

Query: GET_BRAKE

Parameters: none
Robot: Get Brakes status

Query: GET_MOVING

Parameters: none
Robot: Get Joint Moving status

Responses sent by the robot (Robot -> Slicer)

Status: STATUS

Response from Robot: Robot done moving, Error message, Emergency stop
See OpenIGTLink/Protocol#STATUS for description.

Status: BRAKES

Response from Robot: Brake status - boolean[max.16] - true(locked)/false(loose) for each joint
Structure: a number of bytes, defied by BODY_SIZE. 0 means false, 1 means true.

Status: DONE_MOVING

Response from Robot: Done moving/joint - boolean[max.16] - true/false for each joint Structure: a number of bytes, defied by BODY_SIZE. 0 means false, 1 means true.

Response: COORDINATES

Response from Robot: Actual coordinates and insertion depth - 6DOF in global RAS coordinate system
Structure (28 byte total):

Vector of three 32 bit floats (3x4 bytes): position X, Y, Z
Vector of four 32 bit floats (4x4 bytes): orientation quaternion
Three 32 bit floats (3x4 byte): needle offset (to calculate Depth from Z) [X_offset, Y_offset, Z_offset + InsertionDepth]

Response: JOINTS

Response from Robot: Number of Joints - integer
Structure: one 32 bit integer (4 bytes total)

Response: JOINT_POS

Response from Robot: Actual joint positions - float[max.16] (joint positions in mm)
Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.

Response: JOINT_VEL

Response from Robot: Actual joint velocities - float[max.16] (joint velocities in mm/s)
Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.

Response: JOINT_TRACK

Response from Robot: Actual joint tracking error - float[max.16] - "actual joint position" - "set joint position"
Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.

Response: AIR_PRESSURE

Response from Robot: Actual air pressures - float[max.32] - two per joint! (PSI for each valve)
Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.

Mimimum command set

Command to the robot:

START_UP Internally calls the INITIALIZE command.
SET_Z_FRAME Stores the Z-Frame.
TARGETING Just checks the Z-Frame values.
MOVE_TO Repeat this command for each target.
MANUAL Changes to Manual insertion. Switch back to automatic by calling TARGETING.
INSERT Automatic insertion. Not yet implemented on the hardware.
BIOPSY Takes the biopsy automatically, see INSERT.
HOME Ex. after manual insertion to change the biopsy gun.
EMERGENCY If something is wrong. To get out of this state START_UP needs to be called again.
GET_COORDINATE Call this as often as needed to get the latest position of the robot.
GET_STATUS Will respond with OK or other status - again, called often.

Response from the robot:

STATUS
COORDINATES

OpenIGTLink/Protocol/JHUBRP

Contents