Stiffness (K)

How to determine the stiffness of the servo when it is powered.

The Math Behind the Setup

The setup for these tests relies on the angular version of Hooke's law:

τ=kΔθ\tau=k\Delta\theta ----> k=τΔθk = \frac{\tau}{\Delta\theta}

Normally, Δθ\Delta\thetawould be the amount the element displaces given the applied torque. However, for the servo tests we will instead considerΔθ\Delta\thetaas the difference between the commanded position and the actual position.

One way to think about this is to consider the pendulum setup from the inertia/damping tests.

In this case, the gravitational torque is functionally creating stiffness by resisting the commanded motion, causing a difference between the actual position and commanded position. Thus, the "stiffness" in this case is:

k=mgLθcom−θactk = \frac{mgL}{\theta_{com}-\theta{act}}

However, this setup is hard on the servos, so we want to use an alternate setup. Instead, we will have the servo pressing down on a scale.

In this case, commanding a position that the servo cannot reach causes it to apply a force on the scale. That force can then be used to calculate kservok_{servo}. Alternatively, you can record several values for various commanded positions, then plot the Torque vs. Δθ\Delta\thetacurve. The slope of this line will be kservok_{servo}.

It's also crucial to consider how the servo actually produces stiffness. The Dynamixels in general produce stiffness artificially through the PID control loop in the servo. This stiffness can then be changed by altering different addresses in the servo related to the PID loop. In the case of the MX-28s, this address is the "P Gain." For other servos, the E-Manual for the servo should list the valid address to change. However, none of the manuals list how these gain values relate to Nm/rad stiffness, so the stiffness curve tests will need to be done for several Gain values to determine the relationship between them.

Testing Setup

The interfaces between the test setup and the MX-28 brackets have already been developed. The CAD files are included below.

To run the tests, the main 80-20 beam just needs to be clamped to the tabletop. Other considerations include:

  1. The place the servo pad presses down on the scale should be as close to the center as possible

  2. The fan should be blowing on the servo to help heat dissipation

See the page below for a guide on how to read data from the scale on the computer.

OHAUS Ranger 3000 Scale

CAD Files

All files for the MX-28 K setup, as well as the full setup assembly.

Taking Data

The Arduino code for this setup is pasted below:

#include <DynamixelSDK.h>

// AX-series Control table address
#define ADDR_AX_TORQUE_ENABLE           24
#define ADDR_AX_PRESENT_POSITION        36
#define ADDR_AX_GOAL_POSITION           30
#define ADDR_AX_P_GAIN                  28
#define ADDR_AX_PRESENT_TEMP            43        

#define PROTOCOL_VERSION                1.0

#define DXL_ID                          1
#define BAUDRATE                        1000000
#define DEVICENAME                      "1"

#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0

