Introduction

We calulate the torque on a joint and its position as follows:

(1) Torque = LCGain * LCVoltage + LCBias

(2) Theta = PGain * PVoltage + PBias

Note that LCGain is in Nm/V (not Nm/count) and PGain is in rad/v (not rad/count) since the count/voltage relationship drifts. The AD converters have their own internal calibration circuitry to facilitate calibration. The controller calibrates the AD converters regularly (the manual recommends hourly) and uses these results to convert the counts to voltages. The ControlShell device driver for these AD converters contains all the code for this calibration, making this one step of calibration transparent.

The load cells and the potentiometers also drift, requiring new torque and position gains and biases. Since the robot is not mounted permanently, the gravity vector can also change. Further, the mass moments of each link of a robot (which must be known to provide accurate gravity compensation) are not necessarily known even by the manufacturer. The following discussion details the methods and software used to calibrate these values.

(As an aside, the filtering boards which condition the voltage signals also apply a gain and a bias to the raw input signal. The purpose is simply to scale and adjust the voltage input to make the best possible use of the -10V to +10V range of the AD converters. These conditioning gains and biases are adjusted by changing the potentiometers located on the backplane powering the filter boards. If you change these settings, the torque and position gains will have to change by large amounts. You will find the specs for these filter boards in the Sarcos manuals.)

Methods

1. Identifying Mass Parameters for Gravity Compensation and Automatic Torque Sensor Calibration Donghai Ma and John M. Hollerbach
2. Gravity Based Autonomous Calibration for Robot Manipulators Donghai Ma, John M. Hollerbach and Yangming Xu

Summary

Files

Contents

• Step 0
• Step 1
• Step 2
• Step 3
• Step 4
• Step 5
• Notes on Using the Controller

In order to calibrate the position gains and biases, we assume that each joint has fixed joint limits, and thus each joint has a fixed joint range in radians. We define our coordinate system for the first joint so that one joint limit coincides with the zero position for that joint, and the other joint limit is at a positive joint position equal to the joint range. For the next five joints, the zero position is defined following the DH convention: the zero position of the robot is that which aligns the x axes of all joint coordinate systems. Because there is a branch at the wrist (into finger and thumb links), the orientation of the wrist frame depends on whether you follow the thumb or finger links. So there are two sets of alpha's, a's, and d's: one for the 9-link robot from shoulder to thumb tip, and one for the 8-link robot from shoulder to finger tip. But each set follows the convention that the zero position aligns the joint axes.

Using the optotrak, we determine the joint ranges by circle fitting, and the joint angle positions of the joint limits by calibrating the dh parameters to determine new potentiometer gains and biases and then reading the position of the joint limits. This needs to be performed only once, since we assume the joint limits are immobile.

We can then calibrate PGain and PBias for a given joint by moving the joint to its highest and lowest limit and recording the potentiometer readings. We know the joint angle for these positions, and so these two data points allow us to calculate PGain and PBias.

One complication: The device driver for the AD converter applies gains and biases to its readings before making them available to the user. If you set the gain to unity and the bias to zero, then the device driver returns the raw voltage (equation 2 above). However, if you are using one of the standard ControlShell applications to interact with the arm, the gains and biases will be set to some nominal values which are close to the ideal values but not exact due to drift since they were determined. If you want to move each joint to its limits by hand (i.e. with no hydraulic help), then it is acceptable to simply modify the gains and biases to unity and zero respectively so that the values coming out of the device driver represent the raw voltage read. But if you intend to use position control software to move the arm, then you need fairly accurate gains and biases simply to enable position control.

So, I give the math for calibrating the position gains and biases assuming some nominal values are already in use, and document the matlab code which moves the arm, takes data, and processes the values.

PGain_a = position gain currently in use

PBias_a = position bias currently in use

Theta_a1 = measured joint angle at limit 1

Theta_a2 = measured joint angle at limit 2

PGain_d = calibrated gain value (i.e. desired)

PBias_d = calibrated bias value (i.e. desired)

Theta_d1 = what the joint angle should be at limit1

Theta_d2 = what the joint angle should be at limit2

V₁ = potentiometer voltage at limit1

V₂ = potentiometer voltage at limit2

Now, from equation (2) above,

(3) Theta_a1 = PGain_a * V₁ + PBias_a

(4) Theta_a2 = PGain_a * V₂ + PBias_a

(5) Theta_d1 = PGain_d * V₁ + PBias_d

(6) Theta_d2 = PGain_d * V₂ + PBias_d

Expressing PGain_d and PBias_d in terms of the measured quantities gives:

(7) PGain_d = PGain_a * (Theta_d2 - Theta_d1)/(Theta_a2 - Theta_a1)

(8a) PBias_d = Theta_d1 - (Theta_a1-PBias_a)(Theta_d2 - Theta_d1)/(Theta_a2 - Theta_a1)

