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

ArticulatedLink.cpp

Go to the documentation of this file.
00001 // ArticulatedLink.cpp: implementation of the ArticulatedLink class.
00002 //
00004 
00005 #include "ArticulatedLink.h"
00006 #include "ArticulatedLimb.h"
00007 #include "Simulator.h"
00008 #include "Integrator.h"
00009 
00011 // Construction/Destruction
00013 
00014 ArticulatedLink::ArticulatedLink(const std::string &label) :
00015     ArticulatedComponent(label){
00016    parentLink=NULL;
00017    theta=0.0;
00018    thetaD=0.0;
00019    thetaDD=0.0;
00020    deltaThetaMax=90*degrees;
00021    theta0=0.0;
00022    theta0Min=-80.0*degrees;
00023    theta0Max=80.0*degrees;
00024    k=DEFAULT_K;
00025    eta=DEFAULT_ETA;
00026    torqueContact=NULL;
00027    motorTorque=0;
00028 }
00029 
00030 ArticulatedLink::~ArticulatedLink(){
00031 }
00032 
00033 void ArticulatedLink::integrate(const Integrator &integrator){ 
00034     thetaOld = theta;
00035     integrator.integrate(theta, thetaD);
00036 }
00037 
00038 void ArticulatedLink::normalizeTheta(){
00039    while(theta-theta0>M_PI)
00040       theta-=2*M_PI;
00041    while(theta-theta0<-M_PI)
00042       theta+=2*M_PI;
00043 }
00044 
00045 void ArticulatedLink::computeMotorTorque(){
00046    if(hasElasticTorque){
00047       //normalizeTheta();
00048       motorTorque=-k*(theta-theta0);//-eta*thetaD;
00049    }
00050 }
00051 
00052 
00053 void ArticulatedLink::computeSinCos(){
00054    sinTheta=sin(theta);
00055    cosTheta=cos(theta);
00056    sin2=sinTheta*sinTheta;
00057    cos2=cosTheta*cosTheta;
00058    sinCos=sinTheta*cosTheta;
00059 }
00060 
00061 void ArticulatedLink::setParentLink(ArticulatedLink* parentLink){
00062    this->parentLink=parentLink;
00063    if(parentLink!=NULL){
00064       parentLink->childLinks.push_back(this);
00065    }
00066 }
00067 
00068 
00069 void ArticulatedLink::computeForceQuasistatic(GlobalContactInfoVector* globalContacts){
00070    Vector2 vt2;
00071    LinkContactInfoPVector::iterator Ki, Kend;
00072    LinkContactInfoVector::iterator Qi, Qend;
00073 
00074    force.setToZero();
00075    Ki=K.begin();
00076    Kend=K.end();
00077 
00078    for (Qi=Q.begin(), Qend=Q.end(); Qi!=Qend; Qi++){
00079       Vector3& vt=Qi->v;
00080       vt2.x=IStar.getElement(1,2)*vt.x+IStar.getElement(2,2)*vt.y+
00081                      IStar.getElement(2,3)*vt.z;
00082       vt2.y=IStar.getElement(1,3)*vt.x+IStar.getElement(2,3)*vt.y+
00083                      IStar.getElement(3,3)*vt.z;
00084       //advance K until K.alpha>=alpha
00085       while(Ki!=Kend && (*Ki)->alpha<Qi->alpha)
00086          Ki++;
00087       if(Ki!=Kend && (*Ki)->alpha==Qi->alpha){
00088          vt2.x+=(*Ki)->v.y;
00089          vt2.y+=(*Ki)->v.z;
00090       }
00091       vt2*=(*globalContacts)[Qi->alpha].force;
00092       force+=vt2;
00093    }
00094    force.rotate(alpha);
00095 }
00096 
00097 
00098 void ArticulatedLink::detectTorqueContact(GlobalContactInfoVector* contacts){
00099    if(theta>theta0Max || theta<theta0Min){
00100         int sigma = (theta < theta0Min) ? 1 : -1;
00101       torqueContact=new ContactInfo(this, sigma);  
00102       contacts->push_back(GlobalContactInfo(torqueContact, NULL, (real)0.0));
00103    }  
00104 }
00105 
00106 void ArticulatedLink::draw(GUI *gui){
00107    //draws the composing object
00108    ComposedPhysicalObject::draw(gui);
00109    //draws itself
00110    
00111    real cosAlpha=cos(alpha);
00112    real sinAlpha=sin(alpha);
00113    
00114    //the link point of reference
00115    gui->setPenColor(outlineColor);
00116    gui->drawCircle(r.x, r.y, 0.02f);
00117 
00118    //the link itself
00119    //gui->drawLine(r.x, r.y, r.x+l*cosAlpha, r.y+l*sinAlpha);
00120    
00121    /*
00122    //a line at the target angle, theta0
00123    p->dc->SetPen(*wxMEDIUM_GREY_PEN);
00124    p->dc->DrawLine(
00125       p->panX+p->zoomX*(r.x),
00126       p->panY+p->zoomY*(r.y), 
00127       p->panX+p->zoomX*(r.x+l*cos(alpha+theta0-theta)),
00128       p->panY+p->zoomY*(r.y+l*sin(alpha+theta0-theta)) );
00129    */
00130 
00131    /*
00132    real ll; 
00133    ll=l;
00134    if(ll<0.1) ll=0.1;
00135    //illustrate torque contacts
00136    if(torqueContact!=NULL){
00137       //the link itself
00138       //gui->setPenColor(GUI::GREEN);
00139       //gui->drawLine(r.x, r.y, r.x+ll*cosAlpha, r.y+ll*sinAlpha);
00140       //if contact, draw min and max theta
00141       gui->setPenColor(GUI::RED);
00142       if(torqueContact->sigma<0){
00143          gui->drawLine(r.x, r.y, r.x+ll*cos(alpha+theta0Max-theta),
00144                r.y+ll*sin(alpha+theta0Max-theta));
00145          gui->drawTorque(r,alpha+theta0Max-theta,torqueContact->force*torqueContact->sigma);
00146       } else {
00147          gui->drawLine(r.x, r.y, r.x+ll*cos(alpha+theta0Min-theta),
00148                r.y+ll*sin(alpha+theta0Min-theta) );
00149          gui->drawTorque(r,alpha+theta0Min-theta,torqueContact->force*torqueContact->sigma);
00150       }  
00151       
00152    }
00153    */
00154    
00155    /*
00156    //the total force
00157    gui->setPenColor(Color("red"));
00158    Vector2 v;
00159    v=s;
00160    v.rotate(alpha);
00161    v+=r;
00162    gui->drawForce(v,force);
00163    */
00164    
00165 }

Thyrix homepageUsers' guide

(C) Arxia 2004-2005