#define ESC_ASCII_VALUE                 0x1b

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while(!Serial);

  Serial.println("Start..");

  dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
  dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
  int index = 0;
  int dxl_comm_result = COMM_TX_FAIL;
  unsigned long t = 0;
  //int dxl_comm_position[10] = {2080, 2332, 2108, 2304, 2136, 2276, 2192, 2248, 2164, 2220}; //K = 16
  //int dxl_comm_position[10] = {2080, 2161, 2089, 2152, 2098, 2143, 2107, 2134, 2116, 2125}; //K = 32
  //int dxl_comm_position[10] = {2080, 2134, 2086, 2128, 2092, 2122, 2098, 2116, 2104, 2110}; //K = 48
  //int dxl_comm_position[10] = {2062, 2116, 2068, 2110, 2074, 2104, 2080, 2098, 2086, 2092}; //K = 64
  //int dxl_comm_position[10] = {2042, 2096, 2048, 2090, 2054, 2084, 2060, 2078, 2066, 2072}; //K = 96
  //int dxl_comm_position[10] = {2045, 2090, 2050, 2085, 2055, 2080, 2060, 2075, 2065, 2070}; //K = 128
  //int dxl_comm_position[10] = {2040, 2085, 2045, 2080, 2050, 2075, 2055, 2070, 2060, 2065}; //K = 160
  int dxl_comm_position[10] = {2045, 2090, 2050, 2085, 2055, 2080, 2060, 2075, 2065, 2070}; //K = 254
  int dxl_rest_position = 2020;
  int dxl_p_gain = 254;

  uint8_t dxl_error = 0;
  int16_t dxl_present_position = 0;
  uint8_t dxl_present_temp = 0;

  pinMode(BOARD_BUTTON_PIN, INPUT_PULLDOWN);
  pinMode(BOARD_LED_PIN, OUTPUT);

  // Open port
  if (portHandler->openPort())
  {
    Serial.print("Succeeded to open the port!\n");
  }
  else
  {
    Serial.print("Failed to open the port!\n");
    Serial.print("Press any key to terminate...\n");
    return;
  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE))
  {
    Serial.print("Succeeded to change the baudrate!\n");
  }
  else
  {
    Serial.print("Failed to change the baudrate!\n");
    Serial.print("Press any key to terminate...\n");
    return;
  }
  //dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);

  if (dxl_comm_result != COMM_SUCCESS)
  {
    packetHandler->getTxRxResult(dxl_comm_result);
  }
  else if (dxl_error != 0)
  {
    packetHandler->getRxPacketError(dxl_error);
    Serial.print("Dynamixel");
    Serial.print("has NOT connected \n");
  }
  else
  {
    Serial.print("Dynamixel");
    Serial.print("has been successfully connected \n");
  }

  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_P_GAIN, dxl_p_gain, &dxl_error);
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_AX_GOAL_POSITION, dxl_rest_position, &dxl_error);
  
  while(digitalRead(BOARD_BUTTON_PIN)==LOW);

  Serial.print("Comm.");
  Serial.print("\t");
  Serial.print("Actual");
  Serial.println(" ");
  
  int ii = 0;
  while(ii < 10) {
    dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_AX_GOAL_POSITION, dxl_comm_position[ii], &dxl_error);  
    delay(5000);
    dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_AX_PRESENT_POSITION, (uint16_t*)&dxl_present_position, &dxl_error);
  
    Serial.print(dxl_comm_position[ii]);
    Serial.print('\t');
    Serial.print(dxl_present_position);
    Serial.println(" ");
    dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_AX_GOAL_POSITION, dxl_rest_position, &dxl_error);
    while(digitalRead(BOARD_BUTTON_PIN)==LOW);    

    ii++;
  }
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
  Serial.print("End");
  Serial.println(" ");
  dxl_comm_result = packetHandler->read1ByteTxRx(portHandler, DXL_ID, ADDR_AX_PRESENT_TEMP, (uint8_t*)&dxl_present_temp, &dxl_error);
  Serial.print("Present Temp: ");
  Serial.print(dxl_present_temp);
}

void loop() {
  // put your main code here, to run repeatedly:


}

Essentially, the code cycles through attempting to reach each of the positions defined in one of the arrays in lines 33-40 (only one should be uncommented at a time) on each button press for 5 seconds, then goes back to the resting position. After each press on the scale ends, the code prints the commanded position and actual position. Once all positions have been attempted, the code disables the servo torque, prints the current temperature, then ends.

The positions listed in the arrays range from the minimum position to create good contact with the scale, and the approximate position at which the servo will produce max force. This helps ensure that the resulting torque-displacement curve will be linear. The data collection is randomized between these two extremes, to help ensure there isn't fatigue over the test decreasing the stiffness slope. In the past, I've selected an array order from some sort of online randomizer.

All of the position values in the base code are tuned for the stall torque of the MX-28s. If you are using other servos, experiment to find the right ranges. Some considerations for this are:

  • The minimum to ensure good contact with the scale seems to be around 2040-2045. This will likely not change unless the setup is modified

  • The difference between successive commanded positions should be≥\geq6 bits, otherwise data points will not chart well on theΔθ\Delta\thetaaxis later on.

  • Try to aim for around 10 data points for each test. This may not become feasible for higher stiffnesses to keep the difference between successive positions great enough, which is fine.

