![]() |
00001 // ArticulatedAgentQuasistatic.cpp: implementation of the ArticulatedAgentQuasistatic class. 00002 // 00003 // 00005 00006 00007 #include "ArticulatedAgentQuasistatic.h" 00008 #include "Simulator.h" 00009 #include "ContactInfo.h" 00010 #include <algorithm> 00011 00012 00013 00015 // Construction/Destruction 00017 00018 00019 ArticulatedAgentQuasistatic::ArticulatedAgentQuasistatic(std::string label) 00020 :ArticulatedAgentBase(label){ 00021 } 00022 00023 ArticulatedAgentQuasistatic::~ArticulatedAgentQuasistatic(){ 00024 } 00025 00026 00027 void ArticulatedAgentQuasistatic::forwardKinematics(){ 00028 Vector2 rt, rc; 00029 real alphat; 00030 ArticulatedLink* link; 00031 ArticulatedLimb* limb; 00032 ArticulatedLimbPVector::iterator limbsI, limbsEnd; 00033 ArticulatedLinkPVector::iterator linksI, linksEnd; 00034 for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){ 00035 limb=*limbsI; 00036 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){ 00037 link=*linksI; 00038 assert(link); 00039 link->computeSinCos(); 00040 if(link->parentLink==NULL) { 00041 rt=r; 00042 alphat=alpha+limb->theta; 00043 //update alpha 00044 link->alpha = alphat + link->theta; 00045 //update r 00046 rc.setXY(limb->l,0); 00047 rc.rotate(alphat); 00048 rt+=rc; 00049 link->r=rt; 00050 } else { 00051 rt=link->parentLink->r; 00052 alphat=link->parentLink->alpha; 00053 //update alpha 00054 link->alpha = alphat + link->theta; 00055 //update r 00056 rc.setXY(link->parentLink->l,0); 00057 rc.rotate(alphat); 00058 rt+=rc; 00059 link->r=rt; 00060 } 00061 link->computeMemberPositions(); 00062 link->computeMotorTorque(); 00063 } 00064 } 00065 } 00066 00067 void ArticulatedAgentQuasistatic::backwardDynamics(){ 00068 ArticulatedLimb* limb; 00069 ArticulatedLink* link; 00070 ArticulatedLimbPVector::iterator limbsI, limbsEnd; 00071 ArticulatedLinkPVector::reverse_iterator linksI, linksEnd; 00072 ArticulatedLink* childLink; 00073 ArticulatedLinkPVector::iterator cli, clend; 00074 real N11,N12,N22,Na,Nb,Nc,lNa; 00075 SymmetricMatrix3 N; 00076 Vector3 betaLocal; 00077 Vector3 rt; 00078 Vector3 Kt; 00079 Vector3 n; 00080 Vector2 r2; 00081 ContactInfo* contactObject; 00082 LinkContactInfoPVector::iterator Ki1, Ki2, Kend1, Kend2; 00083 ContactInfoPVector::iterator ci, cend; 00084 int isNewContact; 00085 00086 //body properties 00087 IStar=IStar0; 00088 r2=externalForce; 00089 r2.rotate(-alpha); //the components of beta are expressed in the object frame of reference 00090 rt.setXYZ(externalTorque,r2.x,r2.y); 00091 betaExternal+=rt; 00092 beta=betaExternal; 00093 betaStar=beta; 00094 //reset external force and torque 00095 externalForce.setToZero(); 00096 externalTorque=0; 00097 /* 00098 It is simplified from: 00099 beta.setToZero(); //there is no coriolis component in the quasistaic case 00100 beta+=betaExternal; //thus beta=betaExternal 00101 betaStar=beta; 00102 Note that for classical mechanics, beta is not 0, we cannot do this simplification: 00103 beta.setXYZ(0.0,s.x,s.y); 00104 beta*=m*sqr(omega); 00105 beta thus varies in time. We will have then 00106 beta+=betaExternal; 00107 betaStar=beta; 00108 */ 00109 //delete old body K 00110 deleteK(); 00111 00112 for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; ++limbsI){ 00113 limb=*limbsI; 00114 //reverse links traversal 00115 for (linksI=limb->links.rbegin(), linksEnd=limb->links.rend(); linksI!=linksEnd; ++linksI){ 00116 link=*linksI; 00117 assert(link); 00118 00119 r2=link->externalForce; 00120 r2.rotate(-link->alpha); 00121 link->betaExternal.setXYZ(link->externalTorque,r2.x,r2.y); 00122 link->beta=link->betaExternal; 00123 link->betaStar=link->beta; 00124 //reset external force and torque 00125 link->externalForce.setToZero(); 00126 link->externalTorque=0; 00127 /* 00128 It is simplified from: 00129 link->beta.setToZero(); 00130 link->beta+=link->betaExternal; 00131 link->betaStar=link->beta; 00132 Note that for classical mechanics, beta is not 0, we cannot do this simplification: 00133 link->beta.setXYZ(0.0,link->s.x,link->s.y); 00134 link->beta*=link->m*sqr(link->omega); 00135 beta thus varies in time. We will have then 00136 link->beta+=link->betaExternal; 00137 link->betaStar=link->beta; 00138 */ 00139 //delete old link K 00140 link->deleteK(); 00141 00142 if(link->childLinks.size()){ 00143 /*it is not a tip link; 00144 if it is a tip link, IStar already set to IStar0 by constructor, 00145 and betaStar already set to beta, above */ 00146 link->IStar=link->IStar0; 00147 for(cli=link->childLinks.begin(), clend=link->childLinks.end(); cli!=clend; ++cli){ 00148 childLink=*cli; 00149 assert(childLink); 00150 //computation of N 00151 N11=-sqr(childLink->IStar.getElement(1,2))/childLink->IStar.getElement(1,1)+ 00152 childLink->IStar.getElement(2,2); 00153 N12=-childLink->IStar.getElement(1,2)*childLink->IStar.getElement(1,3)/ 00154 childLink->IStar.getElement(1,1)+ 00155 childLink->IStar.getElement(2,3); 00156 N22=-sqr(childLink->IStar.getElement(1,3))/childLink->IStar.getElement(1,1)+ 00157 childLink->IStar.getElement(3,3); 00158 //rotation of N 00159 Na=N11*childLink->sin2+N22*childLink->cos2+2*N12*childLink->sinCos; 00160 Nb=(N11-N22)*childLink->sinCos+N12*(childLink->cos2-childLink->sin2); 00161 Nc=N11*childLink->cos2+N22*childLink->sin2-2*N12*childLink->sinCos; 00162 lNa=link->l*Na; 00163 N.setElements(link->l * lNa, link->l * Nb, lNa, Nc, Nb, Na); 00164 link->IStar+=N; 00165 //compute betaStar 00166 betaLocal=childLink->betaStar; 00167 //the n_i (M_i star)/(m_i star) term 00168 //n_i 00169 n.setXYZ(childLink->IStar.getElement(1,1),childLink->IStar.getElement(1,2), 00170 childLink->IStar.getElement(1,3)); 00171 //rt=n_i * (M_i star)/(m_i star) 00172 rt=n; 00173 rt*=(childLink->motorTorque+childLink->betaStar.x)/ 00174 childLink->IStar.getElement(1,1); 00175 betaLocal-=rt; 00176 //rotate 00177 rt.z=betaLocal.y*childLink->sinTheta+betaLocal.z*childLink->cosTheta; 00178 rt.x=betaLocal.x+link->l*(rt.z); 00179 rt.y=betaLocal.y*childLink->cosTheta-betaLocal.z*childLink->sinTheta; 00180 link->betaStar+=rt; 00181 00182 //add the K's 00183 for(Ki1=childLink->K.begin(), Kend1=childLink->K.end(); 00184 Ki1!=Kend1; Ki1++){ 00185 Kt=(*Ki1)->v; 00186 rt=n; 00187 rt*=-Kt.x/childLink->IStar.getElement(1,1); 00188 Kt+=rt; 00189 //rotate 00190 rt.z=Kt.y*childLink->sinTheta+Kt.z*childLink->cosTheta; 00191 rt.x=Kt.x+link->l*(rt.z); 00192 rt.y=Kt.y*childLink->cosTheta-Kt.z*childLink->sinTheta; 00193 isNewContact=1; 00194 for(Ki2=link->K.begin(), Kend2=link->K.end(); 00195 Ki2!=Kend2 && isNewContact; Ki2++) 00196 if((*Ki1)->alpha==(*Ki2)->alpha){ 00197 isNewContact=0; 00198 (*Ki2)->v+=rt; 00199 } 00200 if(isNewContact){ 00201 link->K.push_back(new LinkContactInfo((*Ki1)->alpha, rt)); 00202 } 00203 } 00204 //add the K for torque contact 00205 if(childLink->torqueContact!=NULL){ 00206 Kt=n; 00207 Kt*=-childLink->torqueContact->sigma/childLink->IStar.getElement(1,1); 00208 //rotate 00209 rt.z=Kt.y*childLink->sinTheta+Kt.z*childLink->cosTheta; 00210 rt.x=Kt.x+link->l*(rt.z); 00211 rt.y=Kt.y*childLink->cosTheta-Kt.z*childLink->sinTheta; 00212 link->K.push_back(new LinkContactInfo(childLink->torqueContact->alpha, rt)); 00213 } 00214 } //for child links 00215 } //if it has child links 00216 //add contacts of the current link to K 00217 for(ci=link->contacts.begin(), cend=link->contacts.end(); ci!=cend; ++ci){ 00218 contactObject=*ci; 00219 r2=contactObject->n; 00220 r2.rotate(-link->alpha); 00221 rt.setXYZ(contactObject->pxn, r2.x, r2.y); 00222 rt*=contactObject->sigma; 00223 isNewContact=1; 00224 for(Ki1=link->K.begin(), Kend1=link->K.end(); 00225 Ki1!=Kend1 && isNewContact; Ki1++) 00226 if(contactObject->alpha==(*Ki1)->alpha){ 00227 isNewContact=0; 00228 (*Ki1)->v+=rt; 00229 } 00230 if(isNewContact){ 00231 link->K.push_back(new LinkContactInfo(contactObject->alpha, rt)); 00232 } 00233 } 00234 } //for each link 00235 //**** Body computations *********************************** 00236 //compute force exercited on the body by the current limb 00237 childLink=*(limb->links.begin()); //the first link of the limb 00238 assert(childLink); 00239 //computation of N 00240 N11=-sqr(childLink->IStar.getElement(1,2))/childLink->IStar.getElement(1,1)+ 00241 childLink->IStar.getElement(2,2); 00242 N12=-childLink->IStar.getElement(1,2)*childLink->IStar.getElement(1,3)/ 00243 childLink->IStar.getElement(1,1)+ 00244 childLink->IStar.getElement(2,3); 00245 N22=-sqr(childLink->IStar.getElement(1,3))/childLink->IStar.getElement(1,1)+ 00246 childLink->IStar.getElement(3,3); 00247 //rotation of N 00248 Na=N11*childLink->sin2+N22*childLink->cos2+2*N12*childLink->sinCos; 00249 Nb=(N11-N22)*childLink->sinCos+N12*(childLink->cos2-childLink->sin2); 00250 Nc=N11*childLink->cos2+N22*childLink->sin2-2*N12*childLink->sinCos; 00251 //second rotation of N 00252 N.setElements(limb->l*limb->l*Na, limb->l*(Nb*limb->cosTheta-Na*limb->sinTheta), 00253 limb->l*(Nb*limb->sinTheta+Na*limb->cosTheta), 00254 Nc*limb->cos2-2*Nb*limb->sinCos+Na*limb->sin2, 00255 (Nc-Na)*limb->sinCos+Nb*(limb->cos2-limb->sin2), 00256 Nc*limb->sin2+2*Nb*limb->sinCos+Na*limb->cos2); 00257 IStar+=N; 00258 //compute betaStar 00259 betaLocal=childLink->betaStar; 00260 //the n_i (M_i star)/(m_i star) term 00261 n.setXYZ(childLink->IStar.getElement(1,1),childLink->IStar.getElement(1,2), 00262 childLink->IStar.getElement(1,3)); 00263 rt=n; 00264 rt*=-(childLink->motorTorque+childLink->betaStar.x)/ 00265 childLink->IStar.getElement(1,1); 00266 betaLocal+=rt; 00267 //rotate 00268 rt.z=betaLocal.y*childLink->sinTheta+betaLocal.z*childLink->cosTheta; 00269 rt.x=betaLocal.x+limb->l*(rt.z); 00270 rt.y=betaLocal.y*childLink->cosTheta-betaLocal.z*childLink->sinTheta; 00271 betaLocal=rt; 00272 //second rotation 00273 rt.x=betaLocal.x; 00274 rt.y=betaLocal.y*limb->cosTheta-betaLocal.z*limb->sinTheta; 00275 rt.z=betaLocal.y*limb->sinTheta+betaLocal.z*limb->cosTheta; 00276 betaStar+=rt; 00277 00278 //compute the K of the body 00279 for(Ki1=childLink->K.begin(), Kend1=childLink->K.end(); Ki1!=Kend1; Ki1++){ 00280 Kt=(*Ki1)->v; 00281 rt=n; 00282 rt*=-Kt.x/childLink->IStar.getElement(1,1); 00283 Kt+=rt; 00284 //rotate 00285 rt.z=Kt.y*childLink->sinTheta+Kt.z*childLink->cosTheta; 00286 rt.x=Kt.x+limb->l*(rt.z); 00287 rt.y=Kt.y*childLink->cosTheta-Kt.z*childLink->sinTheta; 00288 Kt=rt; 00289 //second rotation 00290 rt.x=Kt.x; 00291 rt.y=Kt.y*limb->cosTheta-Kt.z*limb->sinTheta; 00292 rt.z=Kt.y*limb->sinTheta+Kt.z*limb->cosTheta; 00293 isNewContact=1; 00294 for(Ki2=K.begin(), Kend2=K.end(); 00295 Ki2!=Kend2 && isNewContact; Ki2++) 00296 if((*Ki1)->alpha==(*Ki2)->alpha){ 00297 isNewContact=0; 00298 (*Ki2)->v+=rt; 00299 } 00300 if(isNewContact){ 00301 K.push_back(new LinkContactInfo((*Ki1)->alpha, rt)); 00302 } 00303 } 00304 //add the K for torque contact 00305 if(childLink->torqueContact!=NULL){ 00306 Kt=n; 00307 Kt*=-childLink->torqueContact->sigma/childLink->IStar.getElement(1,1); 00308 //rotate 00309 rt.z=Kt.y*childLink->sinTheta+Kt.z*childLink->cosTheta; 00310 rt.x=Kt.x+limb->l*(rt.z); 00311 rt.y=Kt.y*childLink->cosTheta-Kt.z*childLink->sinTheta; 00312 Kt=rt; 00313 //second rotation 00314 rt.x=Kt.x; 00315 rt.y=Kt.y*limb->cosTheta-Kt.z*limb->sinTheta; 00316 rt.z=Kt.y*limb->sinTheta+Kt.z*limb->cosTheta; 00317 K.push_back(new LinkContactInfo(childLink->torqueContact->alpha, rt)); 00318 } 00319 } // for each limb 00320 00321 real determinant=IStar.getDeterminant(); 00322 IStar.mirror(); 00323 computeBodyDerivativesWithoutContacts(determinant); 00324 00325 //add body contacts to K 00326 for(ci=contacts.begin(), cend=contacts.end(); ci!=cend; ci++){ 00327 contactObject=*ci; 00328 r2=contactObject->n; 00329 r2.rotate(-alpha); 00330 rt.setXYZ(contactObject->pxn, r2.x, r2.y); 00331 rt*=contactObject->sigma; 00332 isNewContact=1; 00333 for(Ki1=K.begin(), Kend1=K.end(); Ki1!=Kend1 && isNewContact; Ki1++) 00334 if(contactObject->alpha==(*Ki1)->alpha){ 00335 isNewContact=0; 00336 (*Ki1)->v+=rt; 00337 } 00338 if(isNewContact){ 00339 K.push_back(new LinkContactInfo(contactObject->alpha, rt)); 00340 } 00341 } 00342 //increase Q size if needed 00343 nContacts=K.size(); 00344 Q.resize(nContacts); 00345 //compute the Q's 00346 int i=0; 00347 for(Ki1=K.begin(), Kend1=K.end(); Ki1!=Kend1; Ki1++){ 00348 Q[i].alpha=(*Ki1)->alpha; 00349 solveSystem(IStar, determinant, (*Ki1)->v, Q[i].v); 00350 i++; 00351 } 00352 //order the Q vector 00353 std::sort(Q.begin(), Q.end()); 00354 /* 00355 //fill the contact matrix for the body: se face la apelarea din Simulator::fillContactMatrix 00356 for(contacts.rewindIndex();contacts.notAtEnd();contacts.advanceIndex()){ 00357 fillContactMatrix(contacts.getCurrentElement()); 00358 } 00359 */ 00360 } 00361 00362 00363 void ArticulatedAgentQuasistatic::computeBodyDerivativesWithoutContacts(real determinant){ 00364 //compute the velocity of the body: requires inversion of a symmetric matrix 00365 solveSystem(IStar, determinant, betaStar, beta); 00366 omega=beta.x; 00367 vLocal.setXY(beta.y, beta.z); 00368 v=vLocal; 00369 v.rotate(alpha); 00370 } 00371 00372 void ArticulatedAgentQuasistatic::forwardAccelerations(ContactSolver* contactSolver){ 00373 ArticulatedLink* link; 00374 ArticulatedLimb* limb; 00375 ArticulatedLimbPVector::iterator limbsI, limbsEnd; 00376 ArticulatedLinkPVector::iterator linksI, linksEnd; 00377 Vector2 vLocalParent, vLocalParentRotated; 00378 real omegaParent, omegaLParent; 00379 real iq; 00380 real lParent; 00381 Vector2 vt1, vt2; 00382 ContactInfo* contactInfo; 00383 00384 unsigned i; 00385 for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){ 00386 limb=*limbsI; 00387 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){ 00388 link=*linksI; 00389 assert(link); 00390 //update the link Q number of elements 00391 link->Q.resize(nContacts); 00392 link->nContacts=nContacts; 00393 if(linksI==limb->links.begin()) { 00394 vLocalParent=vLocal; 00395 vLocalParent.rotate(-limb->theta); 00396 omegaParent=omega; 00397 lParent=limb->l; 00398 for (i = 0; i < nContacts; i++) { 00399 link->Q[i]=Q[i]; 00400 link->Q[i].v.rotate(-limb->theta); 00401 } 00402 } else { 00403 vLocalParent=link->parentLink->vLocal; 00404 omegaParent=link->parentLink->omega; 00405 lParent=link->parentLink->l; 00406 for ( i=0; i < nContacts; i++) { 00407 link->Q[i]=link->parentLink->Q[i]; 00408 } 00409 } 00410 omegaLParent=omegaParent*lParent; 00411 //rotate vLocalParent with the spatial rotation matrix; this includes the actual rotation, 00412 //and the w l term 00413 vLocalParentRotated.setXY( 00414 (omegaLParent+vLocalParent.y)*link->sinTheta+vLocalParent.x*link->cosTheta, 00415 (omegaLParent+vLocalParent.y)*link->cosTheta-vLocalParent.x*link->sinTheta); 00416 //update omega 00417 link->omega=(link->motorTorque+link->betaStar.x- 00418 ( link->IStar.getElement(1,2)*vLocalParentRotated.x+ 00419 link->IStar.getElement(1,3)*vLocalParentRotated.y))/link->IStar.getElement(1,1); 00420 //update thetaD 00421 link->thetaD=link->omega-omegaParent; 00422 //update vLocal 00423 link->vLocal=vLocalParentRotated; 00424 //update v 00425 link->v=link->vLocal; 00426 link->v.rotate(link->alpha); 00427 00428 //update Q 00429 LinkContactInfoVector::iterator Qi, Qend; 00430 for(Qi=link->Q.begin(), Qend=link->Q.end(); Qi!=Qend; Qi++){ 00431 //rotate Q 00432 Vector3& vt=Qi->v; 00433 vt.setXYZ( vt.x, 00434 (vt.x*lParent+vt.z)*link->sinTheta+vt.y*link->cosTheta, 00435 (vt.x*lParent+vt.z)*link->cosTheta-vt.y*link->sinTheta); 00436 iq=-( link->IStar.getElement(1,1)*vt.x+ 00437 link->IStar.getElement(1,2)*vt.y+ 00438 link->IStar.getElement(1,3)*vt.z); 00439 for(LinkContactInfoPVector::iterator Ki=link->K.begin(), Kend=link->K.end(); 00440 Ki!=Kend; Ki++) 00441 if((*Ki)->alpha==Qi->alpha) 00442 iq+=(*Ki)->v.x; 00443 //TODO: de optimizat ca sa nu trebuiasca ciclul K in ciclul Q, sa mearga in paralel 00444 //cum poate fi K ordonat? trebuie vazut atat cu contacte externe, cat si interne 00445 //are rost atunci ordonarea lui Q? 00446 iq/=link->IStar.getElement(1,1); 00447 //add iq to Q 00448 vt.x+=iq; 00449 //set the thetaFactor 00450 Qi->thetaFactor=iq; 00451 if(link->torqueContact!=NULL){ 00452 if(link->torqueContact->alpha==Qi->alpha){ 00453 //add component to Q 00454 vt.x+=link->torqueContact->sigma/link->IStar.getElement(1,1); 00455 //add component to thetaFactor 00456 Qi->thetaFactor+=link->torqueContact->sigma/link->IStar.getElement(1,1); 00457 //TODO: de folosit real temporar pentru a nu face impartirea de 2 ori 00458 //add component to the contact matrix 00459 contactSolver->contactMatrix[link->torqueContact->alpha][link->torqueContact->alpha]+= 00460 1.0f/link->IStar.getElement(1,1); 00461 //set the contact vector component 00462 contactSolver->contactVector[link->torqueContact->alpha]= 00463 link->torqueContact->sigma*link->thetaD; 00464 } 00465 //fill the contact matrix for the torque contact 00466 contactSolver->contactMatrix[link->torqueContact->alpha][Qi->alpha]+= 00467 link->torqueContact->sigma*iq; 00468 } 00469 } 00470 //fill the matrix for this link 00471 for(ContactInfoPVector::iterator ci=link->contacts.begin(), 00472 cend=link->contacts.end(); ci!=cend; ci++){ 00473 contactInfo=*ci; 00474 for(LinkContactInfoVector::iterator Qi=link->Q.begin(), 00475 Qend=link->Q.end(); Qi!=Qend; Qi++){ 00476 vt1.setXY(Qi->v.y,Qi->v.z); 00477 vt1.rotate(link->alpha); 00478 vt2=contactInfo->p; 00479 vt2.cross(Qi->v.x); 00480 vt1+=vt2; 00481 contactSolver->contactMatrix[contactInfo->alpha][Qi->alpha]+= 00482 vt1*contactInfo->n*contactInfo->sigma; 00483 } 00484 vt1=contactInfo->p; 00485 vt1.cross(link->omega); 00486 vt1+=link->v; 00487 contactSolver->contactVector[contactInfo->alpha]+=vt1*contactInfo->n*contactInfo->sigma; 00488 } 00489 } 00490 } 00491 } 00492 00493 00494 00495 void ArticulatedAgentQuasistatic::computeDerivatives(GlobalContactInfoVector* globalContacts){ 00496 real f; 00497 Vector2 vt; 00498 LinkContactInfoVector::iterator Qi, Qend; 00499 00500 computeBodyDerivatives(globalContacts); 00501 00502 setSensors(); 00503 00504 ArticulatedLink* link; 00505 ArticulatedLimb* limb; 00506 ArticulatedLimbPVector::iterator limbsI, limbsEnd; 00507 ArticulatedLinkPVector::iterator linksI, linksEnd; 00508 for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){ 00509 limb=*limbsI; 00510 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){ 00511 link=*linksI; 00512 assert(link); 00513 for (Qi=link->Q.begin(), Qend=link->Q.end(); Qi!=Qend; Qi++){ 00514 f=(*globalContacts)[Qi->alpha].force; 00515 /* 00516 link->omega+=link->Q[i].v.x*f; 00517 link->vLocal.x+=link->Q[i].v.y*f; 00518 link->vLocal.y+=link->Q[i].v.z*f; 00519 */ 00520 link->thetaD+=Qi->thetaFactor*f; 00521 } 00522 link->setSensors(); 00523 } 00524 } 00525 //TEST: computes the total force that act on each link 00526 //computeForces(globalContacts); 00527 } 00528 00529 void ArticulatedAgentQuasistatic::computeBodyDerivatives(GlobalContactInfoVector* globalContacts){ 00530 real f; 00531 Vector2 vt; 00532 LinkContactInfoVector::iterator Qi, Qend; 00533 //update body velocity 00534 for (Qi=Q.begin(), Qend=Q.end(); Qi!=Qend; Qi++){ 00535 f=(*globalContacts)[Qi->alpha].force; 00536 omega+=Qi->v.x*f; 00537 vt.setXY(Qi->v.y*f,Qi->v.z*f); 00538 vt.rotate(alpha); 00539 v+=vt; 00540 } 00541 } 00542 00543 void ArticulatedAgentQuasistatic::computeForces(GlobalContactInfoVector* globalContacts){ 00544 ArticulatedLink* link; 00545 ArticulatedLimb* limb; 00546 ArticulatedLimbPVector::iterator limbsI, limbsEnd; 00547 ArticulatedLinkPVector::iterator linksI, linksEnd; 00548 for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){ 00549 limb=*limbsI; 00550 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){ 00551 link=*linksI; 00552 assert(link); 00553 link->computeForceQuasistatic(globalContacts); 00554 } 00555 } 00556 } 00557 00558 void ArticulatedAgentQuasistatic::computeTotalForces(GlobalContactInfoVector* globalContacts){ 00559 00560 ArticulatedLink* link; 00561 ArticulatedLink* childLink; 00562 ArticulatedLinkPVector::iterator cli, clend; 00563 ArticulatedLimb* limb; 00564 ArticulatedLimbPVector::iterator limbsI, limbsEnd; 00565 ArticulatedLinkPVector::iterator linksI, linksEnd; 00566 00567 00568 for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){ 00569 limb=*limbsI; 00570 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){ 00571 link=*linksI; 00572 assert(link); 00573 link->computeForceQuasistatic(globalContacts); 00574 link->totalTorque=link->motorTorque; 00575 link->totalForce=link->force; 00576 for(cli=link->childLinks.begin(), clend=link->childLinks.end(); cli!=clend; cli++){ 00577 childLink=*cli; 00578 assert(childLink); 00579 link->totalForce-=childLink->force; 00580 link->totalTorque-=childLink->motorTorque+link->l*( 00581 childLink->sinTheta*childLink->forceLocal.x+ 00582 childLink->cosTheta*childLink->forceLocal.y); 00583 } 00584 /* 00585 Vector2 vt; 00586 vt.x=-link->s.y; 00587 vt.y=link->s.x; 00588 vt*=link->omega; 00589 vt+=link->vLocal; 00590 vt*=link->m; 00591 vt.rotate(link->alpha); 00592 //TEST: tests that the forces and torques computed with the Featherstone algorithm verify 00593 //the dynamic ecuations 00594 wxString string; 00595 string.Printf("%10f\t%10f\t%10f\n", 00596 link->totalForce.x-vt.x, 00597 link->totalForce.y-vt.y, 00598 link->totalTorque- 00599 (link->I*link->omega+link->m*(-link->s.y*link->vLocal.x+link->s.x*link->vLocal.y)) 00600 ); 00601 if(simulator->textControl) 00602 simulator->textControl->AppendText(string); 00603 00604 */ 00605 } 00606 /* 00607 00608 if(simulator->textControl) 00609 simulator->textControl->AppendText("\n"); 00610 00611 */ 00612 } 00613 } 00614 00615 void ArticulatedAgentQuasistatic::integrate(const Integrator &integrator) { 00616 ArticulatedLink* link; 00617 ArticulatedLimb* limb; 00618 ArticulatedLimbPVector::iterator limbsI, limbsEnd=limbs.end(); 00619 ArticulatedLinkPVector::iterator linksI, linksEnd; 00620 00621 for(limbsI=limbs.begin(); limbsI!=limbsEnd; ++limbsI){ 00622 limb=*limbsI; 00623 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; ++linksI){ 00624 link=*linksI; 00625 assert(link); 00626 link->integrate(integrator); //integrates only theta, based on thetaD 00627 } 00628 } 00629 ComposedPhysicalObject::integrate(integrator); 00630 forwardKinematics(); 00631 00632 //now the bounding box is the box of the base 00633 for(limbsI=limbs.begin(); limbsI!=limbsEnd; ++limbsI){ 00634 limb=*limbsI; 00635 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; ++linksI){ 00636 link=*linksI; 00637 boxMin.updateMin(link->boxMin); 00638 boxMax.updateMax(link->boxMax); 00639 } 00640 } 00641 } 00642 00643 void ArticulatedAgentQuasistatic::rollback(){ 00644 ArticulatedLink* link; 00645 ArticulatedLimb* limb; 00646 ArticulatedLimbPVector::iterator limbsI, limbsEnd=limbs.end(); 00647 ArticulatedLinkPVector::iterator linksI, linksEnd; 00648 00649 00650 for(limbsI=limbs.begin(); limbsI!=limbsEnd; ++limbsI){ 00651 limb=*limbsI; 00652 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; ++linksI){ 00653 link=*linksI; 00654 assert(link); 00655 link->rollback(); 00656 } 00657 } 00658 ComposedPhysicalObject::rollback(); 00659 //now the bounding box is the box of the base 00660 for(limbsI=limbs.begin(); limbsI!=limbsEnd; ++limbsI){ 00661 limb=*limbsI; 00662 for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; ++linksI){ 00663 link=*linksI; 00664 boxMin.updateMin(link->boxMin); 00665 boxMax.updateMax(link->boxMax); 00666 } 00667 } 00668 } 00669 00670 00671 00672 00673 00674 00675
![]() |
Thyrix homepage Users' guide
(C) Arxia 2004-2005