![]() |
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 homepage Users' guide
(C) Arxia 2004-2005