double sKinematics::CalAngle(MathLib::Vector3 vBase, MathLib::Vector3 vMeasure)
{
double theta;
MathLib::Vector3 vOrtho;
MathLib::Vector3 vDir;
theta = acos( vBase.Dot(vMeasure) / (vBase.Norm()*vMeasure.Norm()) );
vOrtho = vBase.Cross(vMeasure);
vOrtho.Normalize();
vDir = vOrtho.Cross(vBase);
if( vDir.Dot(vMeasure) > 0 ) return theta;
else return -theta;
}
void HandSkinControllerThread::PredictPoseFromAngle(){
// Apply the model and rock the field
bool bdm = bDebugMode;
bDebugMode = false;
if(bDebugMode) cout << "Predicting pose from angle *********"<<endl;
// Count the number of valid contact
// Reliability measure for each fingertip
double validFTip[FINGERS_COUNT];
int validFTipCount = 0;
if(bNoMissingCheck){
// If we don't care, let's move on
for(int i=0;i<FINGERS_COUNT;i++){
validFTip[i] = 1.0;
}
validFTipCount = FINGERS_COUNT;
}else{
for(int i=0;i<FINGERS_COUNT;i++){
// Higher than min threshold?
if(mSFingerTip[i][0] >= FTIPTHRESHOLD){
validFTipCount++;
// Higher than max threshold?
if(mSFingerTip[i][0] >= FTIPTHRESHOLDMAX){
validFTip[i] = 1.0;
}else{
// Save validity between 0 and 1
validFTip[i] = (mSFingerTip[i][0]-FTIPTHRESHOLD)/(FTIPTHRESHOLDMAX-FTIPTHRESHOLD);
}
}else{
validFTip[i] = 0.0;
}
}
}
// Additional input weight matrix using the realiability measure
MathLib::Matrix inWeights(validFTipCount*3,validFTipCount*3);
inWeights.Zero();
int cnt = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
inWeights(cnt*3+0,i*3+0) = validFTip[i];
inWeights(cnt*3+1,i*3+1) = validFTip[i];
inWeights(cnt*3+2,i*3+2) = validFTip[i];
cnt++;
}
}
// Some variables
MathLib::Vector inComp(validFTipCount*3);
MathLib::Vector outComp(FINGERS_COUNT*2+1+FINGERS_COUNT*1);
MathLib::Vector inV(validFTipCount*3);
MathLib::Vector outV(FINGERS_COUNT*2+1+FINGERS_COUNT*1); outV.Zero();
MathLib::Matrix sigma(FINGERS_COUNT*2+1+FINGERS_COUNT*1,FINGERS_COUNT*2+1+FINGERS_COUNT*1);
// Output components indices of the gmm
for(int i=0;i<outComp.Size();i++)
outComp(i) = i;
// Input components indices of the gmm
cnt = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
inComp(cnt++) = outComp.Size()+i*3+0;
inComp(cnt++) = outComp.Size()+i*3+1;
inComp(cnt++) = outComp.Size()+i*3+2;
}
}
if(bDebugMode){ cout <<"Vld: "; for(int i=0;i<FINGERS_COUNT;i++) cout << validFTip[i]<<" "; cout<<endl;}
if(bDebugMode) cout <<"InC: " << inComp<<endl;
// Filling the input vector with sensor data
cnt = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
inV(cnt++) = mSFingerTipDir[i][0];
inV(cnt++) = mSFingerTipDir[i][1];
inV(cnt++) = mSFingerTipDir[i][2];
}
}
if(bDebugMode) cout <<"InV: "<< inV<<endl;
MathLib::Vector grad,gradNoExp;
MathLib::Vector inV2,inV0;
inV2 = inV;
inV0 = inV;
if(validFTipCount>0){
// We have at least one valid input
if(bGradientAscent){
// We enjoy gradient ascent
// Get the probability that input query point is in the model
double val = mGMModel.GetPx(inV2,inComp,&inWeights);
// Iterate until we're happy
int iter = 0;
int iterNoExp = 0;
while(1){
// Get the gradient of current point
val = mGMModel.GetGradX(inV2,inComp,grad,&inWeights,&gradNoExp);
// Get the membership of the current input query point,
// if higher than threshold we're done
if(val >= exp(-0.5*2.5*2.5))
break;
// Norm of the gradient
double norm = grad.Norm();
if(val<1e-12){
// Too small? use another safer version in gradNoExp
grad = gradNoExp;
norm = gradNoExp.Norm();
norm = 1.0/norm;
iterNoExp++;
if(!isnormal(norm)){
// Still a not - number... big failure... but we
// exit still...
iter = -1;
break;
}
}else{
// Big enough? Normalize
norm = 1.0/norm;
}
// Normalize
grad *= (-norm);
// Scale the gradient to a little step size and add it
// to the query point
grad.ScaleAddTo(0.01,inV2);
// Normalization of the query point (i.e. the contact normals
// should be better unit norm
int cnt2 = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
MathLib::Vector3 vn;
vn(0) = inV2(cnt2+0);
vn(1) = inV2(cnt2+1);
vn(2) = inV2(cnt2+2);
vn.Normalize();
inV2(cnt2+0) = vn(0);
inV2(cnt2+1) = vn(1);
inV2(cnt2+2) = vn(2);
cnt2 += 3;
}
}
iter++;
if(iter>300){
// Too much iteration.... again bg failure, exit gracefully
break;
}
}
if(bDebugMode) cout << " Gradient Descent: # iterations : "<<iter<< "("<<iterNoExp<<")"<<endl;
if((iter<0)||(iter>300)){
mSFingerDesCoupling.Identity();
if(bDebugMode){
cout << " Failed: Membership Value (threshold: "<<exp(-0.5*2.5*2.5)<<") -> "<<val <<endl;
cout << endl<<endl;
}
bDebugMode= bdm;
return;
}
}
}
if(bDebugMode) cout <<"Input: "<< inV<<endl;
// Finally do the regression :)
inV = inV2;
double res = mGMModel.doRegression( inV , inComp, outComp, outV, sigma,&inWeights);
if(bDebugMode) cout << "Confidence: "<<res<<endl;
if(bDebugMode) cout << "Output: "<<outV<<endl;
// Desired position
for(int i=0;i<FINGERS_COUNT;i++){
mSFingerDesPos[i][0] = outV(1+i*2+0);// / mPosFactor;
mSFingerDesPos[i][1] = outV(1+i*2+1);// / mPosFactor;
}
// Desired pressure
for(int i=0;i<FINGERS_COUNT;i++){
mSFingerDesTip[i][0] = outV(1+FINGERS_COUNT*2+i)+2.0;
// Tweaking the output sigma matrice (identiting the pressure part,
// and keeping the position part only (for the force-position control
// switch))
sigma.SetRow(0.0,1+FINGERS_COUNT*2+i);
sigma.SetColumn(0.0,1+FINGERS_COUNT*2+i);
sigma(1+FINGERS_COUNT*2+i,1+FINGERS_COUNT*2+i) = 1.0;
}
// Inverting the sigma matrix
MathLib::Matrix nsigma(sigma);
nsigma.Identity();
nsigma *= 10.0; // A bit more power in the diagonal
nsigma += sigma;
// Inverse and store it for later use
nsigma.InverseSymmetric(mSFingerDesCoupling);
// More coping fo data here and there
mJointsTargetPredPos = mJointsRest;
for(int i=mJointsFingerOffset+1;i<mJointsSize-1;i++){
mJointsTargetPredPos(i) = outV(i-mJointsFingerOffset-1);
}
bDebugMode= bdm;
}