Differences in Input to output motion of dynamixel motors

rajkiran · February 20, 2022, 1:37pm

Use the following template to help create your post:

What model of servo are you using?
MX-28(2.0)AT
Describe your control environment. This includes the controller or interface, and any power source.
U2D2 controller with 3 motors connected in daisy chain powered by 12V SMPS.
Specify the operating mode for applicable models, and any firmware settings you are using.
Position control mode, firmware 44.
Include pictures if possible.

Motor-I_O-Position2421×1299 196 KB
Include a full description of the issue.
clc;
clear all;
close all ;

lib_name = ‘’;

if strcmp(computer, ‘PCWIN’)
lib_name = ‘dxl_x86_c’;
elseif strcmp(computer, ‘PCWIN64’)
lib_name = ‘dxl_x64_c’;
elseif strcmp(computer, ‘GLNX86’)
lib_name = ‘libdxl_x86_c’;
elseif strcmp(computer, ‘GLNXA64’)
lib_name = ‘libdxl_x64_c’;
elseif strcmp(computer, ‘MACI64’)
lib_name = ‘libdxl_mac_c’;
end

% Load Libraries
if ~libisloaded(lib_name)
[notfound, warnings] = loadlibrary(lib_name, ‘dynamixel_sdk.h’, ‘addheader’, ‘port_handler.h’, ‘addheader’, ‘packet_handler.h’, ‘addheader’, ‘group_sync_write.h’, ‘addheader’, ‘group_sync_read.h’);
end

% Control table address
ADDR_PRO_TORQUE_ENABLE = 64; % Control table address is different in Dynamixel model
ADDR_OPERATING_MODE = 11;
ADDR_PRO_PROFILE_VELOCITY = 112;
ADDR_PRO_GOAL_POSITION = 116;
ADDR_PRO_PRESENT_POSITION = 132;

% Data Byte Length
LEN_PRO_GOAL_POSITION = 4;
LEN_PRO_PRESENT_POSITION = 4;

% Protocol version
PROTOCOL_VERSION = 2.0; % See which protocol version is used in the Dynamixel

% Default setting
DXL1_ID = 1; % Dynamixel#1 ID: 1
DXL2_ID = 2; % Dynamixel#2 ID: 2
DXL3_ID = 3;
BAUDRATE = 57600;
DEVICENAME = ‘COM3’; % Check which port is being used on your controller
% ex) Windows: ‘COM1’ Linux: ‘/dev/ttyUSB0’ Mac: ‘/dev/tty.usbserial-*’

TORQUE_ENABLE = 1; % Value for enabling the torque
TORQUE_DISABLE = 0; % Value for disabling the torque
EXT_POSITION_CONTROL_MODE = 4;
DXL_MINIMUM_POSITION_VALUE = -100; % Dynamixel will rotate between this value
DXL_MAXIMUM_POSITION_VALUE = 5000; % and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
DXL_MOVING_STATUS_THRESHOLD = 20; % Dynamixel moving status threshold

ESC_CHARACTER = ‘e’; % Key for escaping loop

COMM_SUCCESS = 0; % Communication Success result value
COMM_TX_FAIL = -1001; % Communication Tx Failed

% Initialize PortHandler Structs
% Set the port path
% Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = portHandler(DEVICENAME);

% Initialize PacketHandler Structs
packetHandler();

% Initialize Groupsyncwrite Structs
groupwrite_num = groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION);

% Initialize Groupsyncread Structs for Present Position
groupread_num = groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);

% Initialize Groupsyncread Structs for Present Position
% groupwriteVel_num = groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_PROFILE_VELOCITY, LEN_PRO_PRESENT_POSITION);

% index = 1;
dxl_comm_result = COMM_TX_FAIL; % Communication result
dxl_addparam_result = false; % AddParam result
% dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE DXL_MAXIMUM_POSITION_VALUE]; % Goal position

dxl_error = 0; % Dynamixel error
% dxl1_present_position = 0; % Present position
% dxl2_present_position = 0;

% Open port
if (openPort(port_num))
fprintf(‘Succeeded to open the port!\n’);
else
unloadlibrary(lib_name);
fprintf(‘Failed to open the port!\n’);
input(‘Press any key to terminate…\n’);
return;
end

% Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
fprintf(‘Succeeded to change the baudrate!\n’);
else
unloadlibrary(lib_name);
fprintf(‘Failed to change the baudrate!\n’);
input(‘Press any key to terminate…\n’);
return;
end

