Latest revision as of 14:49, 25 June 2008

Home < OpenIGTLink < Protocol < JHUBRP

<< OpenIGTLink | Protocol

JHU BRP Robot protocol description

System Diagram

Source code:
svn co http://svn.na-mic.org/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) - 28 bytes

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

@@ Line 1: / Line 1: @@
 [[OpenIGTLink | &lt;&lt; OpenIGTLink]] | [[OpenIGTLink/Protocol | Protocol]]
+<div class="redirectText">'''JHU BRP Robot protocol description'''</div>
 <div class="floatright">__TOC__</div>
 = System Diagram =
-[[Image:MRI_Robot_System_Diagram2.png]]
+<div class="center">[[Image:MRI_Robot_System_Diagram2.png]]</div>
+Source code:<br /> svn co http://svn.na-mic.org/NAMICSandBox/trunk/OpenIGTjhu
 = Workflow =
@@ Line 20: / Line 24: @@
 # [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 [[OpenIGTLink/Protocol#Header_Structure | 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)
+      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 ==
-Initialize robot, respond with status as part of the command
+Slicer: Send "START_UP" Workphase command<br />
+Robot: Initialize robot (INITIALIZE command) and respond with status message
 == PLANNING ==
-Respond with status
+Robot: Do nothing, respond with status
 == CALIBRATION ==
-Robot responds with status
+Robot: Do nothing, respond with status. Waiting for SET_Z_FRAME command (Z-Frame transformation).
 == TARGETING ==
-Robot responds with status
+Robot: Fail if Z-Frame was not set. Respond with status. Waiting for targets (MOVE command).
 == MANUAL ==
-Robot responds with status. <br />
+Robot: Engage breaks, responds with status. <br />
 Slicer sends "GetCoordinate" query periodically to update the display.
 == EMERGENCY ==
-Robot responds with status
+Robot: Responds with status
-= Query / Command (Slicer -> Robot) =
+= Command / Query (Slicer -> Robot) =
 == Command: INITIALIZE ==
-Initialize robot (calibration, set the encoders, this will be "home") - respond with status
+Parameters: none<br />
+Robot: Initialize, set the encoders, remember this position as "HOME". Respond with status.
 == Command: SET_Z_FRAME ==
-Z-frame position and orientation (global RAS coordinate system) - respond with status
+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 ==
-Go to coordinate - position, orientation (later: speed) (global RAS coordinate system)
+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?)
-== Query: GET_COORDINATE ==
+Global RAS coordinate system).
-Get Coordinates (position and orientation in global RAS coordinate system)
-== Query: GET_STATUS ==
+Robot responds with status.
-Get Robot Status
 == Command: INSERT ==
-Insertion depth - float (in mm)
+Parameters (4 byte): 32 bit float<br />
+Robot: Insert the needle to this depth - float (in mm)
 == Command: BIOPSY ==
-Take biopsy (Retract Cannula)
+Parameters: none<br />
+Robot: Take biopsy (Retract Cannula)
-== Query: GET_BIOPSY ==
-Get Actual Insertion Depth
 == Command: HOME ==
-Go to Home (collapses the robot to remove/replace needle)
+Parameters: none<br />
+Robot: Go to Home (collapses the robot to remove/replace needle)
 == Command: STOP ==
-Stop moving
+Parameters: none<br />
+Robot: Stop moving
 == Command: E_STOP ==
-Emergency Stop (stop + purges the air lines)
+Parameters: none<br />
+Robot: Emergency Stop (stop + purges the air lines)
 == Command: SET_JOINT ==
-Set Joint Positions - float[max.16] - move joint here
+Parameters: a set of 32bit floats. The number of floats is BODY_SIZE/4.<br />
+Robot: Set Joint Positions - float[max.16] - move joint here
 == Command: SET_VELOCITY ==
-Set Joint Velocities - float[max.16] - move joint with this speed
+Parameters: a set of 32bit floats. The number of floats is BODY_SIZE/4.<br />
+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.<br />
+Robot: sets the air pressure / voltage output
+== Command: LOCK ==
+Parameters: none<br />
+Robot: Engage brakes - all of them
+== Command: UNLOCK ==
+Parameters: none<br />
+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)<br />
+Robot: Engage brakes on a give axis
+== Command: UNLOCK_AXIS ==
+Parameters: one or more 32bit integers (BODY_SIZE/4)<br />
+Robot: Unlock - disengage the brakes on a give axis, so it can be moved manually
+== Command: LIMIT_VELOCITY==
+Parameters: one 32 bit float<br />
+Robot: Set entire robot velocity limit in Robot Coords. - float (like 10 mm/s)
+== Query: GET_COORDINATE ==
+Parameters: none<br />
+Robot: Respond with current coordinate (position and orientation in global RAS coordinate system) - 28 bytes
+== Query: GET_STATUS ==
+'''This was moved to the generic header''' implemented by every device!<br />
+Parameters: none<br />
+Robot: Respond with STATUS packet
+== Query: GET_BIOPSY ==
+Parameters: none<br />
+Robot: Respond with Actual Insertion Depth
 == Query: GET_NR_JOINT ==