For an undetermined reason, the scale will not reach equilibrium with the servo pressing on it. Instead, it will slowly decay over time as the servo presses. However, because it decays so slowly, we can consider the value it reaches after the impulse of hitting the scale as the defacto force value. I have tuned the code to stop pressing down on the scale around the time the value will really reach equilibrium (5 s). Thus, you can use the value right before the servo stops pressing as F.

The curve generated by the servo pressing on the scale.

The steps for data collection are as follows:

  1. Open the ComDebug Logger software

  2. Upload the code to the board or restart the board. Open the Serial Monitor to begin the code.

  3. Start the Logger, then press the circle button on the board to begin a press down.

  4. Once the servo stops pressing down, pause the Logger and note down the final value before it begins to rapidly decay back to zero.

  5. Resume the logger and press the button again

  6. Repeat steps 3-5 until the code is finished.

Processing Data

function [] = StiffnessDataProcessing()
%%
clear all
close all
stiff = [16 32 48 64 96 128 160 224 254]; %Stiffness values in bits
trials = 5; %Max number of trials per set of commanded positions
servo = 'mx-28';
errors.scale = .001;
errors.posBits = 1; %In bits

%Lift data from text files
thetaComm = {};
thetaAct = {};
weights = {};
for k = 1:length(stiff)
    filename = [servo ' kp=' num2str(stiff(k)) '.txt']; 
    raw = importdata(filename);
    thetaComm{k} = raw(1,:);
    thetaAct{k} = raw(2,:);
    weights{k} = raw(3:end, :);
end

for j=1:length(stiff)
    [kServoDeg(j,:),kServoRad(j,:),errors, kRadError, kDegError] = ProcessTrials(weights{j}, thetaComm{j}, thetaAct{j}, stiff(j), trials, errors);
    kServoDegAvg(j) = sum(kServoDeg(j,:))/sum(kServoDeg(j,:)~=0);
    kServoRadAvg(j) = sum(kServoRad(j,:))/sum(kServoRad(j,:)~=0);
    errors.kDeg{j} = kDegError;
    errors.kRad{j} = kRadError;
    errors.kDegAvg(j) = norm(errors.kDeg{j})/trials;
    errors.kRadAvg(j) = norm(errors.kRad{j})/trials;

end
figure
kError = ones(1,length(kServoRadAvg)).*errors.kRadAvg;
errorbar(stiff, kServoRadAvg,kError,'-.');
hold on
pLin = polyfit(stiff, kServoRadAvg,1);
p2nd = polyfit(stiff, kServoRadAvg,2);
fLin = polyval(pLin,stiff);
f2nd = polyval(p2nd,stiff);
plot(stiff, fLin);
plot(stiff, f2nd);
xlabel('K_p Value (bits)');
ylabel('K_{servo} Value (Nm/rad)');
grid on
legend('Data Points','Linear Fit','Quadratic Fit');
end

function [kServoDeg, kServoRad, errors, kRadError, kDegError] = ProcessTrials(weights, thetaComm, thetaAct, stiffness, trials, errors)

torques = weights*9.81*.1103; %Where .1103 is the length of the presser
errors.torque = errors.scale*9.81*.1103;
[r,~] = size(weights);

%Convert thetas from bits to degrees and radians
thetaComm = thetaComm*.088;
thetaAct = thetaAct*.088;
errors.posDeg = errors.posBits*.088;
thetaCommRad = thetaComm*(pi/180);
thetaActRad = thetaAct*(pi/180);
errors.posRad = errors.posDeg*(pi/180);
delTheta = thetaComm - thetaAct;
errors.delThetaDeg = sqrt(errors.posDeg^2+errors.posDeg^2);
delThetaRad = thetaCommRad - thetaActRad;
errors.delThetaRad = sqrt(errors.posRad^2+errors.posRad^2);

%Define the error bar lengths
yneg = ones(1,length(thetaComm))*errors.torque/2;
ypos = yneg;
xneg = ones(1,length(thetaComm))*errors.delThetaDeg/2;
xpos = xneg;
errors.stiffNum = sqrt(errors.torque^2+errors.torque^2);
errors.stiffDegDen = sqrt(errors.delThetaDeg^2 + errors.delThetaDeg^2);
errors.stiffRadDen = sqrt(errors.delThetaRad^2 + errors.delThetaRad^2);