% Enable Dynamixel#1 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf(’%s\n’, getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf(’%s\n’, getRxPacketError(PROTOCOL_VERSION, dxl_error));
else
fprintf(‘Dynamixel #%d has been successfully connected \n’, DXL1_ID);
end

% Enable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf(’%s\n’, getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf(’%s\n’, getRxPacketError(PROTOCOL_VERSION, dxl_error));
else
fprintf(‘Dynamixel #%d has been successfully connected \n’, DXL2_ID);
end

% Enable Dynamixel#3 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf(’%s\n’, getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf(’%s\n’, getRxPacketError(PROTOCOL_VERSION, dxl_error));
else
fprintf(‘Dynamixel #%d has been successfully connected \n’, DXL3_ID);
end

% Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL1_ID);
if dxl_addparam_result ~= true
fprintf(’[ID:%03d] groupSyncRead addparam failed’, DXL1_ID);
return;
end

% Add parameter storage for Dynamixel#2 present position value
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL2_ID);
if dxl_addparam_result ~= true
fprintf(’[ID:%03d] groupSyncRead addparam failed’, DXL2_ID);
return;
end

% Add parameter storage for Dynamixel#3 present position value
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL3_ID);
if dxl_addparam_result ~= true
fprintf(’[ID:%03d] groupSyncRead addparam failed’, DXL3_ID);
return;
end

d1 = ([]);
d2 = ([]);
d3 = ([]);

T = 1 ;
f = 1/T ;
j = 0;
alpha_old = 0;
phi_old = 0 ;
itr1 = 0 ;
itr2 = 0 ;
itr3 = 0 ;

phase_diff1 = 0 ;
phase_diff2 = 0 ;
phase_diff3 = 0 ;
for t = 0:0.005:T
j = j +1;
time(j) = t;
alpha = -140/(2 tanh(2.7)) * tanh(2.7 sin(2 pi f t));
phi = -300/(2 asin(0.8)) * asin(0.8* cos(2 pi f*t)) ;
sweep(j) = phi ;
if t<=T/2
alpha_new = alpha;
pitch(j) = alpha ;

    if abs(alpha_new-alpha_old) <= 0.0879  && j>1

        itr1 = itr1+1;
        if itr1==1
            t_dash = time(j);
        end

        if t == t_dash
            phase_diff1 = alpha - phi ;
        end
        pitch(j) = phi + phase_diff1 ;
        kite = pitch(j);

    end


    if abs(alpha_new-alpha_old) > 0.0879 && t>T/4
        itr2 = itr2 + 1;
        if itr2 == 1
            t_dash2 = time(j);
            phase_diff2 = kite - alpha ;
        end
        pitch(j) = alpha + phase_diff2;
    end

    alpha_old = alpha_new;
    pitch(1) = alpha;
end
k = j ;
if t>T/2
    alpha_new = alpha;
    pitch(k) = alpha+phase_diff2;

    if abs(alpha_new-alpha_old) <= 0.0879
        itr3 = itr3+1;
        if itr3==1
            t_dash3 = time(k);
            phase_diff3 = phi-pitch(k) ;
        end
        pitch(k) = phi - phase_diff3 ;
    end

    if t > (3*T/4) && abs(alpha_new-alpha_old) > 0.0879
        pitch (k) = alpha ;
    end
    alpha_old = alpha_new;
end

end
p1 = 900 ;
p2 = 2000 ;
p3 = 2000 ;
Q = ([]) ;

pause(1);
for i = 1 : 3
for n = 1 : length(pitch)
%motor1
q1 = pitch(1,n)/0.0879 ;
Q(1,n) = q1 ;
m_pos(1) = p1 + Q(1,n);
M1(n) = m_pos(1);

    %motor2
    q1 =  sweep(1,n)/0.0879 ;
    Q(2,n) = q1 ;
    m_pos(2) = p2 + Q(2,n);
    M2(n) = m_pos(2);

    %motor3
    q3 = sweep(1,n)/0.0879 ;
    Q(3,n) = q3 ;
    m_pos(3) = p3 + Q(3,n);
    M3(n) = m_pos(3);

% Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL1_ID, typecast(int32(m_pos(1)), 'uint32'), LEN_PRO_GOAL_POSITION);
if dxl_addparam_result ~= true
    fprintf('[ID:%03d] groupSyncWrite addparam failed', DXL1_ID);
    return;
end

% Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL2_ID, typecast(int32(m_pos(2)), 'uint32'), LEN_PRO_GOAL_POSITION);
if dxl_addparam_result ~= true
    fprintf('[ID:%03d] groupSyncWrite addparam failed', DXL2_ID);
    return;
end

% Add Dynamixel#3 goal position value to the Syncwrite parameter storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL3_ID, typecast(int32(m_pos(3)), 'uint32'), LEN_PRO_GOAL_POSITION);
if dxl_addparam_result ~= true
    fprintf('[ID:%03d] groupSyncWrite addparam failed', DXL3_ID);
    return;
end

% Syncwrite goal position
groupSyncWriteTxPacket(groupwrite_num);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
    fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end

% Clear syncwrite parameter storage
groupSyncWriteClearParam(groupwrite_num);

% Syncread present position
groupSyncReadTxRxPacket(groupread_num);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
    fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end

% Get Dynamixel#1 present position value
dxl1_present_position = groupSyncReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);

