void
PFEMElement2DBubble::getdFp(Matrix& dfp) const {
Matrix gb(2,3);
getGbub(gb);
Vector fb(2);
getFbub(fb);
double invmb = getinvMbub();
getdGbt(fb*invmb, dfp);
Matrix dmb(2,6);
getdinvMbub(fb,dmb);
dfp.addMatrixTransposeProduct(-1.0, gb, dmb, -1.0);
Matrix dfb(2,6);
getdFbub(dfb);
dfp.addMatrixTransposeProduct(1.0, gb, dfb, -invmb);
}
void
PFEMElement2DBubble::getdFp(Vector& dfp) const
{
Matrix gb(2,3);
getGbub(gb);
double invmb = getinvMbub();
Vector fb(2);
getFbub(fb);
double dinvmb = getdinvMbub();
Vector dfb(2);
getdFbub(dfb);
// bubble force
dfp.resize(3);
dfp.Zero();
dfp.addMatrixTransposeVector(0.0, gb, fb, -dinvmb);
dfp.addMatrixTransposeVector(1.0, gb, dfb, -invmb);
}
double IK::solve_ik()
{
int i, j;
double current_max_condnum = -1.0;
copy_jacobian();
if(n_all_const > 0)
{
////
// check rank when HIGH_IF_POSSIBLE constraints have high priority
int n_high_const;
fMat Jhigh;
fMat wJhigh;
fVec fb_high;
fVec weight_high;
int* is_high_const = 0;
double cond_number = 1.0;
// cerr << "---" << endl;
// cerr << n_const[HIGH_PRIORITY] << " " << n_const[HIGH_IF_POSSIBLE] << " " << n_const[LOW_PRIORITY] << endl;
if(n_const[HIGH_IF_POSSIBLE] > 0)
{
is_high_const = new int [n_const[HIGH_IF_POSSIBLE]];
// initialize
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
is_high_const[i] = true;
// search
int search_phase = 0;
while(1)
{
n_high_const = n_const[HIGH_PRIORITY];
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(is_high_const[i]) n_high_const++;
}
Jhigh.resize(n_high_const, n_dof);
wJhigh.resize(n_high_const, n_dof);
fb_high.resize(n_high_const);
weight_high.resize(n_high_const);
if(n_const[HIGH_PRIORITY] > 0)
{
// set fb and J of higher priority pins
for(i=0; i<n_const[HIGH_PRIORITY]; i++)
{
fb_high(i) = fb[HIGH_PRIORITY](i);
weight_high(i) = weight[HIGH_PRIORITY](i);
for(j=0; j<n_dof; j++)
{
Jhigh(i, j) = J[HIGH_PRIORITY](i, j);
wJhigh(i, j) = Jhigh(i, j) * weight_high(i) / joint_weights(j);
}
}
}
int count = 0;
// set fb and J of medium priority pins
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(is_high_const[i])
{
fb_high(n_const[HIGH_PRIORITY]+count) = fb[HIGH_IF_POSSIBLE](i);
weight_high(n_const[HIGH_PRIORITY]+count) = weight[HIGH_IF_POSSIBLE](i);
for(j=0; j<n_dof; j++)
{
Jhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j);
wJhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j) * weight[HIGH_IF_POSSIBLE](i) / joint_weights(j);
}
count++;
}
}
// singular value decomposition
fMat U(n_high_const, n_high_const), VT(n_dof, n_dof);
fVec s;
int s_size;
if(n_high_const < n_dof) s_size = n_high_const;
else s_size = n_dof;
s.resize(s_size);
wJhigh.svd(U, s, VT);
double condnum_limit = max_condnum * 100.0;
if(s(s_size-1) > s(0)/(max_condnum*condnum_limit))
cond_number = s(0) / s(s_size-1);
else
cond_number = condnum_limit;
if(current_max_condnum < 0.0 || cond_number > current_max_condnum)
{
current_max_condnum = cond_number;
}
if(n_high_const <= n_const[HIGH_PRIORITY]) break;
// remove some constraints
if(cond_number > max_condnum)
{
int reduced = false;
for(i=n_constraints-1; i>=0; i--)
{
if(constraints[i]->enabled &&
constraints[i]->priority == HIGH_IF_POSSIBLE &&
constraints[i]->i_const >= 0 &&
constraints[i]->GetType() == HANDLE_CONSTRAINT &&
is_high_const[constraints[i]->i_const])
{
IKHandle* h = (IKHandle*)constraints[i];
if(search_phase ||
(!search_phase && h->joint->DescendantDOF() > 0))
{
for(j=0; j<constraints[i]->n_const; j++)
{
is_high_const[constraints[i]->i_const + j] = false;
}
constraints[i]->is_dropped = true;
// cerr << "r" << flush;
reduced = true;
break;
}
}
}
if(!reduced) search_phase++;
}
else break;
}
}
else
{
n_high_const = n_const[HIGH_PRIORITY];
Jhigh.resize(n_high_const, n_dof);
wJhigh.resize(n_high_const, n_dof);
fb_high.resize(n_high_const);
weight_high.resize(n_high_const);
if(n_high_const > 0)
{
Jhigh.set(J[HIGH_PRIORITY]);
fb_high.set(fb[HIGH_PRIORITY]);
weight_high.set(weight[HIGH_PRIORITY]);
}
}
#if 0
////
// adjust feedback according to the condition number
if(current_max_condnum > max_condnum)
fb_high.zero();
else
{
double k = (current_max_condnum-max_condnum)/(1.0-max_condnum);
fb_high *= k;
cerr << current_max_condnum << ", " << k << endl;
}
////
////
if(current_max_condnum < 0.0) current_max_condnum = 1.0;
#endif
int n_low_const = n_all_const - n_high_const;
int low_first = 0, count = 0;
fMat Jlow(n_low_const, n_dof);
fVec fb_low(n_low_const);
fVec weight_low(n_low_const);
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(!is_high_const[i])
{
fb_low(count) = fb[HIGH_IF_POSSIBLE](i);
weight_low(count) = weight[HIGH_IF_POSSIBLE](i);
for(j=0; j<n_dof; j++)
{
Jlow(count, j) = J[HIGH_IF_POSSIBLE](i, j);
}
count++;
}
}
low_first = count;
double* p = fb_low.data() + low_first;
double* q = fb[LOW_PRIORITY].data();
double* r = weight_low.data() + low_first;
double* s = weight[LOW_PRIORITY].data();
for(i=0; i<n_const[LOW_PRIORITY]; p++, q++, r++, s++, i++)
{
// fb_low(low_first+i) = fb[LOW_PRIORITY](i);
*p = *q;
*r = *s;
double* a = Jlow.data() + low_first + i;
int a_row = Jlow.row();
double* b = J[LOW_PRIORITY].data() + i;
int b_row = J[LOW_PRIORITY].row();
for(j=0; j<n_dof; a+=a_row, b+=b_row, j++)
{
// Jlow(low_first+i, j) = J[LOW_PRIORITY](i, j);
*a = *b;
}
}
if(is_high_const) delete[] is_high_const;
fVec jvel(n_dof); // ³û¼â1¡¦x
fVec jvel0(n_dof); // ¹ó/¡¦¾å
fVec fb_low_0(n_low_const), dfb(n_low_const), y(n_dof);
fMat Jinv(n_dof, n_high_const), W(n_dof, n_dof), JW(n_low_const, n_dof);
fVec w_error(n_low_const), w_norm(n_dof);
// weighted
double damping = 0.1;
// w_error = 1.0;
w_error.set(weight_low);
w_norm = 1.0;
// w_norm.set(joint_weights);
// cerr << "joint_weights = " << joint_weights << endl;
// cerr << "weight_high = " << weight_high << endl;
// cerr << "weight_low = " << weight_low << endl;
#ifdef MEASURE_TIME
clock_t t1 = clock();
#endif
// ¹ó/¡¦¾å¡¦€ÅæéòÉçïàïêvZ
if(n_high_const > 0)
{
for(i=0; i<n_dof; i++)
{
int a_row = wJhigh.row();
double* a = wJhigh.data() + a_row*i;
int b_row = Jhigh.row();
double* b = Jhigh.data() + b_row*i;
double* c = joint_weights.data() + i;
double* d = weight_high.data();
for(j=0; j<n_high_const; a++, b++, d++, j++)
{
// wJhigh(j, i) = Jhigh(j, i) / joint_weights(i);
*a = *b * *d / *c;
}
}
fVec w_fb_high(fb_high);
for(i=0; i<n_high_const; i++)
w_fb_high(i) *= weight_high(i);
Jinv.p_inv(wJhigh);
jvel0.mul(Jinv, w_fb_high);
W.mul(Jinv, wJhigh);
for(i=0; i<n_dof; i++)
{
double* w = W.data() + i;
double a = joint_weights(i);
for(j=0; j<n_dof; w+=n_dof, j++)
{
if(i==j) *w -= 1.0;
*w /= -a;
}
jvel0(i) /= a;
}
JW.mul(Jlow, W);
}
else
{
jvel0.zero();
JW.set(Jlow);
}
#ifdef MEASURE_TIME
clock_t t2 = clock();
high_constraint_time += (double)(t2-t1)/(double)CLOCKS_PER_SEC;
#endif
// Ǥ¡¦#x¥¯¥È¥ë¹à¤ê³×Z
if(n_low_const > 0)
{
fb_low_0.mul(Jlow, jvel0);
dfb.sub(fb_low, fb_low_0);
y.lineq_sr(JW, w_error, w_norm, damping, dfb);
if(n_high_const > 0) jvel.mul(W, y);
else jvel.set(y);
// fVec error(n_low_const);
// error = dfb-Jlow*jvel;
// cerr << dfb*dfb << "->" << error*error << endl;
}
else
{
jvel.zero();
}
// ³û¼â1¡¦x
jvel += jvel0;
#ifdef MEASURE_TIME
clock_t t3 = clock();
low_constraint_time += (double)(t3-t2)/(double)CLOCKS_PER_SEC;
#endif
SetJointVel(jvel);
// cerr << fb_high - Jhigh * jvel << endl;
}
if(current_max_condnum < 0.0) current_max_condnum = 1.0;
return current_max_condnum;
}
