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;
}
}
