function [H,C] = HandC( model, q, qd, f_ext, grav_accn ) % HandC Calculate coefficients of equation of motion. % [H,C]=HandC(model,q,qd,f_ext,grav_accn) calculates the coefficients of % the joint-space equation of motion, tau=H(q)qdd+C(d,qd,f_ext), where q, % qd and qdd are the joint position, velocity and acceleration vectors, H % is the joint-space inertia matrix, C is the vector of gravity, % external-force and velocity-product terms, and tau is the joint force % vector. Algorithm: recursive Newton-Euler for C, and % Composite-Rigid-Body for H. f_ext is a cell array specifying external % forces acting on the bodies. If f_ext == {} then there are no external % forces; otherwise, f_ext{i} is a spatial force vector giving the force % acting on body i, expressed in body i coordinates. Empty cells in f_ext % are interpreted as zero forces. grav_accn is a 3D vector expressing the % linear acceleration due to gravity. The arguments f_ext and grav_accn % are optional, and default to the values {} and [0,0,-9.81], respectively, % if omitted. if nargin < 5 a_grav = [0;0;0;0;0;-9.81]; else a_grav = [0;0;0;grav_accn(1);grav_accn(2);grav_accn(3)]; end external_force = ( nargin > 3 && length(f_ext) > 0 ); for i = 1:model.NB [ XJ, S{i} ] = jcalc( model.pitch(i), q(i) ); vJ = S{i}*qd(i); Xup{i} = XJ * model.Xtree{i}; if model.parent(i) == 0 v{i} = vJ; avp{i} = Xup{i} * -a_grav; else v{i} = Xup{i}*v{model.parent(i)} + vJ; avp{i} = Xup{i}*avp{model.parent(i)} + crm(v{i})*vJ; end fvp{i} = model.I{i}*avp{i} + crf(v{i})*model.I{i}*v{i}; if external_force && length(f_ext{i}) > 0 fvp{i} = fvp{i} - f_ext{i}; end end for i = model.NB:-1:1 C(i,1) = S{i}' * fvp{i}; if model.parent(i) ~= 0 fvp{model.parent(i)} = fvp{model.parent(i)} + Xup{i}'*fvp{i}; end end IC = model.I; % composite inertia calculation for i = model.NB:-1:1 if model.parent(i) ~= 0 IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}'*IC{i}*Xup{i}; end end H = zeros(model.NB); for i = 1:model.NB fh = IC{i} * S{i}; H(i,i) = S{i}' * fh; j = i; while model.parent(j) > 0 fh = Xup{j}' * fh; j = model.parent(j); H(i,j) = S{j}' * fh; H(j,i) = H(i,j); end end