This section describes the spatial reference frames, the config files, and the method applied for spatial calibration of the fMRI robot. There are different spatial reference frames that need to be transformed. Important system constants for this transformation are stored for each robot in the configuration file. During calibration of the robot these system constants need to be recalculated.
Spatial reference frames
Raw encoder counts: the optical encoders on elbow and shoulder deliver raw counts that are set to zero when the angle crosses the index. Because different setups may lead to different directions of counts, the robot configuration file contains a number that specifies the spatial direction of the counter.
Joint Angles: Shoulder angle (θ1), and Elbow angle (θ2) are defined that they are 0 in the zero-position: the position when the upper arm inline with body of the robot and elbow bend at 90 deg (see Figure 1). Clockwise rotation is defined as positive, counter-clockwise rotation is negative. Note that the elbow angle is in absolute coordinates rather than in relative coordinates.
Local (robot) reference frame: Reference frame with the origin centered on the main axle of the robot. x- and y-direction are standard Cartesian, when looking down ont the robot, while standing behind it. Local coordinates are calculated from the joint angles using the following formula.
Global (workspace) reference frame: Can be set arbitrarily with the matrix A (ALocalToGlobal) and the offset vector v (vLocalToGlobal). I set these matrices for a right hand robot such that the origin in the handle position when elbow angle is 0 degree and shoulder angle is 0.
For the force computation you need a model of how the pistons transmit the forces over the links (a1, a2) to the Elbow and Shoulder joint. For calculation of the piston length (p1, p2), we need d the diagonal between center of the robot and the base of the piston, and α the angle enclosed by the link at zero position of the robot and the and the diagonal.
Given this we can calculate the two relevant Jacobian Matrices
The force produce at the pistons can be captured very well by a first-order exponential low-pass filter with a time constant of ca. 15ms.
Using the Geometry of the robot, we can now calculate the forces that are produced at the endpoint in workspace coordinates:
So if we want to produce a certain force on the endpoint, we can calculate
To improve the time constant I sometimes use overdriving of the command with constant o.
Finally, it the pistons show some onset-hysteresis, such that the psitons do not really open up when small force need to be produced. Do reduce this effect, one can preactivate all the valves, such that both the push- and pull-valve are set to the same voltage, and thus do not produce any overt force. The offset constant can be set with the Manipulandum-member function setOffset().
Each robot has a calibration that is used to initialize the robot, using the function Manipulandum::init(string configfile);.The necessary system constants are set here.
2 // Channel for Shoulder Encoder
-1 // Direction for Shoulder Encoder
0 // Board for Elbow Encoder
5 // Channel for Elbow Encoder
1 // Direction for Elbow Encoder
0 // Board for DA Output to the valves
0.0082 // Offset for Shoulder encoder [radians]
0.0541 // Offset for Elbow Enconder [radians]
0.4785 // Length of upper arm [m]
0.5231 // Length of lower arm [m]
0.1270 // Link Length for Piston, Shoulder [m]
0.1245 // Link Length for Piston, Elbow [m]
0.0700 // Tranformation N to V at pistons
0.0150 // Time constant for the pistons [ms]
Note that these values are just an example, and the values for each individual robot will vary.
Spatial calibration is perfomed by moving the to robot arm to a number of predefined spatial positions in workspace. The locations of these points in cm have to be put into a tab-delimted text file. The calibration points then can be recorded using command
cal points_file out_file
in the sample program fMRIrobotTest. The locations of the sampled points, as well as the state of the shoulder and elbow counter are recorded in the out_file. The matlab routine fMRIrobot_calibrate.m can then be used to generate a new configuration file for the robot.