-Get Number of Joints - integer
+Parameters: none<br />
+Robot: Get Number of Joints - integer, see the description below
 == Query: GET_JOINT ==
-Get Actual Joint Positions
+Parameters: one or more 32bit integers (BODY_SIZE/4)<br />
+Robot: Get Actual Joint Positions
 == Query: GET_VELOCITY==
-Get Actual Joint Velocities
+Parameters: one or more 32bit integers (BODY_SIZE/4)<br />
+Robot: Get Actual Joint Velocities
-== Query: GET_AIR_PRESSURE ==
+== Query: GET_VOLTAGE_OUTPUT ==
-Get Actual Air Pressures
+Parameters: one or more 32bit integers (BODY_SIZE/4)<br />
+Robot: Get Actual Air Pressures
 == Query: GET_JOINT_TRACK ==
-Get Actual Joint Tracking Errors
+Parameters: one or more 32bit integers (BODY_SIZE/4)<br />
+Robot: Get Actual Joint Tracking Errors
 == Query: GET_BRAKE ==
-Get Brakes status
+Parameters: none<br />
+Robot: Get Brakes status
 == Query: GET_MOVING ==
-Get Joint Moving status
+Parameters: none<br />
+Robot: Get Joint Moving status
-== Command: LOCK ==
+= Responses sent by the robot (Robot -> Slicer) =
-Lock (engage brakes - all of them)
-== Command: UNLOCK ==
+== Status: STATUS ==
-Unlock (disengage the brakes so it can be moved manually)
+Response from Robot: Robot done moving, Error message, Emergency stop<br />
+See [[OpenIGTLink/Protocol#STATUS]] for description.
-== Command: LIMIT_VELOCITY==
+== Status: BRAKES ==
-Set entire robot velocity limit in Robot Coords. - float (like 10 mm/s)
+Response from Robot: Brake status - boolean[max.16] - true(locked)/false(loose) for each joint<br />
+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.
-= Responses sent by the robot (Robot -> Slicer) =
+== Response: COORDINATES ==
-== Value: COORDINATES ==
+Response from Robot: Actual coordinates and insertion depth - 6DOF in global RAS coordinate system<br />
-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
-== Status: STATUS ==
+* Vector of four 32 bit floats (4x4 bytes): orientation quaternion
-Robot done moving, Error message, Emergency stop
+* Three 32 bit floats (3x4 byte): needle offset (to calculate Depth from Z) [X_offset, Y_offset, Z_offset + InsertionDepth]
 == Response: JOINTS ==
-Number of Joints - integer
+Response from Robot: Number of Joints - integer<br />
+Structure: one 32 bit integer (4 bytes total)
 == Response: JOINT_POS ==
-Actual joint positions - float[max.16] (joint positions in mm)
+Response from Robot: Actual joint positions - float[max.16] (joint positions in mm)<br />
+Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.
 == Response: JOINT_VEL ==
-Actual joint velocities - float[max.16] (joint velocities in mm/s)
+Response from Robot: Actual joint velocities - float[max.16] (joint velocities in mm/s)<br />
+Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.
 == Response: JOINT_TRACK ==
-Actual joint tracking error - float[max.16] - "actual joint position" - "set joint position"
+Response from Robot: Actual joint tracking error - float[max.16] - "actual joint position" - "set joint position"<br />
+Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.
 == Response: AIR_PRESSURE ==
-Actual air pressures - float[max.32] - two per joint! (PSI for each valve)
+Response from Robot: Actual air pressures - float[max.32] - '''two per joint!''' (PSI for each valve)<br />
+Structure: a set of 32bit floats. The number of floats is BODY_SIZE/4.
-== Status: BRAKES ==
+= Mimimum command set =
-Brake status - float[max.16] - true(locked)/false(loose) for each joint
+Command to the robot:
+# [[#START_UP | START_UP]] Internally calls the INITIALIZE command.
+# [[#Command:_SET_Z_FRAME | SET_Z_FRAME]] Stores the Z-Frame.
+# [[#TARGETING | TARGETING]] Just checks the Z-Frame values.
+# [[#Command:_MOVE_TO | MOVE_TO]] Repeat this command for each target.
+# [[#MANUAL | MANUAL]] Changes to Manual insertion. Switch back to automatic by calling TARGETING.
+# [[#Command:_INSERT | INSERT]] Automatic insertion. Not yet implemented on the hardware.
+# [[#Command:_BIOPSY | BIOPSY]] Takes the biopsy automatically, see INSERT.
+# [[#Command:_HOME | HOME]] Ex. after manual insertion to change the biopsy gun.
+# [[#EMERGENCY | EMERGENCY]] If something is wrong. To get out of this state START_UP needs to be called again.
+# [[#Query:_GET_COORDINATE | GET_COORDINATE]] Call this as often as needed to get the latest position of the robot.
+# [[#Query:_GET_STATUS | GET_STATUS]] Will respond with OK or other status - again, called often.
-== Status: DONE_MOVING ==
+Response from the robot:
-Done moving/joint - float[max.16] - true/false for each joint
+# [[#Status:_STATUS | STATUS]]
+# [[#Response:_COORDINATES | COORDINATES]]

Difference between revisions of "OpenIGTLink/Protocol/JHUBRP"