search

ROBOTIS-GIT / DynamixelSDK

ROBOTIS Dynamixel SDK (Protocol1.0/2.0)
http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/
Apache License 2.0
466 stars 412 forks source link

Input to output motion differences in dynamixel motors #551

Open Raj-Kiran127 opened 2 years ago

Raj-Kiran127 commented 2 years ago

ISSUE TEMPLATE ver. 1.2.0

Please fill this template for more accurate and prompt support.

  1. Which DYNAMIXEL SDK version do you use? 3.7.31

  2. Which programming language/tool do you use? MATLAB R2021b

  3. Which operating system do you use? Windows 10

  4. Which USB serial converter do you use? U2D2

  5. Which DYNAMIXEL do you use? MX-28(2.0)AT

  6. Have you searched the issue from the closed issue threads? Yes

  7. Please describe the issue in detail I am using 3 MX-28AT (2.0) motors connected in a daisy chain. For a desired output (which is attached in the image) I have written a code(attached as well) in which there is some lag between the desired output and the output generated by the motors. Is it MATLAB issue ? Or communication speed issue ? If not, can you please tell me what might be the problem am facing here? (Code of sync-read write is used as base to write my code. )

  8. How can we reproduce 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/(2tanh(2.7)) tanh(2.7sin(2pift)); phi = -300/(2asin(0.8)) asin(0.8 cos(2pift)) ; 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

Motor-I_O-Position

ROBOTIS-Will commented 2 years ago

Hi @Raj-Kiran127

Usually the USB device introduces the latency which is set to 16ms by default in the OS. In order to minimize the latency, please adjust the USB latency as below. https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/#usb-latency-setting

Thanks.