IRDYn/get_rne_mdh.m

function tau = get_rne_mdh(robot, a1, a2, a3, a4, a5)

	z0 = [0;0;1];
	grav = robot.gravity;	% default gravity from the object
	fext = zeros(6, 1);

	% Set debug to:
	%	0 no messages
	%	1 display results of forward and backward recursions
	%	2 display print R and p*
	debug = 0;

	n = robot.n;
	if numcols(a1) == 3*n
		Q = a1(:,1:n);
		Qd = a1(:,n+1:2*n);
		Qdd = a1(:,2*n+1:3*n);
		np = numrows(Q);
		if nargin >= 3,	
			grav = a2(:);
		end
		if nargin == 4
			fext = a3;
		end
	else
		np = numrows(a1);
		Q = a1;
		Qd = a2;
		Qdd = a3;
		if numcols(a1) ~= n || numcols(Qd) ~= n || numcols(Qdd) ~= n || ...
			numrows(Qd) ~= np || numrows(Qdd) ~= np
			error('bad data');
		end
		if nargin >= 5,	
			grav = a4(:);
		end
		if nargin == 6
			fext = a5;
		end
    end
	
    if robot.issym || any([isa(Q,'sym'), isa(Qd,'sym'), isa(Qdd,'sym')])
        tau = zeros(np,n, 'sym');
    else
        tau = zeros(np,n);
    end

	for p=1:np
		q = Q(p,:).';
		qd = Qd(p,:).';
		qdd = Qdd(p,:).';
	
		Fm = [];
		Nm = [];
		pstarm = [];
		Rm = [];
		w = zeros(3,1);
		wd = zeros(3,1);
		vd = grav(:);

	%
	% init some variables, compute the link rotation matrices
	%
		for j=1:n
			link = robot.links(j);
			Tj = link.A(q(j));
            switch link.type
                case 'R'
                    D = link.d;
                case 'P'
                    D = q(j);
            end
			alpha = link.alpha;
			pm = [link.a; -D*sin(alpha); D*cos(alpha)];	% (i-1) P i
			if j == 1
				pm = t2r(robot.base) * pm;
				Tj = robot.base * Tj;
			end
			Pm(:,j) = pm;
			Rm{j} = t2r(Tj);
			if debug>1
				Rm{j}
				Pm(:,j).'
			end
		end

	%
	%  the forward recursion
	%
		for j=1:n
			link = robot.links(j);

			R = Rm{j}.';	% transpose!!
			P = Pm(:,j);
            Pc = link.r;
            
            %
            % trailing underscore means new value
            %
            switch link.type
                case 'R'
                    % revolute axis
                    w_ = R*w + z0*qd(j);
                    wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j);
                    %v = cross(w,P) + R*v;
                    vd_ = R * (cross(wd,P) + ...
                        cross(w, cross(w,P)) + vd);
                    
                case 'P'
                    % prismatic axis
                    w_ = R*w;
                    wd_ = R*wd;
                    %v = R*(z0*qd(j) + v) + cross(w,P);
                    vd_ = R*(cross(wd,P) + ...
                        cross(w, cross(w,P)) + vd ...
                        ) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j);
            end
			% update variables
			w = w_;
			wd = wd_;
			vd = vd_;

			vdC = cross(wd,Pc).' + ...
				cross(w,cross(w,Pc)).' + vd;
			F = link.m*vdC;
			N = link.I*wd + cross(w,link.I*w);
			Fm = [Fm F];
			Nm = [Nm N];
			if debug
				fprintf('w: '); fprintf('%.3f ', w)
				fprintf('\nwd: '); fprintf('%.3f ', wd)
				fprintf('\nvd: '); fprintf('%.3f ', vd)
				fprintf('\nvdbar: '); fprintf('%.3f ', vdC)
				fprintf('\n');
			end
		end

	%
	%  the backward recursion
	%

		fext = fext(:);
		f = fext(1:3);		% force/moments on end of arm
		nn = fext(4:6);

		for j=n:-1:1
			
			%
			% order of these statements is important, since both
			% nn and f are functions of previous f.
			%
			link = robot.links(j);
			
			if j == n
				R = eye(3,3);
				P = [0;0;0];
			else
				R = Rm{j+1};
				P = Pm(:,j+1);		% i/P/(i+1)
			end
			Pc = link.r;
			
			f_ = R*f + Fm(:,j);
			nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)).' + ...
				cross(P,R*f);
			
			f = f_;
			nn = nn_;

			if debug
				fprintf('f: '); fprintf('%.3f ', f)
				fprintf('\nn: '); fprintf('%.3f ', nn)
				fprintf('\n');
			end
            switch link.type
                case 'R'
                    % revolute
                    tau(p,j) = nn.'*z0 + ...
                        link.G^2 * link.Jm*qdd(j) - ...
                        friction(link, qd(j));
                case 'P'
                    % prismatic
                    tau(p,j) = f.'*z0 + ...
                        link.G^2 * link.Jm*qdd(j) - ...
                        friction(link, qd(j));
            end
		end
	end