Vector EyePinvRefGen::getEyesCounterVelocity(const Matrix &eyesJ, const Vector &fp) { // ********** implement VOR Matrix H=imu->getH(cat(fbTorso,fbHead.subVector(0,2))); H(0,3)=fp[0]-H(0,3); H(1,3)=fp[1]-H(1,3); H(2,3)=fp[2]-H(2,3); // gyro rate [rad/s] Vector gyro=CTRL_DEG2RAD*commData->get_imu().subVector(6,8); // filter out the noise on the gyro readouts if (norm(gyro)<commData->gyro_noise_threshold) gyro=0.0; Vector vor_fprelv=(gyro[0]*cross(H,0,H,3)+ gyro[1]*cross(H,1,H,3)+ gyro[2]*cross(H,2,H,3)); // ********** implement OCR H=chainNeck->getH(); Matrix HN=eye(4,4); HN(0,3)=fp[0]-H(0,3); HN(1,3)=fp[1]-H(1,3); HN(2,3)=fp[2]-H(2,3); chainNeck->setHN(HN); Vector ocr_fprelv=chainNeck->GeoJacobian()*commData->get_v().subVector(0,2); ocr_fprelv=ocr_fprelv.subVector(0,2); chainNeck->setHN(eye(4,4)); // ********** blend the contributions return -1.0*(pinv(eyesJ)*(counterRotGain[0]*vor_fprelv+counterRotGain[1]*ocr_fprelv)); }
Vector Controller::computeNeckVelFromdxFP(const Vector &fp, const Vector &dfp) { // convert fp from root to the neck reference frame Vector fpR=fp; fpR.push_back(1.0); Vector fpE=SE3inv(chainNeck->getH())*fpR; // compute the Jacobian of the head joints alone // (by adding the new fixation point beforehand) Matrix HN=eye(4); HN(0,3)=fpE[0]; HN(1,3)=fpE[1]; HN(2,3)=fpE[2]; mutexChain.lock(); chainNeck->setHN(HN); Matrix JN=chainNeck->GeoJacobian(); chainNeck->setHN(eye(4,4)); mutexChain.unlock(); // take only the last three rows of the Jacobian // belonging to the head joints Matrix JNp=JN.submatrix(3,5,3,5); return pinv(JNp)*dfp.subVector(3,5); }
bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose) { ResourceFinder &_rf=const_cast<ResourceFinder&>(rf); if ((chain!=NULL) && _rf.isConfigured()) { string message=_rf.findFile("from").c_str(); if (!message.empty()) { message+=": aligning matrix for "+type; Bottle &parType=_rf.findGroup(type.c_str()); if (!parType.isNull()) { if (Bottle *bH=parType.find("HN").asList()) { int i=0; int j=0; Matrix HN(4,4); HN=0.0; for (int cnt=0; (cnt<bH->size()) && (cnt<HN.rows()*HN.cols()); cnt++) { HN(i,j)=bH->get(cnt).asDouble(); if (++j>=HN.cols()) { i++; j=0; } } // enforce the homogeneous property HN(3,0)=HN(3,1)=HN(3,2)=0.0; HN(3,3)=1.0; chain->setHN(HN); if (verbose) { fprintf(stdout,"%s found:",message.c_str()); fprintf(stdout,"%s",HN.toString(3,3).c_str()); } return true; } } } else { message=_rf.find("from").asString().c_str(); message+=": aligning matrix for "+type; } if (verbose) fprintf(stdout,"%s not found!",message.c_str()); } return false; }
void System::probeInterfaces( void ) { // probe interfaces int sockfd; // get list of all interfaces struct ifreq ifrs; InterfaceInfo * IFI; // flag all as 'down' for( QDictIterator<InterfaceInfo> it( ProbedInterfaces ); it.current(); ++it ) { it.current()->IsUp = 0; } sockfd = socket(PF_INET, SOCK_DGRAM, 0); if(sockfd == -1) { odebug << "Cannot open INET socket " << errno << " " << strerror( errno ) << oendl; return; } // read interfaces from /proc/dev/net // SIOCGIFCONF does not return ALL interfaces ???!? ProcDevNet = new QFile(PROCNETDEV); if( ! ProcDevNet->open(IO_ReadOnly) ) { odebug << "Cannot open " << PROCNETDEV << " " << errno << " " << strerror( errno ) << oendl; delete ProcDevNet; ProcDevNet =0; ::close( sockfd ); return; } QString line; QString NicName; QTextStream procTs(ProcDevNet); int loc = -1; procTs.readLine(); // eat a line procTs.readLine(); // eat a line while((line = procTs.readLine().simplifyWhiteSpace()) != QString::null) { if((loc = line.find(":")) == -1) { continue; } NicName = line.left(loc); // set name for ioctl strncpy( ifrs.ifr_name, NicName.latin1(), IFNAMSIZ - 1 ); if ( ! ( IFI = ProbedInterfaces.find( NicName ) ) ) { // new nic Log(("New NIC found : %s\n", NicName.latin1())); IFI = new InterfaceInfo; IFI->Name = line.left(loc); IFI->Collection = 0; ProbedInterfaces.insert( IFI->Name, IFI ); // get dynamic info if( ioctl(sockfd, SIOCGIFFLAGS, &ifrs) >= 0 ) { IFI->IsPointToPoint = ((ifrs.ifr_flags & IFF_POINTOPOINT) == IFF_POINTOPOINT); } else { IFI->IsPointToPoint = 0; } // settings that never change IFI->DstAddress = ""; if( IFI->IsPointToPoint ) { if( ioctl(sockfd, SIOCGIFDSTADDR, &ifrs) >= 0 ) { IFI->DstAddress = inet_ntoa(((struct sockaddr_in*)&ifrs.ifr_dstaddr)->sin_addr); } } IFI->CardType = 999999; IFI->MACAddress = ""; if( ioctl(sockfd, SIOCGIFHWADDR, &ifrs) >= 0 ) { Log(("Family for NIC %s : %d\n", IFI->Name.latin1(), ifrs.ifr_hwaddr.sa_family )); IFI->CardType = ifrs.ifr_hwaddr.sa_family; switch( ifrs.ifr_hwaddr.sa_family ) { case ARPHRD_ETHER : // regular MAC address // valid address -> convert to regular ::: format // length = 6 bytes = 12 DIGITS -> 6 : IFI->MACAddress.sprintf( "%c%c:%c%c:%c%c:%c%c:%c%c:%c%c", HN( ifrs.ifr_hwaddr.sa_data[0] ), LN( ifrs.ifr_hwaddr.sa_data[0] ), HN( ifrs.ifr_hwaddr.sa_data[1] ), LN( ifrs.ifr_hwaddr.sa_data[1] ), HN( ifrs.ifr_hwaddr.sa_data[2] ), LN( ifrs.ifr_hwaddr.sa_data[2] ), HN( ifrs.ifr_hwaddr.sa_data[3] ), LN( ifrs.ifr_hwaddr.sa_data[3] ), HN( ifrs.ifr_hwaddr.sa_data[4] ), LN( ifrs.ifr_hwaddr.sa_data[4] ), HN( ifrs.ifr_hwaddr.sa_data[5] ), LN( ifrs.ifr_hwaddr.sa_data[5] ) ); break; #ifdef ARPHRD_IEEE1394 case ARPHRD_IEEE1394 : // Firewire Eth address IFI->MACAddress.sprintf( "%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-%c%c-00-00", HN( ifrs.ifr_hwaddr.sa_data[0] ), LN( ifrs.ifr_hwaddr.sa_data[0] ), HN( ifrs.ifr_hwaddr.sa_data[1] ), LN( ifrs.ifr_hwaddr.sa_data[1] ), HN( ifrs.ifr_hwaddr.sa_data[2] ), LN( ifrs.ifr_hwaddr.sa_data[2] ), HN( ifrs.ifr_hwaddr.sa_data[3] ), LN( ifrs.ifr_hwaddr.sa_data[3] ), HN( ifrs.ifr_hwaddr.sa_data[4] ), LN( ifrs.ifr_hwaddr.sa_data[4] ), HN( ifrs.ifr_hwaddr.sa_data[5] ), LN( ifrs.ifr_hwaddr.sa_data[5] ), HN( ifrs.ifr_hwaddr.sa_data[6] ), LN( ifrs.ifr_hwaddr.sa_data[6] ), HN( ifrs.ifr_hwaddr.sa_data[7] ), LN( ifrs.ifr_hwaddr.sa_data[7] ), HN( ifrs.ifr_hwaddr.sa_data[8] ), LN( ifrs.ifr_hwaddr.sa_data[8] ), HN( ifrs.ifr_hwaddr.sa_data[9] ), LN( ifrs.ifr_hwaddr.sa_data[9] ), HN( ifrs.ifr_hwaddr.sa_data[10] ), LN( ifrs.ifr_hwaddr.sa_data[10] ), HN( ifrs.ifr_hwaddr.sa_data[11] ), LN( ifrs.ifr_hwaddr.sa_data[11] ), HN( ifrs.ifr_hwaddr.sa_data[12] ), LN( ifrs.ifr_hwaddr.sa_data[12] ), HN( ifrs.ifr_hwaddr.sa_data[13] ), LN( ifrs.ifr_hwaddr.sa_data[13] ) ); break; #endif case ARPHRD_PPP : // PPP break; case ARPHRD_IEEE80211 : // WLAN break; case ARPHRD_IRDA : // IRDA break; } } } else // else already probed before -> just update Log(("Redetected NIC %s\n", NicName.latin1())); // get dynamic info if( ioctl(sockfd, SIOCGIFFLAGS, &ifrs) >= 0 ) { IFI->IsUp = ((ifrs.ifr_flags & IFF_UP) == IFF_UP); IFI->HasMulticast = ((ifrs.ifr_flags & IFF_MULTICAST) == IFF_MULTICAST); } else { IFI->IsUp = 0; IFI->HasMulticast = 0; } if( ioctl(sockfd, SIOCGIFADDR, &ifrs) >= 0 ) { IFI->Address = inet_ntoa(((struct sockaddr_in*)&ifrs.ifr_addr)->sin_addr); } else { IFI->Address = ""; IFI->IsUp = 0; } if( ioctl(sockfd, SIOCGIFBRDADDR, &ifrs) >= 0 ) { IFI->BCastAddress = inet_ntoa(((struct sockaddr_in*)&ifrs.ifr_broadaddr)->sin_addr); } else { IFI->BCastAddress = ""; } if( ioctl(sockfd, SIOCGIFNETMASK, &ifrs) >= 0 ) { IFI->Netmask = inet_ntoa(((struct sockaddr_in*)&ifrs.ifr_netmask)->sin_addr); } else { IFI->Netmask = ""; } Log(("NIC %s UP ? %d\n", NicName.latin1(), IFI->IsUp )); } ::close( sockfd ); }
/* Run printf on an atom! The format field takes the form: * %{keyword}: Always display the field that matches "keyword" * %[keyword]: Only display the field when it's valid (or pverbose) * The possible "keywords" are: * CATEGORY P PN PV PVR PF PR SLOT * - these are all the standard portage variables (so see ebuild(5)) * pfx - the version qualifier if set (e.g. > < = !) * sfx - the version qualifier if set (e.g. *) */ static void qatom_printf(const char *format, const depend_atom *atom, int pverbose) { char bracket; const char *fmt, *p; if (!atom) { printf("(NULL:atom)"); return; } p = format; while (*p != '\0') { fmt = strchr(p, '%'); if (fmt == NULL) { printf("%s", p); return; } else if (fmt != p) printf("%.*s", (int)(fmt - p), p); bracket = fmt[1]; if (bracket == '{' || bracket == '[') { fmt += 2; p = strchr(fmt, bracket == '{' ? '}' : ']'); if (p) { size_t len = p - fmt; bool showit = (bracket == '{') || pverbose; #define HN(X) (X ? X : "<unset>") if (!strncmp("CATEGORY", fmt, len)) { if (showit || atom->CATEGORY) printf("%s", HN(atom->CATEGORY)); } else if (!strncmp("P", fmt, len)) { if (showit || atom->P) printf("%s", HN(atom->P)); } else if (!strncmp("PN", fmt, len)) { if (showit || atom->PN) printf("%s", HN(atom->PN)); } else if (!strncmp("PV", fmt, len)) { if (showit || atom->PV) printf("%s", HN(atom->PV)); } else if (!strncmp("PVR", fmt, len)) { if (showit || atom->PVR) printf("%s", HN(atom->PVR)); } else if (!strncmp("PF", fmt, len)) { printf("%s", atom->PN); if (atom->PV) printf("-%s", atom->PV); if (atom->PR_int) printf("-r%i", atom->PR_int); } else if (!strncmp("PR", fmt, len)) { if (showit || atom->PR_int) printf("r%i", atom->PR_int); } else if (!strncmp("SLOT", fmt, len)) { if (showit || atom->SLOT) printf("%s%s%s%s%s", atom->SLOT ? ":" : "<unset>", atom->SLOT ? atom->SLOT : "", atom->SUBSLOT ? "/" : "", atom->SUBSLOT ? atom->SUBSLOT : "", atom_slotdep_str[atom->slotdep]); } else if (!strncmp("REPO", fmt, len)) { if (showit || atom->REPO) printf("::%s", HN(atom->REPO)); } else if (!strncmp("pfx", fmt, len)) { if (showit || atom->pfx_op != ATOM_OP_NONE) printf("%s", atom->pfx_op == ATOM_OP_NONE ? "<unset>" : atom_op_str[atom->pfx_op]); } else if (!strncmp("sfx", fmt, len)) { if (showit || atom->sfx_op != ATOM_OP_NONE) printf("%s", atom->sfx_op == ATOM_OP_NONE ? "<unset>" : atom_op_str[atom->sfx_op]); } else printf("<BAD:%.*s>", (int)len, fmt); ++p; #undef HN } else p = fmt + 1; } else ++p; } }