rne.html

Robot tool box - provides many functions that are useful in robotics including such things as kinem

0071         <span class="keyword">end</span>
0072     <span class="keyword">else</span>
0073         np = <a href="numrows.html" class="code" title="function r = numrows(m)">numrows</a>(a1);
0074         Q = a1;
0075         Qd = a2;
0076         Qdd = a3;
0077         <span class="keyword">if</span> <a href="numcols.html" class="code" title="function c = numcols(m)">numcols</a>(a1) ~= n | <a href="numcols.html" class="code" title="function c = numcols(m)">numcols</a>(Qd) ~= n | <a href="numcols.html" class="code" title="function c = numcols(m)">numcols</a>(Qdd) ~= n | <span class="keyword">...</span>
0078             <a href="numrows.html" class="code" title="function r = numrows(m)">numrows</a>(Qd) ~= np | <a href="numrows.html" class="code" title="function r = numrows(m)">numrows</a>(Qdd) ~= np,
0079             error(<span class="string">'bad data'</span>);
0080         <span class="keyword">end</span>
0081         <span class="keyword">if</span> nargin &gt;= 5,    
0082             grav = a4;
0083         <span class="keyword">end</span>
0084         <span class="keyword">if</span> nargin == 6,
0085             fext = a5;
0086         <span class="keyword">end</span>
0087     <span class="keyword">end</span>
0088     
0089     tau = zeros(np,n);
0090 
0091     <span class="keyword">for</span> p=1:np,        <span class="comment">% for all points on path</span>
0092         q = Q(p,:)';
0093         qd = Qd(p,:)';
0094         qdd = Qdd(p,:)';
0095     
0096         Fm = [];
0097         Nm = [];
0098         pstarm = [];
0099         Rm = [];
0100         w = zeros(3,1);
0101         wd = zeros(3,1);
0102         v = zeros(3,1);
0103         vd = grav;
0104 
0105     <span class="comment">%</span>
0106     <span class="comment">% init some variables, compute the link rotation matrices</span>
0107     <span class="comment">%</span>
0108         <span class="keyword">for</span> j=1:n,
0109             link = robot.link{j};
0110             Tj = link(q(j));
0111             <span class="keyword">if</span> link.RP == <span class="string">'R'</span>,
0112                 D = link.D;
0113             <span class="keyword">else</span>
0114                 D = q(j);
0115             <span class="keyword">end</span>
0116             alpha = link.alpha;
0117             pstarm(:,j) = [link.A; D*sin(alpha); D*cos(alpha)];
0118             <span class="keyword">if</span> j == 1,
0119                 robot.base
0120                 <span class="comment">%pstarm(:,j) = t2r(robot.base) * pstar(:,j);</span>
0121                 Tj = robot.base * Tj;
0122             <span class="keyword">end</span>
0123             Rm{j} = <a href="t2r.html" class="code" title="function R = tr2rot(T)">t2r</a>(Tj);
0124         <span class="keyword">end</span>
0125 
0126     <span class="comment">%</span>
0127     <span class="comment">%  the forward recursion</span>
0128     <span class="comment">%</span>
0129         <span class="keyword">for</span> j=1:n,
0130             link = robot.link{j};
0131 
0132             R = Rm{j}';
0133             pstar = pstarm(:,j);
0134             r = link.r;
0135 
0136             <span class="comment">%</span>
0137             <span class="comment">% statement order is important here</span>
0138             <span class="comment">%</span>
0139             <span class="keyword">if</span> link.RP == <span class="string">'R'</span>,
0140                 <span class="comment">% revolute axis</span>
0141                 wd = R*(wd + z0*qdd(j) + <span class="keyword">...</span>
0142                     cross(w,z0*qd(j)));
0143                 w = R*(w + z0*qd(j));
0144                 <span class="comment">%v = cross(w,pstar) + R*v;</span>
0145                 vd = cross(wd,pstar) + <span class="keyword">...</span>
0146                     cross(w, cross(w,pstar)) +R*vd;
0147 
0148             <span class="keyword">else</span>
0149                 <span class="comment">% prismatic axis</span>
0150                 w = R*w;
0151                 wd = R*wd;
0152                 vd = R*(z0*qdd(j)+vd) + <span class="keyword">...</span>
0153                     cross(wd,pstar) + <span class="keyword">...</span>
0154                     2*cross(w,R*z0*qd(j)) +<span class="keyword">...</span>
0155                     cross(w, cross(w,pstar));
0156             <span class="keyword">end</span>
0157 
0158             vhat = cross(wd,r) + <span class="keyword">...</span>
0159                 cross(w,cross(w,r)) + vd;
0160             F = link.m*vhat;
0161             N = link.I*wd + cross(w,link.I*w);
0162             Fm = [Fm F];
0163             Nm = [Nm N];
0164         <span class="keyword">end</span>
0165 
0166     <span class="comment">%</span>
0167     <span class="comment">%  the backward recursion</span>
0168     <span class="comment">%</span>
0169 
0170         f = fext(1:3);        <span class="comment">% force/moments on end of arm</span>
0171         nn = fext(4:6);
0172 
0173         <span class="keyword">for</span> j=n:-1:1,
0174             link = robot.link{j};
0175             pstar = pstarm(:,j);
0176             
0177             <span class="comment">%</span>
0178             <span class="comment">% order of these statements is important, since both</span>
0179             <span class="comment">% nn and f are functions of previous f.</span>
0180             <span class="comment">%</span>
0181             <span class="keyword">if</span> j == n,
0182                 R = eye(3,3);
0183             <span class="keyword">else</span>
0184                 R = Rm{j+1};
0185             <span class="keyword">end</span>
0186             r = link.r;
0187             nn = R*(nn + cross(R'*pstar,f)) + <span class="keyword">...</span>
0188                 cross(pstar+r,Fm(:,j)) + <span class="keyword">...</span>
0189                 Nm(:,j);
0190             f = R*f + Fm(:,j);
0191             R = Rm{j};
0192             <span class="keyword">if</span> link.RP == <span class="string">'R'</span>,
0193                 <span class="comment">% revolute</span>
0194                 tau(p,j) = nn'*(R'*z0) + <span class="keyword">...</span>
0195                     link.G^2 * ( link.Jm*qdd(j) + <span class="keyword">...</span>
0196                         friction(link, qd(j)) <span class="keyword">...</span>
0197                     );
0198             <span class="keyword">else</span>
0199                 <span class="comment">% prismatic</span>
0200                 tau(p,j) = f'*(R'*z0) + <span class="keyword">...</span>
0201                     link.G^2 * ( link.Jm*qdd(j) + <span class="keyword">...</span>
0202                         friction(link, qd(j)) <span class="keyword">...</span>
0203                     );
0204             <span class="keyword">end</span>
0205         <span class="keyword">end</span>
0206     <span class="keyword">end</span></pre></div>
<hr><address>Generated on Sun 15-Feb-2009 18:09:29 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>