The setup for these tests relies on the angular version of Hooke's law:
τ=kΔθ ----> k=Δθτ​
Normally, Δθwould be the amount the element displaces given the applied torque. However, for the servo tests we will instead considerΔθas 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=θcom​−θactmgL​
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 kservo​. Alternatively, you can record several values for various commanded positions, then plot the Torque vs. Δθcurve. The slope of this line will be kservo​.
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:
The place the servo pad presses down on the scale should be as close to the center as possible
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.
#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≥6 bits, otherwise data points will not chart well on theΔθaxis 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 steps for data collection are as follows:
Open the ComDebug Logger software
Upload the code to the board or restart the board. Open the Serial Monitor to begin the code.
Start the Logger, then press the circle button on the board to begin a press down.
Once the servo stops pressing down, pause the Logger and note down the final value before it begins to rapidly decay back to zero.
Resume the logger and press the button again
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:
Converts the provided bit displacement values in bits and force values in kgs into deg/rad values and torque values, respectively.
Plots the torque vs. displacement curve for each trial in a figure
Finds the line of best fit for each trial and takes the slope as a possible value of kservo​
Averages the trials for a value of kservo​for the value of P Gain
Plots kservo​vs. P Gain to highlight the relationship between them
kServoDegAvg and kServoRadAvg are the end variables that store the values for kservo​ 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:
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 τvs. Δθ 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.
Ideally the final plot will be roughly linear. For reference, here is the data for the MX-28s:
