├── README.md ├── do_estimation.m ├── do_simulation.m ├── estimate_payload.m ├── model_6_dof.slx ├── model_6_dof_r2014b.slx └── skew.m /README.md: -------------------------------------------------------------------------------- 1 | # payload_estimation 2 | Implementation of payload estimation algorithm for a robot. 3 | 4 | **Reference:** 5 | C. G. Atkeson, C. H. An, and J. M. Hollerbach, “Estimation of Inertial Parameters of Manipulator Loads and Links,” Int. J. Rob. Res., vol. 5, no. 3, pp. 101–119, Sep. 1986. 6 | 7 | **How to Run:** 8 | 1. Run "do_simulation.m". 9 | 2. Run "do_estimation.m". 10 | 11 | **The estimation function:** 12 | Check: "estimate_payload.m". 13 | 14 | **6-DOF rigid body (Simulink + Simmechanics):** 15 | Check: "model_6_dof.slx". 16 | 17 | manurunga@yandex.com 18 | -------------------------------------------------------------------------------- /do_estimation.m: -------------------------------------------------------------------------------- 1 | k = 200; % Starting index 2 | 3 | % Force, torque and gravity are respect to global/world frame 4 | % Therefore, we need the rotation matrix of the rigid body from body to 5 | % global frame 6 | rot = R(k:end, :); % from body to global frame 7 | f_g = force(k:end, :)'; % forces in global frame [N] 8 | n_g = torque(k:end, :)'; % torques in global frame [Nm] 9 | g_g = [0, 0, -9.81]'; % global frame [m/s^2] 10 | 11 | omega = omega(k:end, :); % angular velocity in body frame [rad/s] 12 | omegad_dot = omega_dot(k:end, :); % angular acceleration in body frame [rad/s^s] 13 | a = acc(k:end, :); % joint acceleration in body frame [m/s^s] 14 | 15 | % Very crucial: convert forces, torques, and gravity vector into body frame 16 | N = length(f_g(1, :)); 17 | f = zeros(N, 3); 18 | n = zeros(N, 3); 19 | g = zeros(N, 3); 20 | 21 | for i = 1 : N 22 | roti = reshape(rot(i,:), 3, 3); 23 | f(i,:) = (roti.' * f_g(:,i))'; 24 | n(i,:) = (roti.' * n_g(:,i))'; 25 | g(i,:) = roti.' * g_g; 26 | end 27 | 28 | % Run the estimation: 29 | [m, c, I, A, res] = estimate_payload(f, n, omega, omegad_dot, a, g); 30 | 31 | disp('Mass:') 32 | disp(m) 33 | disp('CoM:') 34 | disp(c) 35 | disp('Inertias:') 36 | disp(I) 37 | -------------------------------------------------------------------------------- /do_simulation.m: -------------------------------------------------------------------------------- 1 | %% Cleaning up, warning!! 2 | clear all 3 | clc 4 | 5 | %% Parameters to estimate. Later, compare the estimation results with these: 6 | % Ixx = ; 7 | % Ixy = ; 8 | % Ixz = ; 9 | % Ixx = ; 10 | % Iyy = ; 11 | % Izz = ; 12 | % Inertias = [Ixx Ixy Ixz; Ixy Iyy Iyz; Ixz Iyz Izz]; 13 | Inertias = eye(3); % kg*m^2 14 | m = 0.765; % kg 15 | CoM = [0.058 -0.007 0]; % m 16 | 17 | %% Do the simulation 18 | sim_time = 50; 19 | ts = 0.0050; 20 | 21 | % This is using MATLAB R2015b 22 | sim('model_6_dof.slx'); -------------------------------------------------------------------------------- /estimate_payload.m: -------------------------------------------------------------------------------- 1 | function [ m, c, I, A, res] = estimate_payload(f, n, omega, omegad, a, g) 2 | % Seraina Anne Dual 3 | % 29.07.2014 4 | % Auralis Manurung 5 | % 30.07.2015 6 | 7 | % Payload estimation based on IJRR papaer: "Estimation of Inertial 8 | % Parameters of Manipulator Loads and Links" by Atkenson et. al. 9 | % 10 | % INPUT: 11 | % N measured values of forces (f), torques (n), angular velocity 12 | % (omega), angular acceleration (omegad), linear accleration (a), and 13 | % gravity vector (g), where: 14 | % f is N x 3 (N) 15 | % n is N x 3 (Nmm) 16 | % omega is N x 3 (rad/s) 17 | % omegad is N x 3 (rad/s^2) 18 | % a is N x 3 (m/s^2) 19 | % g is N x 3 (m/s^2) 20 | % Measurments are respect to joint origin coordinate system (see p.104 21 | % of the aforementioned paper). 22 | % 23 | % OUTPUT: 24 | % parameter vector of the inertial parameters: 25 | % [m, [c2,c2,c3]',[Ixx,Ixy,Ixz,Iyy,Iyz,Izz]'] 26 | % where: 27 | % m is the mass of the body 28 | % [c1 c2 c3] are the coordinates of the center of mass of the body 29 | % [Ixx,Ixy,Ixz,Iyy,Iyz,Izz] are the moments of inertia with respect to 30 | % the center of mass of the body. 31 | 32 | %% Prepare some storages 33 | N = length(f(:,1)); % number of measurement points 34 | 35 | wn = zeros(N*6,1); 36 | omegax = zeros(N*3,3); 37 | omegadx = zeros(N*3,3); 38 | dotomega = zeros(N*3,6); 39 | dotomegad = zeros(N*3,6); 40 | A = zeros(N*6,10); 41 | 42 | %% Build the augmented matrix (see equ. 8) 43 | for i = 1 : N 44 | wn(6*(i-1)+1:6*i) = [f(i,:),n(i,:)]; 45 | 46 | % define variables 47 | omegax(3*(i-1)+1:3*i,:) = skew(omega(i,:)'); 48 | omegadx(3*(i-1)+1:3*i,:) = skew(omegad(i,:)'); 49 | 50 | g_a = a(i,:)-g(i,:); 51 | g_ax = skew(-g_a'); 52 | 53 | dotomega(3*(i-1)+1:3*i,:) = [omega(i,1), omega(i,2), omega(i,3), 0, 0, 0; 54 | 0, omega(i,1), 0, omega(i,2), omega(i,3), 0; 55 | 0, 0, omega(i,1), 0, omega(i,2), omega(i,3)]; 56 | 57 | dotomegad(3*(i-1)+1:3*i,:)=[omegad(i,1), omegad(i,2), omegad(i,3), 0, 0, 0; 58 | 0, omegad(i,1), 0, omegad(i,2), omegad(i,3), 0; 59 | 0, 0, omegad(i,1), 0, omegad(i,2), omegad(i,3)]; 60 | 61 | % combine parameter matrix 62 | A((i-1)*6+1:i*6,:) = [g_a', (omegadx(3*(i-1)+1:3*i,:)+omegax(3*(i-1)+1:3*i,:)*omegax(3*(i-1)+1:3*i,:)), zeros(3,6); 63 | zeros(3,1), g_ax, dotomegad(3*(i-1)+1:3*i,:)+omegax(3*(i-1)+1:3*i,:)*dotomega(3*(i-1)+1:3*i,:)]; 64 | end 65 | 66 | %% Do the least square 67 | 68 | p_ls = A\wn; 69 | 70 | wn_est = A * p_ls; 71 | res = wn - wn_est; 72 | 73 | m_ls = p_ls(1); 74 | cm_ls = p_ls(2:4); 75 | 76 | % This is respect to joint origin 77 | I_ls = [p_ls(5), p_ls(6), p_ls(7); 78 | p_ls(6), p_ls(8), p_ls(9); 79 | p_ls(7), p_ls(9), p_ls(10)]; 80 | 81 | % Find actual center of gravity and moment of inertia at center of mass 82 | m = m_ls; 83 | c = cm_ls/m_ls; 84 | % Convert to about the center of the body mass (see equ. 6) 85 | I = I_ls-m * ((c.'*c) * eye(3) - (c*c.')); 86 | 87 | % When rank(A) < 10, calculated inertias are worthless, but mass and CoM 88 | % might still be correct 89 | if rank(A) < 10 90 | I = zeros(3,3); 91 | if rank(A) < 4 % Minimum rank to estimate mass and COM 92 | m = 0; 93 | c = zeros(3,1); 94 | end 95 | end 96 | 97 | end -------------------------------------------------------------------------------- /model_6_dof.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/payload_estimation/7c4c73b80bfcc17f9f1ac552172b4b8abe227092/model_6_dof.slx -------------------------------------------------------------------------------- /model_6_dof_r2014b.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/payload_estimation/7c4c73b80bfcc17f9f1ac552172b4b8abe227092/model_6_dof_r2014b.slx -------------------------------------------------------------------------------- /skew.m: -------------------------------------------------------------------------------- 1 | function R = skew(w) 2 | %SKEW generates a skew-symmetric matrix given a vector w 3 | % 4 | % R = SKEW(w) 5 | % 6 | % See also: ROTAXIS, SKEWEXP, SKEWCOORDS. 7 | 8 | % $Id: skew.m,v 1.1 2009-03-17 16:40:18 bradleyk Exp $ 9 | % Copyright (C) 2005, by Brad Kratochvil 10 | 11 | if 3 ~= size(w,1), 12 | error('SCREWS:skew','vector must be 3x1') 13 | end 14 | 15 | if isnumeric(w), 16 | R = zeros(3,3); 17 | end 18 | 19 | R(1,2) = -w(3); 20 | R(1,3) = w(2); 21 | R(2,3) = -w(1); 22 | 23 | R(2,1) = w(3); 24 | R(3,1) = -w(2); 25 | R(3,2) = w(1); 26 | 27 | % R(1,1) = 0; 28 | % R(2,2) = 0; 29 | % R(3,3) = 0; 30 | 31 | end --------------------------------------------------------------------------------