Main Page | Class Hierarchy | Alphabetical List | Class List | File List | Class Members | File Members

ArticulatedAgentQuasistatic.cpp

Go to the documentation of this file.
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 homepageUsers' guide

(C) Arxia 2004-2005