(8b) PBias_d = Theta_d2 - (Theta_a2-PBias_a)(Theta_d2 - Theta_d1)/(Theta_a2 - Theta_a1)

The matlab file calibrate_step0.m will perform all the arm motion and data collecting required to perform this calibration. The matlab file process_step0.m will take the resulting data and calculate the new values for gain and offset. (Note that the process_step0 code works regardless of what those old gains and biases were, but the calibrate_step0 code will only work if these old biases and gains are nominal values, since somewhat accurate values are needed for position control of the robot.) The calibrate_step0 code assumes that the control software in ~freier/cs/master_ppc is running on vxw0, and the software in ~freier/cs/m68k is running on vxw1. To run this step, start matlab version 5.0 in the /home/robotics/rod/matlab_unixside directory and enter these commands at the matlab prompt:

>> fd = ConnectToRobot('vxw0',5070)
>> SetPCStatusSafe(fd)
>> HaltRobot(fd)

When you're satisfied the arm is warmed up, enter:

>> [t1,t2] = calibrate_step0(fd,robot)

When it's finished, do the following to calculate the new potentiometer gains and biases, with robot=1 for the master and robot=2 for the slave:

>> [newg,newb] = process_step0(t1,t2,'/home/robotics/calibration/csmats/mastercalibration.csmats')

WARNING: Process_step0 will update this calibration file with new values for PBias and PGain. But it assumes that the data was collected using the original values in this file. Therefore, you should only run process_step0 ONCE for a given set of step 0 data. If you want to repeat the calibration, you must reboot the controller (so that the new calibration values are used), rerun the calibration to collect new data, and then run process_step0 on this new data.

The code will ask you if you want to update the calibration file or not. If you update it, the old one will be backed up.