% Get Dynamixel#2 present position value
dxl2_present_position = groupSyncReadGetData(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);

% Get Dynamixel#3 present position value
dxl3_present_position = groupSyncReadGetData(groupread_num, DXL3_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);

d1(n) = dxl1_present_position;%*0.0879;
d2(n) = dxl2_present_position;%*0.0879;
d3(n) = dxl3_present_position;%*0.0879;

end
end

% Disable Dynamixel#1 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf(’%s\n’, getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf(’%s\n’, getRxPacketError(PROTOCOL_VERSION, dxl_error));
end

% Disable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf(’%s\n’, getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf(’%s\n’, getRxPacketError(PROTOCOL_VERSION, dxl_error));
end

% Disable Dynamixel#3 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf(’%s\n’, getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf(’%s\n’, getRxPacketError(PROTOCOL_VERSION, dxl_error));
end

% Close port
closePort(port_num);

% Unload Library
unloadlibrary(lib_name);

figure(1)
movegui(‘east’)
plot(time,M1,’-’,time,d1,‘x’,time,M2,’.’,time,d2,‘s’,time,M3,’-.’,time,d3,‘o’)
ylabel(‘Input Positions’)
xlabel(‘Time in Second’)
legend(‘Desired-Output-M1’,‘Motor-Output-M1’,‘Desired-Output-M2’,‘Motor-Output-M2’,‘Desired-Output-M3’,‘Motor-Output-M3’)

where,
M1,M2,M3 are desired positions to motors
d1,d2,d3 are output positions from motors

Yogurt_Man · February 21, 2022, 1:40am

As I am seeing your Time in Second legend, one of possibility is that you are updating the position too fast over its specification.

Try to loose the update period of your Input Position, and see it resolves.

rajkiran · February 21, 2022, 4:39am

Thanks for the response.
Does that mean increasing the time step size from say 0.005 to 0.05 thereby decreasing the number of input positions to motors?
In the same context, I would like to ask one more question -
Is there any scaling between the number of position inputs to the speed of the motors?

rajkiran · February 21, 2022, 6:10am

This is a simple sine wave with a reduced number of time steps.
Code snippet is attached

Q = ([]);
d1 = ([]);
e1 = ([]);
dt= 0.01;
T = 8;
k = T/dt;
time = linspace(0,T,k+1);
j = 0;
Fc = 1/T;
p1 =2000;
write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_GOAL_POSITION, typecast(int32(p1), ‘uint32’));
pause(2);
for i = 0:dt:T

j = j+1;
%motor 1
q1 =  1024*sin(2*pi*Fc*i);
Q(1,j) = q1;
m_pos(1) = p1 + Q(1,j);

% m_pos(1) = q1 ;
M1(j) = m_pos(1);

end

Yogurt_Man · February 21, 2022, 6:12am

Yes, you should give enough time to the DYNAMIXEL until it reaches the Goal Position.

When Iook at the highlighed part, DYNAMIXEL only reaches ~3000 at around 0.7 sec and the position is re-updated to the given Goal Position (3000) at the time. It means that DYNAMIXEL has failed to go “4000” in a given time.

Please see time-based profile. I guess this is what you are looking for the scailing the time for the input position. (Changing Profile Velocity under Position Control Mode - #3 by Yogurt_Man)

I don’t understand the slow curve within 0.3 to 0.4~0.5. Seems that the velocity is too slow or failed to get Goal Position during this period.

Yogurt_Man · February 21, 2022, 6:12am

Hmm… Seems it’s good result?

rajkiran · February 21, 2022, 6:16am

Yeah, but even the updated picture has the same phase shift or whatever it is called. I cannot figure out how to match both curves.

Yogurt_Man · February 21, 2022, 6:18am

How much load on it? Can you test it with no-load status?

And, hmm…

What if you are increasing the Baudrates so that you can send more packets compared to 57600 bps. This would enhance if you are sending so many packets per unit time.

Additinally I recommend, decrease the latency, and set the Return Delay Time as lower as you can.

rajkiran · February 21, 2022, 6:27am

Even without load, the test results are the same.
On load I am driving a mechanism using a belt drive .

rajkiran · February 21, 2022, 6:30am

I am using a baudrate of 4.5 Mbps now and latency is set lowest possible.
Return delay time is something I should check. Is there any other parameter am missing?

Yogurt_Man · February 21, 2022, 6:36am

I think, it should be worthy of setting time-based profile. Make 2 times larger than your frame rates as the example here

Seeing the example, the number of goal positions are updated every 100ms. and the Author tends to set the Profile Velocity (Time-based) 2 times larger that the frame rates (100ms).

Guessing this issue is simiilar to Motor overrun errors - #3 by rajkiran ?

Please allow me to update the ticket and drops other user to here, as the issue is being addressed here in the thread.

rajkiran · February 21, 2022, 7:14am

Ya I will try using time based profile once. But let me know of any other ways I should try.