/* Function: P7PriorifyEmissionVector()
*
* Purpose: Add prior pseudocounts to an observed
* emission count vector and renormalize.
*
* Can return the posterior mixture probabilities
* P(q | counts) if ret_mix[MAXDCHLET] is passed.
* Else, pass NULL.
*
* Args: vec - the 4 or 20-long vector of counts to modify
* pri - prior data structure
* num - pri->mnum or pri->inum; # of mixtures
* eq - pri->mq or pri->iq; prior mixture probabilities
* e - pri->i or pri->m; Dirichlet components
* ret_mix - filled with posterior mixture probabilities, or NULL
*
* Return: (void)
* The counts in vec are changed and normalized to probabilities.
*/
void
P7PriorifyEmissionVector(float *vec, struct p7prior_s *pri,
int num, float eq[MAXDCHLET], float e[MAXDCHLET][MAXABET],
float *ret_mix)
{
int x; /* counter over vec */
int q; /* counter over mixtures */
float mix[MAXDCHLET]; /* posterior distribution over mixtures */
float totc; /* total counts */
float tota; /* total alpha terms */
float xi; /* X_i term, Sjolander eq. 41 */
/* Calculate mix[], which is the posterior probability
* P(q | n) of mixture component q given the count vector n
*
* (side effect note: note that an insert vector in a PAM prior
* is passed with num = 1, bypassing pam prior code; this means
* that inserts cannot be mixture Dirichlets...)
* [SRE, 12/24/00: the above comment is cryptic! what the hell does that
* mean, inserts can't be mixtures? doesn't seem to be true. it
* may mean that in a PAM prior, you can't have a mixture for inserts,
* but I don't even understand that. The insert vectors aren't passed
* with num=1!!]
*/
mix[0] = 1.0;
if (pri->strategy == PRI_DCHLET && num > 1)
{
for (q = 0; q < num; q++)
{
mix[q] = eq[q] > 0.0 ? log(eq[q]) : -999.;
mix[q] += Logp_cvec(vec, Alphabet_size, e[q]);
}
LogNorm(mix, num); /* now mix[q] is P(component_q | n) */
}
else if (pri->strategy == PRI_PAM && num > 1)
{ /* pam prior uses aa frequencies as `P(q|n)' */
for (q = 0; q < Alphabet_size; q++)
mix[q] = vec[q];
FNorm(mix, Alphabet_size);
}
/* Convert the counts to probabilities, following Sjolander (1996)
*/
totc = FSum(vec, Alphabet_size);
for (x = 0; x < Alphabet_size; x++) {
xi = 0.0;
for (q = 0; q < num; q++) {
tota = FSum(e[q], Alphabet_size);
xi += mix[q] * (vec[x] + e[q][x]) / (totc + tota);
}
vec[x] = xi;
}
FNorm(vec, Alphabet_size);
if (ret_mix != NULL)
for (q = 0; q < num; q++)
ret_mix[q] = mix[q];
}
/* Function: P7PriorifyTransitionVector()
*
* Purpose: Add prior pseudocounts to transition vector,
* which contains three different probability vectors
* for m, d, and i.
*
* Args: t - state transitions, counts: 3 for M, 2 for I, 2 for D.
* prior - Dirichlet prior information
* tq - prior distribution over Dirichlet components.
* (overrides prior->iq[]; used for alternative
* methods of conditioning prior on structural data)
*
* Return: (void)
* t is changed, and renormalized -- comes back as
* probability vectors.
*/
void
P7PriorifyTransitionVector(float *t, struct p7prior_s *prior,
float tq[MAXDCHLET])
{
int ts;
int q;
float mix[MAXDCHLET];
float totm, totd, toti; /* total counts in three transition vecs */
float xi; /* Sjolander's X_i term */
mix[0] = 1.0; /* default is simple one component */
if ((prior->strategy == PRI_DCHLET || prior->strategy == PRI_PAM) && prior->mnum > 1)
{
for (q = 0; q < prior->tnum; q++)
{
mix[q] = tq[q] > 0.0 ? log(tq[q]) : -999.;
mix[q] += Logp_cvec(t, 3, prior->t[q]); /* 3 match */
mix[q] += Logp_cvec(t+3, 2, prior->t[q]+3); /* 2 insert */
mix[q] += Logp_cvec(t+5, 2, prior->t[q]+5); /* 2 delete */
}
LogNorm(mix, prior->tnum); /* mix[q] is now P(q | counts) */
}
/* precalc some denominators */
totm = FSum(t,3);
toti = t[TIM] + t[TII];
totd = t[TDM] + t[TDD];
for (ts = 0; ts < 7; ts++)
{
xi = 0.0;
for (q = 0; q < prior->tnum; q++)
{
switch (ts) {
case TMM: case TMI: case TMD:
xi += mix[q] * (t[ts] + prior->t[q][ts]) /
(totm + FSum(prior->t[q], 3));
break;
case TIM: case TII:
xi += mix[q] * (t[ts] + prior->t[q][ts]) /
(toti + prior->t[q][TIM] + prior->t[q][TII]);
break;
case TDM: case TDD:
xi += mix[q] * (t[ts] + prior->t[q][ts]) /
(totd + prior->t[q][TDM] + prior->t[q][TDD]);
break;
}
}
t[ts] = xi;
}
FNorm(t, 3); /* match */
FNorm(t+3, 2); /* insert */
FNorm(t+5, 2); /* delete */
}
/*! One possible formulation of the core GFO problem. Finds the contacts
forces that result in 0 object wrench and are as far as possible
from the edges of the friction cones. Assumes that at least one set
of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
contactForceOptimization(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal, Matrix * objectWrench = NULL)
{
// exact problem formulation
// unknowns: beta (contact forces)
// minimize sum * F * beta (crude measure of friction resistance abilities)
// subject to:
// Q * beta = 0 (0 resultant object wrench)
// sum_normal * beta = 1 (we are applying some contact forces)
// F * beta <= 0 (each individual forces inside friction cones)
// beta >= 0 (all forces must be positive)
// overall equality constraint:
// | Q | |beta| |0|
// | N | = |1|
//right hand of equality
Matrix right_hand = Matrix::ZEROES<Matrix>(Q.rows()+1, 1);
if(objectWrench)
{
assert(objectWrench->rows() == Q.rows());
for(int i = 0; i < Q.rows(); ++i)
right_hand.elem(i,0) = objectWrench->elem(i,0);
}
//a total of 10N of normal force
right_hand.elem(Q.rows(),0) = 1.0e7;
//left hand of equality
Matrix LeftHand(Q.rows() + 1, Q.cols());
LeftHand.copySubMatrix(0, 0, Q);
LeftHand.copySubMatrix(Q.rows(), 0, N);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows()));
//objective: sum of F
Matrix FSum(1,F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1,F.cols());
matrixMultiply(FSum, F, FObj);
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"Left Hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
F.print(fp);
fprintf(fp,"Objective:\n");
Q.print(fp);
fclose(fp);
*/
int result = LPSolver(FObj, LeftHand, right_hand, F, Matrix::ZEROES<Matrix>(F.rows(), 1),
lowerBounds, upperBounds,
beta, objVal);
return result;
}
/* Function: VerifyHMM()
*
* Purpose: Make sure the probabilities in an HMM sum to 1
* where they should.
*
* Return: 1 on success, 0 on failure.
* Prints warnings if the HMM is not OK.
*/
void
VerifyHMM(struct hmm_struc *hmm)
{
int k;
double tolerance = 0.00001;
int bad = 0;
for (k = 0; k <= hmm->M; k++)
{
if ((k > 0 && fabs(FSum(hmm->del[k].t, 3) - 1.0) > tolerance) ||
(fabs(FSum(hmm->mat[k].t, 3) - 1.0) > tolerance) ||
(fabs(FSum(hmm->ins[k].t, 3) - 1.0) > tolerance) ||
(k > 0 && fabs(FSum(hmm->mat[k].p, Alphabet_size) - 1.0) > tolerance) ||
(fabs(FSum(hmm->ins[k].p, Alphabet_size) - 1.0) > tolerance))
bad++;
}
if (bad) Die("That HMM is no good, pal. %d distributions don't sum to 1.0", bad);
}
/* Function: Plan7RenormalizeExits()
* Date: SRE, Fri Aug 14 11:22:19 1998 [St. Louis]
*
* Purpose: Renormalize just the match state transitions;
* for instance, after a Config() function has
* modified the exit distribution.
*
* Args: hmm - hmm to renormalize
*
* Returns: void
*/
void
Plan7RenormalizeExits(struct plan7_s *hmm)
{
int k;
float d;
for (k = 1; k < hmm->M; k++)
{
d = FSum(hmm->t[k], 3);
FScale(hmm->t[k], 3, 1./(d + d*hmm->end[k]));
}
}
/* Function: Plan7Renormalize()
*
* Purpose: Take an HMM in counts form, and renormalize
* all of its probability vectors. Also enforces
* Plan7 restrictions on nonexistent transitions.
*
* Args: hmm - the model to renormalize.
*
* Return: (void)
* hmm is changed.
*/
void
Plan7Renormalize(struct plan7_s *hmm)
{
int k; /* counter for model position */
int st; /* counter for special states */
float d; /* denominator */
/* match emissions */
for (k = 1; k <= hmm->M; k++)
FNorm(hmm->mat[k], Alphabet_size);
/* insert emissions */
for (k = 1; k < hmm->M; k++)
FNorm(hmm->ins[k], Alphabet_size);
/* begin transitions */
d = FSum(hmm->begin+1, hmm->M) + hmm->tbd1;
FScale(hmm->begin+1, hmm->M, 1./d);
hmm->tbd1 /= d;
/* main model transitions */
for (k = 1; k < hmm->M; k++)
{
d = FSum(hmm->t[k], 3) + hmm->end[k];
FScale(hmm->t[k], 3, 1./d);
hmm->end[k] /= d;
FNorm(hmm->t[k]+3, 2); /* insert */
FNorm(hmm->t[k]+5, 2); /* delete */
}
/* null model emissions */
FNorm(hmm->null, Alphabet_size);
/* special transitions */
for (st = 0; st < 4; st++)
FNorm(hmm->xt[st], 2);
/* enforce nonexistent transitions */
/* (is this necessary?) */
hmm->t[0][TDM] = hmm->t[0][TDD] = 0.0;
hmm->flags &= ~PLAN7_HASBITS; /* clear the log-odds ready flag */
hmm->flags |= PLAN7_HASPROB; /* set the probabilities OK flag */
}
/*! One possible formulation of the core GFO problem. Finds the joint
forces that result in 0 object wrench such that contact forces are as
far as possible from the edges of the friction cones. Assumes that at
least one set of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
graspForceOptimization(Matrix &JTD, Matrix &D, Matrix &F, Matrix &G,
Matrix &beta, Matrix &tau, double *objVal)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [sum] [F 0] [beta tau] (sort of as far inside the friction cone as possible, not ideal)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [G 0]* [beta tau] = 0 (0 resultant object wrench)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | G 0 | | tau | = |0|
// | 0 sum| |1|
Matrix beta_tau(beta.rows() + tau.rows(), 1);
int numJoints = tau.rows();
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + G.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e8 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 100 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e10;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + G.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
LeftHand.copySubMatrix(JTD.rows(), 0, G);
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows() + G.rows(), D.cols() + i) = 1.0;
}
//objective matrix
//matrix F padded with zeroes for tau
//will also serve as the left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//summing matrix and objective matrix
Matrix FSum(1, F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1, FO.cols());
matrixMultiply(FSum, FO, FObj);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta_tau.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
FObj.print(fp);
fclose(fp);
// assembled system:
// minimize FObj * beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
int result = LPSolver(FObj, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
