iron76
commented
9 years ago %solveID Inverse Dynamics with sparse-reduced Newton-Euler Algorithm (redNEA) % This function solves the inverse dynamics problem with the sparse % Newton-Euler algorithm, as described in the paper "BERDY: Bayesian % Estimation for Robot Dynamics. A Probabilistic Estimation of Whole-Body % Dynamics with Redundant Measurements." The output 'd' is structured as % follows: % % d = [d_1, d_2, ..., d_obj.IDstate.n] % % where: % % d_i = [a_i, f_i, d2q_i] % % and a_i is the link-i spatial accelration, f_i is spatial wrench % transmitted to link-i from its parent and d2q_i is the acceleration of % joint-i. The input to the algorithm is in obj.IDmeas.y organized as % follows: % % obj.IDmeas.y = [y_1, y_2, ... , y_obj.IDsens.m] % % The relationship between d and y is given by Y(q, dq) d = y where the % matrix Y(q, dq), is represented as a sparse matrix. Moreover, the % variables d should satisfy the Newton-Euler equations represented as % D(q,dq) d + b(q, dq) = 0, again represented as a sparse matrix. % Differently from SNEA, this algorithm does not explicitly represent the % quantities fB_i (net spatial force on the link-i) and tau_i (torque on % joint-i) and fx_i (external force on link-i). If measurements are % availalbe a suitable entry in the matrix Y(q, dq) is defined as follows: % % fB_i = I_i a_i + v_i x* I_i v_i % % tau_i = S_i^T f_i % % fx_i = X__i0^(-1) (f_i - I_i a_i - vi x I_i v_i - sum_j X*_ij f_j) % % % Author: Francesco Nori % Genova, Dec 2014
The 'd' vector can be reduced to include only f_i and a_i