figure
t = tiledlayout(2,3,'TileSpacing','Compact');

for i=1:r
    nexttile

    %Plot the data points
    errorbar(delTheta,torques(i,:),yneg,ypos,xneg,xpos,'.');
    hold on
    %Find p values for line of best fit for the degree and radian cases
    p(i,:) = polyfit(delTheta,torques(i,:),1);
    pRad(i,:) = polyfit(delThetaRad,torques(i,:),1);
    %Use p values to create line and plot it on the figure
    f(i,:) = polyval(p(i,:),delTheta);
    plot(delTheta, f(i,:));
    title(['Trial' ' ' num2str(i)])
    grid on
    %The slope of the line of best fit is k
    kServoDeg(i) = p(i,1);
    kServoRad(i) = pRad(i,1);
    %The error of 
    kDegError(i) = kServoDeg(i)*sqrt((errors.stiffDegDen/(delTheta(end)-delTheta(1)))^2+(errors.stiffNum/((p(i,2)+p(i,1)*delTheta(end))-(p(i,2)+p(i,1)*delTheta(1))))^2);
    kRadError(i) = kServoRad(i)*sqrt((errors.stiffRadDen/(delThetaRad(end)-delThetaRad(1)))^2+(errors.stiffNum/((pRad(i,2)+pRad(i,1)*delThetaRad(end))-(pRad(i,2)+pRad(i,1)*delThetaRad(1))))^2);
end
%     if stiffness == 254
%     keyboard
%     end
title(t, ['K_p = ' num2str(stiffness)]);
xlabel(t,'\Delta\theta (deg)');
ylabel(t,'Torque (Nm)');

if r < trials
    kServoDeg(r+1:trials) = 0;
    kServoRad(r+1:trials) = 0;
end
end

The code I used to parse the MX-28 trials is presented above. The code:

  1. Converts the provided bit displacement values in bits and force values in kgs into deg/rad values and torque values, respectively.

  2. Plots the torque vs. displacement curve for each trial in a figure

  3. Finds the line of best fit for each trial and takes the slope as a possible value of kservok_{servo}

  4. Averages the trials for a value of kservok_{servo}for the value of P Gain

  5. Plots kservok_{servo}vs. P Gain to highlight the relationship between them

kServoDegAvg and kServoRadAvg are the end variables that store the values for kservok_{servo} in Nm/deg and Nm/rad, respectively.

The program imports data from files named with the convention, "<servo type> k=<p gain>" from within the same folder as the program. For example, the MX-28 files were named, "mx-28 k=16", "mx-28 k=32", etc. The files themselves should only have numbers in them. The first row should be the commanded position, the second row the actual position, and then the following rows each of the trials themselves. For example, here is the contents of the "mx-28 k=16" file:

2060	2070	2080	2090	2100	2110	2120	2130	2140	2150
2039	2041	2042	2043	2044	2046	2047	2049	2050	2051
0.1252	0.189	0.244	0.302	0.365	0.423	0.481	0.539	0.595	0.655
0.1318	0.19	0.247	0.307	0.366	0.427	0.483	0.542	0.599	0.663
0.127	0.189	0.248	0.305	0.367	0.426	0.483	0.543	0.6	0.663
0.135	0.195	0.252	0.311	0.373	0.433	0.493	0.558	0.617	0.679
0.131	0.192	0.251	0.309	0.371	0.428	0.488	0.551	0.611	0.673

While parsing the data, check each stiffness' figures to see if data points from the end need to be pruned. As you reach the maximum force the servo can apply, the τ\tau vs. Δθ\Delta\theta will begin to taper to an equilibrium, and the method the program uses to find the line of best fit is very sensitive to these points. For best results, ensure that any sort of tapering is removed.

Data points and line of best fit before pruning the data (left) and after (right).

Ideally the final plot will be roughly linear. For reference, here is the data for the MX-28s:

The K_servo vs. K_p graph for the MX-28s

Last updated

Was this helpful?