rne_dh.html

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

0080         Q = a1;
0081         Qd = a2;
0082         Qdd = a3;
0083         <span class="keyword">if</span> numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | <span class="keyword">...</span>
0084             numrows(Qd) ~= np | numrows(Qdd) ~= np,
0085             error(<span class="string">'bad data'</span>);
0086         <span class="keyword">end</span>
0087         <span class="keyword">if</span> nargin &gt;= 5,    
0088             grav = a4(:);
0089         <span class="keyword">end</span>
0090         <span class="keyword">if</span> nargin == 6,
0091             fext = a5;
0092         <span class="keyword">end</span>
0093     <span class="keyword">end</span>
0094     
0095     tau = zeros(np,n);
0096 
0097     <span class="keyword">for</span> p=1:np,
0098         q = Q(p,:)';
0099         qd = Qd(p,:)';
0100         qdd = Qdd(p,:)';
0101     
0102         Fm = [];
0103         Nm = [];
0104         pstarm = [];
0105         Rm = [];
0106         w = zeros(3,1);
0107         wd = zeros(3,1);
0108         v = zeros(3,1);
0109         vd = grav(:);
0110 
0111     <span class="comment">%</span>
0112     <span class="comment">% init some variables, compute the link rotation matrices</span>
0113     <span class="comment">%</span>
0114         <span class="keyword">for</span> j=1:n,
0115             link = robot.link{j};
0116             Tj = link(q(j));
0117             <span class="keyword">if</span> link.RP == <span class="string">'R'</span>,
0118                 D = link.D;
0119             <span class="keyword">else</span>
0120                 D = q(j);
0121             <span class="keyword">end</span>
0122             alpha = link.alpha;
0123             pstar = [link.A; D*sin(alpha); D*cos(alpha)];
0124             <span class="keyword">if</span> j == 1,
0125                 pstar = t2r(robot.base) * pstar;
0126                 Tj = robot.base * Tj;
0127             <span class="keyword">end</span>
0128             pstarm(:,j) = pstar;
0129             Rm{j} = t2r(Tj);
0130             <span class="keyword">if</span> debug&gt;1,
0131                 Rm{j}
0132                 Pstarm(:,j)'
0133             <span class="keyword">end</span>
0134         <span class="keyword">end</span>
0135 
0136     <span class="comment">%</span>
0137     <span class="comment">%  the forward recursion</span>
0138     <span class="comment">%</span>
0139         <span class="keyword">for</span> j=1:n,
0140             link = robot.link{j};
0141 
0142             Rt = Rm{j}';    <span class="comment">% transpose!!</span>
0143             pstar = pstarm(:,j);
0144             r = link.r;
0145 
0146             <span class="comment">%</span>
0147             <span class="comment">% statement order is important here</span>
0148             <span class="comment">%</span>
0149             <span class="keyword">if</span> link.RP == <span class="string">'R'</span>,
0150                 <span class="comment">% revolute axis</span>
0151                 wd = Rt*(wd + z0*qdd(j) + <span class="keyword">...</span>
0152                     cross(w,z0*qd(j)));
0153                 w = Rt*(w + z0*qd(j));
0154                 <span class="comment">%v = cross(w,pstar) + Rt*v;</span>
0155                 vd = cross(wd,pstar) + <span class="keyword">...</span>
0156                     cross(w, cross(w,pstar)) +Rt*vd;
0157 
0158             <span class="keyword">else</span>
0159                 <span class="comment">% prismatic axis</span>
0160                 w = Rt*w;
0161                 wd = Rt*wd;
0162                 vd = Rt*(z0*qdd(j)+vd) + <span class="keyword">...</span>
0163                     cross(wd,pstar) + <span class="keyword">...</span>
0164                     2*cross(w,Rt*z0*qd(j)) +<span class="keyword">...</span>
0165                     cross(w, cross(w,pstar));
0166             <span class="keyword">end</span>
0167 
0168             vhat = cross(wd,r) + <span class="keyword">...</span>
0169                 cross(w,cross(w,r)) + vd;
0170             F = link.m*vhat;
0171             N = link.I*wd + cross(w,link.I*w);
0172             Fm = [Fm F];
0173             Nm = [Nm N];
0174 
0175             <span class="keyword">if</span> debug,
0176                 fprintf(<span class="string">'w: '</span>); fprintf(<span class="string">'%.3f '</span>, w)
0177                 fprintf(<span class="string">'\nwd: '</span>); fprintf(<span class="string">'%.3f '</span>, wd)
0178                 fprintf(<span class="string">'\nvd: '</span>); fprintf(<span class="string">'%.3f '</span>, vd)
0179                 fprintf(<span class="string">'\nvdbar: '</span>); fprintf(<span class="string">'%.3f '</span>, vhat)
0180                 fprintf(<span class="string">'\n'</span>);
0181             <span class="keyword">end</span>
0182         <span class="keyword">end</span>
0183 
0184     <span class="comment">%</span>
0185     <span class="comment">%  the backward recursion</span>
0186     <span class="comment">%</span>
0187 
0188         fext = fext(:);
0189         f = fext(1:3);        <span class="comment">% force/moments on end of arm</span>
0190         nn = fext(4:6);
0191 
0192         <span class="keyword">for</span> j=n:-1:1,
0193             link = robot.link{j};
0194             pstar = pstarm(:,j);
0195             
0196             <span class="comment">%</span>
0197             <span class="comment">% order of these statements is important, since both</span>
0198             <span class="comment">% nn and f are functions of previous f.</span>
0199             <span class="comment">%</span>
0200             <span class="keyword">if</span> j == n,
0201                 R = eye(3,3);
0202             <span class="keyword">else</span>
0203                 R = Rm{j+1};
0204             <span class="keyword">end</span>
0205             r = link.r;
0206             nn = R*(nn + cross(R'*pstar,f)) + <span class="keyword">...</span>
0207                 cross(pstar+r,Fm(:,j)) + <span class="keyword">...</span>
0208                 Nm(:,j);
0209             f = R*f + Fm(:,j);
0210             <span class="keyword">if</span> debug,
0211                 fprintf(<span class="string">'f: '</span>); fprintf(<span class="string">'%.3f '</span>, f)
0212                 fprintf(<span class="string">'\nn: '</span>); fprintf(<span class="string">'%.3f '</span>, nn)
0213                 fprintf(<span class="string">'\n'</span>);
0214             <span class="keyword">end</span>
0215 
0216             R = Rm{j};
0217             <span class="keyword">if</span> link.RP == <span class="string">'R'</span>,
0218                 <span class="comment">% revolute</span>
0219                 tau(p,j) = nn'*(R'*z0) + <span class="keyword">...</span>
0220                     link.G^2 * link.Jm*qdd(j) + <span class="keyword">...</span>
0221                     link.G * <a href="friction.html" class="code" title="function  tau = friction(robot, qd)">friction</a>(link, qd(j));
0222             <span class="keyword">else</span>
0223                 <span class="comment">% prismatic</span>
0224                 tau(p,j) = f'*(R'*z0) + <span class="keyword">...</span>
0225                     link.G^2 * link.Jm*qdd(j) + <span class="keyword">...</span>
0226                     link.G * <a href="friction.html" class="code" title="function  tau = friction(robot, qd)">friction</a>(link, qd(j));
0227             <span class="keyword">end</span>
0228         <span class="keyword">end</span>
0229     <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>