![]() |
00001 00002 /* 00003 00004 How the agent is built 00005 TODO: This comment should be updated! 00006 00007 - the agent has a circular body; radius and position are initialized, other properties 00008 are set to 0 by default; mass and inertia are set based on default density; 00009 - two limbs are defined; for each limb is defined the angle theta and the distance l of the 00010 position of first joint, relative to the body; 00011 - we add links to the limbs: 00012 - we create the link; by default, theta0Min is -90 degrees, theta0Max is 90 degrees, 00013 everything else is set to 0 on link creation; 00014 - we set theta0, theta as theta0, l; 00015 - we create an object and set its dimensions; mass and inertia are set based on default 00016 density, other properties are 0; 00017 - we add the object to the link, by setting its position and angle relative to the link 00018 - we add the link to the limb; this also computes the total mass, center of mass and 00019 inertia of the link, relative to the link point of reference; and computes IStar0; IStar 00020 is set to IStar0. 00021 - at this moment, the absolute positions, velocities etc. of the link and objects is 0; we 00022 call forwardKinematics() to set these properties based on the absolute position of the 00023 body, and the theta angles, thetaD's etc. of the links 00024 00025 00026 */ 00027 00028 #include "ArticulatedAgentBase.h" 00029 #include "Simulator.h" 00030 #include "GUI.h" 00031 #include "purgeContainer.h" 00032 00034 // Construction/Destruction 00036 00037 00038 ArticulatedAgentBase::ArticulatedAgentBase(std::string label) : 00039 ArticulatedComponent(label) 00040 { 00041 nContacts=0; 00042 } 00043 00044 00045 ArticulatedAgentBase::~ArticulatedAgentBase(){ 00046 deleteLimbs(); 00047 } 00048 00049 00050 ArticulatedLimb* ArticulatedAgentBase::addLimb(){ 00051 ArticulatedLimb* limb = new ArticulatedLimb(0, 0); 00052 limbs.push_back(limb); 00053 00054 int i=limbs.size(); 00055 char s[10]; 00056 sprintf(s,"%d",i); 00057 limb->label=label+": limb "+s; 00058 00059 return limb; 00060 } 00061 00062 ArticulatedLimb* ArticulatedAgentBase::addLimb(real l, real theta){ 00063 ArticulatedLimb* limb; 00064 limb=addLimb(); 00065 limb->setProperties(l, theta); 00066 return limb; 00067 } 00068 00069 void ArticulatedAgentBase::registerPrimitives(Simulator* simulator){ 00070 //register body components 00071 ComposedPhysicalObject::registerPrimitives(simulator); 00072 for(ArticulatedLimbPVector::iterator limbsI = limbs.begin(), 00073 limbsEnd = limbs.end(); limbsI != limbsEnd; ++limbsI){ 00074 ArticulatedLimb* limb = *limbsI; 00075 for(ArticulatedLinkPVector::iterator linksI = limb->links.begin(), 00076 linksEnd = limb->links.end(); linksI != linksEnd; ++linksI){ 00077 //register links components 00078 (*linksI)->registerPrimitives(simulator); 00079 } 00080 } 00081 } 00082 00083 void ArticulatedAgentBase::solveSystem(SymmetricMatrix3& m, const Vector3& c, Vector3& x){ 00084 real determinant; 00085 determinant=m.getDeterminant(); 00086 m.mirror(); 00087 solveSystem(m, determinant, c, x); 00088 } 00089 00090 void ArticulatedAgentBase::solveSystem(const SymmetricMatrix3& m, const real determinant, 00091 const Vector3& c, Vector3& x){ 00092 /* 00093 m is a symmetric matrix. When we change a column, it may not remain symmetrical. mm is not symmetrical, 00094 and we should complete the lower half of m ("mirror") before assigning it to mm. 00095 */ 00096 Matrix3 mm; 00097 assert(determinant!=0.0); 00098 for(int i=1; i<=3; i++){ 00099 mm=m; 00100 mm.setColumn(i,c); 00101 x.setElement(i,mm.getDeterminant()/determinant); 00102 } 00103 } 00104 00105 bool ArticulatedAgentBase::detectContacts(PhysicalObject* object, GlobalContactInfoVector* contacts){ 00106 bool isContact=false; 00107 isContact = isContact || ComposedPhysicalObject::detectContacts(object, contacts); 00108 for(ArticulatedLimbPVector::iterator limbsI = limbs.begin(), 00109 limbsEnd = limbs.end(); limbsI != limbsEnd; ++limbsI){ 00110 ArticulatedLimb* limb = *limbsI; 00111 for(ArticulatedLinkPVector::iterator linksI = limb->links.begin(), 00112 linksEnd = limb->links.end(); linksI != linksEnd; ++linksI){ 00113 isContact = (*linksI)->detectContacts(object, contacts) || isContact; 00114 } 00115 } 00116 return isContact; 00117 } 00118 00119 void ArticulatedAgentBase::detectInternalContacts(GlobalContactInfoVector* contacts){ 00120 ArticulatedLimb* limb1; 00121 ArticulatedLimb* limb2; 00122 ArticulatedLimbPVector::iterator limbsI1, limbsI2, limbsEnd; 00123 ArticulatedLinkPVector::iterator linksI1, linksEnd1, linksI2, linksEnd2; 00124 00125 for(limbsI1=limbs.begin(), limbsEnd=limbs.end(); limbsI1!=limbsEnd; limbsI1++){ 00126 limb1=*limbsI1; 00127 for (linksI1=limb1->links.begin(), linksEnd1=limb1->links.end(); linksI1!=linksEnd1; linksI1++){ 00128 /* Link-body contact; we don't have contact between the body and the first link. */ 00129 if(linksI1!=limb1->links.begin()) { 00130 ComposedPhysicalObject::detectContacts(*linksI1, contacts); 00131 } 00132 // Torque contact 00133 (*linksI1)->detectTorqueContact(contacts); 00134 // Link-link contact with other limbs 00135 limbsI2=limbsI1; 00136 limbsI2++; 00137 if(limbsI2!=limbsEnd){ 00138 for( ; limbsI2!=limbsEnd; limbsI2++){ 00139 limb2=*limbsI2; 00140 for (linksI2=limb2->links.begin(), linksEnd2=limb2->links.end(); linksI2!=linksEnd2; linksI2++){ 00141 (*linksI2)->detectContacts(*linksI1, contacts); 00142 } 00143 } 00144 } 00145 /* Link-link contact on the same limb; we don't have contacts between succesive links. 00146 Assumes that the parent link appears earlier in the link list. */ 00147 linksI2=linksI1; 00148 linksI2++; 00149 if(linksI2!=linksEnd1){ 00150 for (; linksI2!=linksEnd1; linksI2++){ 00151 if((*linksI1)!=(*linksI2)->parentLink) 00152 (*linksI1)->detectContacts(*linksI2, contacts); 00153 } 00154 } 00155 } 00156 } 00157 } 00158 00159 bool ArticulatedAgentBase::detectMouseContact(const Vector2& rMouse, Vector2& p, PhysicalObject*& object) { 00160 bool isContact=false; 00161 isContact = ComposedPhysicalObject::detectMouseContact(rMouse, p, object); 00162 for(ArticulatedLimbPVector::iterator limbsI = limbs.begin(), 00163 limbsEnd = limbs.end(); limbsI != limbsEnd && !isContact; ++limbsI){ 00164 ArticulatedLimb* limb = *limbsI; 00165 for(ArticulatedLinkPVector::iterator linksI = limb->links.begin(), 00166 linksEnd = limb->links.end(); linksI != linksEnd && !isContact; ++linksI){ 00167 isContact = (*linksI)->detectMouseContact(rMouse, p, object); 00168 } 00169 } 00170 return isContact; 00171 } 00172 00173 void ArticulatedAgentBase::deleteContacts(){ 00174 ArticulatedLimb* limb; 00175 ArticulatedLimbPVector::iterator limbsI, limbsEnd; 00176 ArticulatedLinkPVector::iterator linksI, linksEnd; 00177 ComposedPhysicalObject::deleteContacts(); 00178 for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){ 00179 limb=*limbsI; 00180 for(linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){ 00181 (*linksI)->deleteContacts(); 00182 } 00183 } 00184 } 00185 00186 void ArticulatedAgentBase::fillContactMatrix(ContactSolver* contactSolver, ContactInfo* contactInfo){ 00187 /* 00188 contactInfo is the info of contact alpha relative to the current object 00189 Assumes v_0, w_0 (the velocity without the contribution of the contacts) are 00190 already computed as v, omega. 00191 */ 00192 Vector2 vt1, vt2; 00193 for(LinkContactInfoVector::iterator Qi=Q.begin(), Qend=Q.end(); Qi!=Qend; ++Qi){ 00194 vt1.setXY(Qi->v.y,Qi->v.z); 00195 vt1.rotate(alpha); 00196 vt2=contactInfo->p; 00197 vt2.cross(Qi->v.x); 00198 vt1+=vt2; 00199 contactSolver->contactMatrix[contactInfo->alpha][Qi->alpha]+= 00200 vt1*contactInfo->n*contactInfo->sigma; 00201 } 00202 vt1=contactInfo->p; 00203 vt1.cross(omega); 00204 vt1+=v; 00205 contactSolver->contactVector[contactInfo->alpha]+=vt1*contactInfo->n*contactInfo->sigma; 00206 } 00207 00208 void ArticulatedAgentBase::deleteLimbs(){ 00209 purgeContainer(limbs); 00210 } 00211 00212 void ArticulatedAgentBase::draw(GUI *gui){ 00213 ComposedPhysicalObject::draw(gui); 00214 for(ArticulatedLimbPVector::iterator it = limbs.begin(), end=limbs.end(); 00215 it != end; ++it) { 00216 (*it)->draw(gui); 00217 } 00218 } 00219 00220 void ArticulatedAgentBase::setOutlineColor(Color color){ 00221 ComposedPhysicalObject::setOutlineColor(color); 00222 for(ArticulatedLimbPVector::iterator it = limbs.begin(), end=limbs.end(); 00223 it != end; ++it) { 00224 (*it)->setOutlineColor(color); 00225 } 00226 } 00227 00228 void ArticulatedAgentBase::setFillColor(Color color){ 00229 ComposedPhysicalObject::setFillColor(color); 00230 for(ArticulatedLimbPVector::iterator it = limbs.begin(), end=limbs.end(); 00231 it != end; ++it) { 00232 (*it)->setFillColor(color); 00233 } 00234 }
![]() |
Thyrix homepage Users' guide
(C) Arxia 2004-2005