To collect data for this step, reboot the controller software (to make sure it's using the current calibration values), connect to the robot, and warm up the arm as described for step 0. Then in matlab,

> calibrate_step1(fd,1,'/home/robotics/calibration/data/master/step1/')

After the collection is complete, you may wish to examine the data. To do this, switch to the directory holding the j?.1 files and run the viewstep1data.m script. This will graph the positions of the 7 joints and the torque on the 7 joints for each pose of the rotating joint, for a total of 14 graphs for each j?.1 file.

You should check to make sure that each joint except the rotating one is held fixed at one position. The graphs should show a variance of no more than a few counts for the stationary joints. If some of these joint positions vary too much, it may be a sign that you need to increase the controlbias which holds the joints against their limits. Do this by modifying the cbscale vector in the calibrate_step1.m file: increase the value corresponding to the joint which was not held firmly enough against the limit, and then rerun the collection. (Actually, it may not help much to do this, since the cbscale values are already rather large; a value of 5 means that 5 volts off additional control bias are added to the control signal to keep it against a limit, and this already seems excessive.)

If you're satisfied that the data is adequate, process in matlab with

> process_step1('/home/robotics/calibration/data/master/step1/','/home/robotics/calibration/csmats/mastercalibration.csmats')

WARNING: Process_step1 will update this calibration file with new values for LBias. But it assumes that the data was collected using the original values in this file. Therefore, you should only run process_step1 ONCE for a given set of step 1 data. If you want to repeat the calibration, you must reboot the controller (so that the new calibration values are used), rerun the calibration to collect new data, and then run process_step1 on this new data.

The code will ask you if you want to update the calibration file or not. If you update it, the old one will be backed up.

Warm up the arm as usual. It doesn't matter whether or not the glove is attached. Collect the data as follows:

> calibrate_step2(fd,1,'/home/robotics/calibration/data/master/step2/')

Check to make sure the data is good using viewstep2data.m. It will plot the 7 position and 7 torque signals for each pose of joint 2.

If you're satisfied, process the data with

process_step2(1,'/home/robotics/calibration/data/master/step2/','/home/robotics/calibration/csmats/mastercalibration.csmats') 

This will graph the torque on joint 2 as a function of its position for two poses of joint 1. It will also graph the sinusoid fit. Make sure the fit is good.

The code will then calculate the gravity vector in frame -1 and ask the user where to save the results. You will usually just update the g value in the calibration file. Note that since none of the torque or position sensor values are updated, it is safe to run this processing step multiple times for the same set of step 2 data; you should just get the same gravity vector each time.

In order to collect data for this step, first make sure you have 2 hours available to watch the arm. Then attach the additional mass to the last link in position 1.R eboot the controller software (to make sure it's using the current calibration values), connect to the robot, make sure the glove is removed from the arm and the additional mass is attached in position 1, and warm up the arm as usual. Then in matlab:

> calibrate_step3(fd,1,'/home/robotics/calibration/data/master/step3/')

Master with additional mass in position 1	Master with additional mass in position 2

This will create files jm.n in the specified directory, where m is the number of the joint being calibrated, and n = 1,2,3 or 4, where 1 means joint position 1, load position 1; 2 means joint position, load position 1; 3 means joint position 1, load position 2; and 4 means joint position 2 load position 2.

When the data collection is complete, examine it using viewstep3data(dir). If it looks ok, you can process it with

> process_step3('/home/robotics/calibration/data/master/step3/','/home/robotics/calibration/csmats/mastercalibration.csmats',.538)

This code will graph and fit the data, giving you an opportunity to spot a bad fit. It will then calculate the new LGain, which can be saved to the calibration file, and the errors in PBias. The PBias errors are not currently used, but could easily be used to adjust the ideal joint ranges so that step0 could readjust PBias, if we decide this step is accurate enough for that purpose.

After steps 0 through 3 have been performed and the calibration csmat file is up to date, this step will calibrate the inertial mass moments by measuring the torque curve when each joint is rotated through its full range.

Warm up the arm as usual. Make sure the glove is attached. (We want to compensate for the weight of the glove as well.)

In matlab, the command

> calibrate_step4(fd,1,'/home/robotics/calibration/data/master/step4/');

> viewstep4data('/home/robotics/calibration/data/master/step4/');

> process_step4('/home/robotics/calibration/data/master/step4/','/home/robotics/calibration/csmats/mastercalibration.csmats');

Like step 3, this step calibrates the torque sensor gains (LGain), but this time it does it by rotating each joint through its range, collecting torque data, and using the inertial mass moments to calculate the expected torque. The ratio of expected to actual gives the torque gain. This procedure is faster than step 3, and can be done with no additional mass, but it assumes the mass moments are known.

Warm up the arm as usual. Make sure the glove is attached, since the mass moments should include the glove.

In matlab, the command

> calibrate_step5(fd,1,'/home/robotics/calibration/data/master/step5/');

> viewstep5data('/home/robotics/calibration/data/master/step5/');

> process_step5('/home/robotics/calibration/data/master/step5/','/home/robotics/calibration/csmats/mastercalibration.csmats');

First, the interface between the matlab scripts and the controller software is a simple TCP/IP socket. If you are running one of the data collection scripts and you decide to stop it with CTRL-C, you can interrupt it in the middle of receiving data from a socket, which will leave an incomplete message packet in the socket. If you run another script immediately afterwards, matlab may receive this remaining portion of a packet. The mex interface code will consider this a communications protocol error and will exit matlab. If this happens, reboot the controller and start over. I have no solution for this problem, other than to recommend that you don't break a running script with CTRL-C.

Second, if you want to write your own scripts for collecting data, and you look at the calibrate_step*.m scripts for examples, you may find them a little daunting. To properly force joints against limits and to hold joints accurately at set points, the code often has to change integration constants and supply additional control bias values. And because the controller will attempt to change some of these values gradually, the way you change the values is important.

For example, it is a bad idea to turn integration off by simply setting KIntegrate to zeroes. Because an abrupt change in the integration constants would lead to an abrupt change in the control, the controller will gradually apply any change in KIntegrate over a period of 3 seconds. But this still isn't good enough; if you set KIntegrate to zeroes, the integration process will still work to keep the position steady even though the integration constants are changing. When the integral limit is reached, then the reduction in KIntegrate becomes effective, and the integral term goes to zero. During this process the arm can contort, possibly violently. So instead you should shut integration off before making any change to KIntegrate by setting velTol to 0. (velTol is the velocity tolerance below which the integral constant undergoes exponential decay; above this tolerance, the integral term is calculated normally.) Then pause for a few seconds to let the integral term decay. (The decay is set to .1 per second, so in 3 seconds, the accumulated integral term will decay to .001 times its original value.) Then you can safely change KIntegrate, and reset velTol to its original value to reactivate integration.

Also, it's imporant to remember to remove any control bias before attempting movement, and to allow 3 seconds for the change in control bias to complete.

Third, if you stop one of the data collection scripts before it has a chance to clean up the changes it has made, you may end up with strange settings for the integration constants and control biases. It's safest just to reboot the controller whenever you have to stop a script, although it would be straightforward to write a matlab script that would place the robot in a known state with acceptible KIntegrate, velTol, and controlbias values. And if such a script were designed to read from the file descriptor until no other data was readable, it may solve the first problem.

Fourth, the controller will occasionally fail to start correctly. The symptom is an access fault right after starting the hydraulics, heralded by an audible beep. I still don't know what causes this failure, so the only thing to do is reboot the controller if this happens.

Finally, it's very difficult to find integration constants that avoid oscillation while still providing good convergence to the desired position. I have had to watch step 3 closely, especially when joint 4 is moving off of a joint limit, since this joint is particularly bouncy and may begin to shake. If it does start bouncing, I just grab the arm and stabilize it before it begins taking data at that position.

.