$search
00001 /* 00002 ROBOOP -- A robotics object oriented package in C++ 00003 Copyright (C) 1996-2004 Richard Gourdeau 00004 00005 This library is free software; you can redistribute it and/or modify 00006 it under the terms of the GNU Lesser General Public License as 00007 published by the Free Software Foundation; either version 2.1 of the 00008 License, or (at your option) any later version. 00009 00010 This library is distributed in the hope that it will be useful, 00011 but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 GNU Lesser General Public License for more details. 00014 00015 You should have received a copy of the GNU Lesser General Public 00016 License along with this library; if not, write to the Free Software 00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00018 00019 00020 Report problems and direct all questions to: 00021 00022 Richard Gourdeau 00023 Professeur Agrege 00024 Departement de genie electrique 00025 Ecole Polytechnique de Montreal 00026 C.P. 6079, Succ. Centre-Ville 00027 Montreal, Quebec, H3C 3A7 00028 00029 email: richard.gourdeau@polymtl.ca 00030 00031 ------------------------------------------------------------------------------- 00032 Revision_history: 00033 00034 2003/14/05: Etienne Lachance 00035 -Fix Robot::dq_torque. 00036 -Added mRobot/mRobot_min_para::dq_torque. 00037 00038 2004/07/01: Etienne Lachance 00039 -Replace vec_x_prod by CrossProduct. 00040 00041 2004/07/01: Ethan Tira-Thompson 00042 -Added support for newmat's use_namespace #define, using ROBOOP namespace 00043 00044 2004/07/02: Etienne Lachance 00045 -Added Doxygen comments. 00046 ------------------------------------------------------------------------------- 00047 */ 00048 00054 00055 static const char rcsid[] = "$Id: comp_dq.cpp,v 1.17 2004/07/06 02:16:36 gourdeau Exp $"; 00056 00057 #include "robot.h" 00058 00059 #ifdef use_namespace 00060 namespace ROBOOP { 00061 using namespace NEWMAT; 00062 #endif 00063 00064 00065 void Robot::dq_torque(const ColumnVector & q, const ColumnVector & qp, 00066 const ColumnVector & qpp, const ColumnVector & dq, 00067 ColumnVector & ltorque, ColumnVector & dtorque) 00074 { 00075 int i; 00076 Matrix Rt, temp; 00077 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension"); 00078 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension"); 00079 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension"); 00080 if(dq.Ncols() != 1 || qpp.Nrows() != dof) error("dq has wrong dimension"); 00081 ltorque = ColumnVector(dof); 00082 dtorque = ColumnVector(dof); 00083 set_q(q); 00084 00085 vp[0] = gravity; 00086 ColumnVector z0(3); 00087 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0; 00088 Matrix Q(3,3); 00089 Q = 0.0; 00090 Q(1,2) = -1.0; 00091 Q(2,1) = 1.0; 00092 00093 for(i = 1; i <= dof; i++) 00094 { 00095 Rt = links[i].R.t(); 00096 p[i] = ColumnVector(3); 00097 p[i](1) = links[i].get_a(); 00098 p[i](2) = links[i].get_d() * Rt(2,3); 00099 p[i](3) = links[i].get_d() * Rt(3,3); 00100 if(links[i].get_joint_type() != 0) 00101 { 00102 dp[i] = ColumnVector(3); 00103 dp[i](1) = 0.0; 00104 dp[i](2) = Rt(2,3); 00105 dp[i](3) = Rt(3,3); 00106 } 00107 if(links[i].get_joint_type() == 0) 00108 { 00109 w[i] = Rt*(w[i-1] + z0*qp(i)); 00110 dw[i] = Rt*(dw[i-1] - Q*w[i-1]*dq(i)); 00111 wp[i] = Rt*(wp[i-1] + z0*qpp(i) 00112 + CrossProduct(w[i-1],z0*qp(i))); 00113 dwp[i] = Rt*(dwp[i-1] 00114 + CrossProduct(dw[i-1],z0*qp(i)) 00115 - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i))) 00116 *dq(i)); 00117 vp[i] = CrossProduct(wp[i],p[i]) 00118 + CrossProduct(w[i],CrossProduct(w[i],p[i])) 00119 + Rt*(vp[i-1]); 00120 dvp[i] = CrossProduct(dwp[i],p[i]) 00121 + CrossProduct(dw[i],CrossProduct(w[i],p[i])) 00122 + CrossProduct(w[i],CrossProduct(dw[i],p[i])) 00123 + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i)); 00124 } 00125 else 00126 { 00127 w[i] = Rt*w[i-1]; 00128 dw[i] = Rt*dw[i-1]; 00129 wp[i] = Rt*wp[i-1]; 00130 dwp[i] = Rt*dwp[i-1]; 00131 vp[i] = Rt*(vp[i-1] + z0*qpp(i) 00132 + 2.0*CrossProduct(w[i],Rt*z0*qp(i))) 00133 + CrossProduct(wp[i],p[i]) 00134 + CrossProduct(w[i],CrossProduct(w[i],p[i])); 00135 dvp[i] = Rt*(dvp[i-1] 00136 + 2.0*CrossProduct(dw[i-1],z0*qp(i))) 00137 + CrossProduct(dwp[i],p[i]) 00138 + CrossProduct(dw[i],CrossProduct(w[i],p[i])) 00139 + CrossProduct(w[i],CrossProduct(dw[i],p[i])) 00140 + (CrossProduct(wp[i],dp[i]) 00141 + CrossProduct(w[i],CrossProduct(w[i],dp[i]))) 00142 *dq(i); 00143 } 00144 a[i] = CrossProduct(wp[i],links[i].r) 00145 + CrossProduct(w[i],CrossProduct(w[i],links[i].r)) 00146 + vp[i]; 00147 da[i] = CrossProduct(dwp[i],links[i].r) 00148 + CrossProduct(dw[i],CrossProduct(w[i],links[i].r)) 00149 + CrossProduct(w[i],CrossProduct(dw[i],links[i].r)) 00150 + dvp[i]; 00151 } 00152 00153 for(i = dof; i >= 1; i--) 00154 { 00155 F[i] = a[i] * links[i].m; 00156 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]); 00157 dF[i] = da[i] * links[i].m; 00158 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i]) 00159 + CrossProduct(w[i],links[i].I*dw[i]); 00160 if(i == dof) 00161 { 00162 f[i] = F[i]; 00163 df[i] = dF[i]; 00164 n[i] = CrossProduct(p[i],f[i]) 00165 + CrossProduct(links[i].r,F[i]) + N[i]; 00166 dn[i] = CrossProduct(p[i],df[i]) 00167 + CrossProduct(links[i].r,dF[i]) + dN[i]; 00168 if(links[i].get_joint_type() != 0) 00169 dn[i] += CrossProduct(dp[i],f[i])*dq(i); 00170 } 00171 else 00172 { 00173 f[i] = links[i+1].R*f[i+1] + F[i]; 00174 df[i] = links[i+1].R*df[i+1] + dF[i]; 00175 if(links[i+1].get_joint_type() == 0) 00176 df[i] += Q*links[i+1].R*f[i+1]*dq(i+1); 00177 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i]) 00178 + CrossProduct(links[i].r,F[i]) + N[i]; 00179 00180 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i]) 00181 + CrossProduct(links[i].r,dF[i]) + dN[i]; 00182 if(links[i+1].get_joint_type() == 0) 00183 dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1); 00184 else 00185 dn[i] += CrossProduct(dp[i],f[i])*dq(i); 00186 } 00187 if(links[i].get_joint_type() == 0) 00188 { 00189 temp = ((z0.t()*links[i].R)*n[i]); 00190 ltorque(i) = temp(1,1); 00191 temp = ((z0.t()*links[i].R)*dn[i]); 00192 dtorque(i) = temp(1,1); 00193 } 00194 else 00195 { 00196 temp = ((z0.t()*links[i].R)*f[i]); 00197 ltorque(i) = temp(1,1); 00198 temp = ((z0.t()*links[i].R)*df[i]); 00199 dtorque(i) = temp(1,1); 00200 } 00201 } 00202 } 00203 00204 00205 void mRobot::dq_torque(const ColumnVector & q, const ColumnVector & qp, 00206 const ColumnVector & qpp, const ColumnVector & dq, 00207 ColumnVector & ltorque, ColumnVector & dtorque) 00214 { 00215 int i; 00216 Matrix Rt, temp; 00217 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension"); 00218 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension"); 00219 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension"); 00220 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension"); 00221 ltorque = ColumnVector(dof); 00222 dtorque = ColumnVector(dof); 00223 set_q(q); 00224 00225 vp[0] = gravity; 00226 ColumnVector z0(3); 00227 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0; 00228 Matrix Q(3,3); 00229 Q = 0.0; 00230 Q(1,2) = -1.0; 00231 Q(2,1) = 1.0; 00232 for(i = 1; i <= dof; i++) 00233 { 00234 Rt = links[i].R.t(); 00235 p[i] = links[i].p; 00236 if(links[i].get_joint_type() != 0) 00237 { 00238 dp[i] = ColumnVector(3); 00239 dp[i](1) = 0.0; 00240 dp[i](2) = Rt(2,3); 00241 dp[i](3) = Rt(3,3); 00242 } 00243 if(links[i].get_joint_type() == 0) 00244 { 00245 w[i] = Rt*w[i-1] + z0*qp(i); 00246 dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i); 00247 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i)) 00248 + z0*qpp(i); 00249 dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i)) 00250 - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i); 00251 vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) 00252 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) 00253 + vp[i-1]); 00254 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i]) 00255 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i])) 00256 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]) 00257 - Q*Rt*(CrossProduct(wp[i-1],p[i]) 00258 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i); 00259 } 00260 else 00261 { 00262 w[i] = Rt*w[i-1]; 00263 dw[i] = Rt*dw[i-1]; 00264 wp[i] = Rt*wp[i-1]; 00265 dwp[i] = Rt*dwp[i-1]; 00266 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]) 00267 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))) 00268 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i)); 00269 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i]) 00270 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i])) 00271 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1] 00272 + (CrossProduct(wp[i-1],dp[i]) 00273 + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i)) 00274 + 2*CrossProduct(dw[i],z0*qp(i)); 00275 } 00276 a[i] = CrossProduct(wp[i],links[i].r) 00277 + CrossProduct(w[i],CrossProduct(w[i],links[i].r)) 00278 + vp[i]; 00279 da[i] = CrossProduct(dwp[i],links[i].r) 00280 + CrossProduct(dw[i],CrossProduct(w[i],links[i].r)) 00281 + CrossProduct(w[i],CrossProduct(dw[i],links[i].r)) 00282 + dvp[i]; 00283 } 00284 00285 for(i = dof; i >= 1; i--) { 00286 F[i] = a[i] * links[i].m; 00287 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]); 00288 dF[i] = da[i] * links[i].m; 00289 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i]) 00290 + CrossProduct(w[i],links[i].I*dw[i]); 00291 00292 if(i == dof) 00293 { 00294 f[i] = F[i]; 00295 df[i] = dF[i]; 00296 n[i] = CrossProduct(links[i].r,F[i]) + N[i]; 00297 dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]); 00298 } 00299 else 00300 { 00301 f[i] = links[i+1].R*f[i+1] + F[i]; 00302 df[i] = links[i+1].R*df[i+1] + dF[i]; 00303 if(links[i+1].get_joint_type() == 0) 00304 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1); 00305 00306 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) 00307 + CrossProduct(links[i].r,F[i]) + N[i]; 00308 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1]) 00309 + CrossProduct(links[i].r,dF[i]) + dN[i]; 00310 if(links[i+1].get_joint_type() == 0) 00311 dn[i] += (links[i+1].R*Q*n[i+1] + 00312 CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1); 00313 else 00314 dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1); 00315 } 00316 00317 if(links[i].get_joint_type() == 0) 00318 { 00319 temp = z0.t()*n[i]; 00320 ltorque(i) = temp(1,1); 00321 temp = z0.t()*dn[i]; 00322 dtorque(i) = temp(1,1); 00323 } 00324 else 00325 { 00326 temp = z0.t()*f[i]; 00327 ltorque(i) = temp(1,1); 00328 temp = z0.t()*df[i]; 00329 dtorque(i) = temp(1,1); 00330 } 00331 } 00332 } 00333 00334 00335 void mRobot_min_para::dq_torque(const ColumnVector & q, const ColumnVector & qp, 00336 const ColumnVector & qpp, const ColumnVector & dq, 00337 ColumnVector & ltorque, ColumnVector & dtorque) 00344 { 00345 int i; 00346 Matrix Rt, temp; 00347 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension"); 00348 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension"); 00349 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension"); 00350 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension"); 00351 ltorque = ColumnVector(dof); 00352 dtorque = ColumnVector(dof); 00353 set_q(q); 00354 00355 vp[0] = gravity; 00356 ColumnVector z0(3); 00357 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0; 00358 Matrix Q(3,3); 00359 Q = 0.0; 00360 Q(1,2) = -1.0; 00361 Q(2,1) = 1.0; 00362 for(i = 1; i <= dof; i++) 00363 { 00364 Rt = links[i].R.t(); 00365 p[i] = links[i].p; 00366 if(links[i].get_joint_type() != 0) 00367 { 00368 dp[i] = ColumnVector(3); 00369 dp[i](1) = 0.0; 00370 dp[i](2) = Rt(2,3); 00371 dp[i](3) = Rt(3,3); 00372 } 00373 if(links[i].get_joint_type() == 0) 00374 { 00375 w[i] = Rt*w[i-1] + z0*qp(i); 00376 dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i); 00377 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i)) 00378 + z0*qpp(i); 00379 dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i)) 00380 - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i); 00381 vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) 00382 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) 00383 + vp[i-1]); 00384 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i]) 00385 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i])) 00386 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]) 00387 - Q*Rt*(CrossProduct(wp[i-1],p[i]) 00388 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i); 00389 } 00390 else 00391 { 00392 w[i] = Rt*w[i-1]; 00393 dw[i] = Rt*dw[i-1]; 00394 wp[i] = Rt*wp[i-1]; 00395 dwp[i] = Rt*dwp[i-1]; 00396 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]) 00397 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))) 00398 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i)); 00399 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i]) 00400 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i])) 00401 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1] 00402 + (CrossProduct(wp[i-1],dp[i]) 00403 + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i)); 00404 } 00405 } 00406 00407 for(i = dof; i >= 1; i--) { 00408 F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) + 00409 CrossProduct(w[i], CrossProduct(w[i], links[i].mc)); 00410 dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc) 00411 + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc)) 00412 + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc)); 00413 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) 00414 - CrossProduct(vp[i], links[i].mc); 00415 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i]) 00416 + CrossProduct(w[i],links[i].I*dw[i]) 00417 - CrossProduct(dvp[i],links[i].mc); 00418 00419 if(i == dof) 00420 { 00421 f[i] = F[i]; 00422 df[i] = dF[i]; 00423 n[i] = N[i]; 00424 dn[i] = dN[i]; 00425 } 00426 else 00427 { 00428 f[i] = links[i+1].R*f[i+1] + F[i]; 00429 df[i] = links[i+1].R*df[i+1] + dF[i]; 00430 if(links[i+1].get_joint_type() == 0) 00431 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1); 00432 00433 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) 00434 + N[i]; 00435 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1]) 00436 + dN[i]; 00437 if(links[i+1].get_joint_type() == 0) 00438 dn[i] += (links[i+1].R*Q*n[i+1] + 00439 CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1); 00440 else 00441 dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1); 00442 } 00443 00444 if(links[i].get_joint_type() == 0) 00445 { 00446 temp = z0.t()*n[i]; 00447 ltorque(i) = temp(1,1); 00448 temp = z0.t()*dn[i]; 00449 dtorque(i) = temp(1,1); 00450 } 00451 else 00452 { 00453 temp = z0.t()*f[i]; 00454 ltorque(i) = temp(1,1); 00455 temp = z0.t()*df[i]; 00456 dtorque(i) = temp(1,1); 00457 } 00458 } 00459 } 00460 00461 #ifdef use_namespace 00462 } 00463 #endif