rne_mdh.html

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

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