bool GSystem::calcProductOfInvMassAndMatrix2(RMatrix &invM_A, const RMatrix &A)
{
if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
invM_A.SetZero(A.RowSize(), A.ColSize());
int i;
std::list<GJoint *>::iterator iter_pjoint;
std::vector<bool> isprescribed(pJoints.size());
// save current info
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
isprescribed[i] = (*iter_pjoint)->isPrescribed();
}
setAllJointsPrescribed(false);
initDynamicsWithZeroGravityAndVelocity();
for (i=0; i<A.ColSize(); i++) {
set_tau(&(A[i*A.RowSize()]));
calcDynamicsWithZeroGravityAndVelocity();
get_ddq(&(invM_A[i*invM_A.RowSize()]));
}
// restore
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
(*iter_pjoint)->setPrescribed(isprescribed[i]);
}
return true;
}
void GSystem::calcDerivative_PositionCOMGlobal_Dq(RMatrix &DpDq_)
{
int i;
std::list<GBody *>::iterator iter_pbody;
std::list<GCoordinate *>::iterator iter_pcoord;
std::vector<SE3> M(pBodies.size());
std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
Vec3 DpDqi;
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
// save previous settings
M[i] = (*iter_pbody)->fwdJointChain.M1;
jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;
// prerequisites for (*iter_pbody)->getDerivative_PositionCOMGlobal_Dq(...)
(*iter_pbody)->fwdJointChain.setM(SE3());
(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
(*iter_pbody)->fwdJointChain.update_J();
}
// calculate the derivative
DpDq_.SetZero(3, getNumCoordinates());
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
//DpDq_.Push(0, i, convert_to_RMatrix(DpDqi));
matSet(&DpDq_[3*i], DpDqi.GetArray(), 3);
}
// restore the previous settings
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
(*iter_pbody)->fwdJointChain.setM(M[i]);
(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
}
}
bool GSystemIK::solveIK_dq(RMatrix &dq, std::vector<GBody*> pbodies_primary, std::vector<GBody*> pbodies_secondary, std::vector<Vec3> p_primary, std::vector<Vec3> p_secondary, std::vector<se3> V_primary, std::vector<se3> V_secondary, std::vector< std::vector<int> > idxC_primary, std::vector< std::vector<int> > idxC_secondary, gReal alpha_primary, gReal alpha_secondary)
{
RMatrix Jp, Js; // Jacobian matrices for primary/secondary constraints
RMatrix Vp, Vs; // the righthand side of the constraints
if ( !buildConstrIK_dq(Jp, Vp, pbodies_primary, p_primary, V_primary, idxC_primary) ) return false;
if ( !buildConstrIK_dq(Js, Vs, pbodies_secondary, p_secondary, V_secondary, idxC_secondary) ) return false;
dq.SetZero(getNumCoordinates(), 1);
if ( Jp.RowSize() > 0 ) {
RMatrix dq0, N;
dq0 = srInv(Jp, N, alpha_primary) * Vp;
if ( Js.RowSize() > 0 ) {
dq = dq0 + N * ( srInv(Js * N, alpha_secondary) * (Vs - Js * dq0) );
} else {
dq = dq0;
}
} else {
if ( Js.RowSize() > 0 ) {
dq = srInv(Js, alpha_secondary) * Vs;
}
}
return true;
}
bool GSystem::calcProductOfInvMassAndMatrix(RMatrix &invM_A, const RMatrix &A)
{
if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
invM_A.SetZero(A.RowSize(), A.ColSize());
int i;
std::list<GJoint *>::iterator iter_pjoint;
std::vector<bool> isprescribed(pJoints.size());
// save current info
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
isprescribed[i] = (*iter_pjoint)->isPrescribed();
}
Vec3 g = getGravity();
// set all joint unprescribed and set zero gravity
setAllJointsPrescribed(false);
setGravity(Vec3(0,0,0));
update_joint_local_info_short();
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->update_base_joint_info();
(*iter_pbody)->update_T();
(*iter_pbody)->set_eta_zero();
}
for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
(*riter_pbody)->update_aI();
(*riter_pbody)->update_Psi();
(*riter_pbody)->update_Pi();
}
for (i=0; i<A.ColSize(); i++) {
set_ddq(Zeros(pCoordinates.size(),1)); // this isn't necessary for real tree structure systems, but works for the cut joints in closed-loop
set_tau(&(A[i*A.RowSize()]));
for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
(*riter_pbody)->update_aB_zeroV_zeroeta();
(*riter_pbody)->update_beta_zeroeta();
}
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->update_ddq();
(*iter_pbody)->update_dV(true);
}
get_ddq(&(invM_A[i*invM_A.RowSize()]));
}
// restore
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
(*iter_pjoint)->setPrescribed(isprescribed[i]);
}
setGravity(g);
return true;
}
void GSystem::calcDerivative_MomentumGlobal_Dq_Ddq(std::vector<GCoordinate*> pCoordinates_, RMatrix &DHgDq_, RMatrix &DHgDdq_)
{
int i;
std::list<GBody *>::iterator iter_pbody;
std::vector<GCoordinate *>::iterator iter_pcoord;
std::vector<SE3> M(pBodies.size());
std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
dse3 DHDqi, DHDdqi;
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
// save previous settings
M[i] = (*iter_pbody)->fwdJointChain.M1;
jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;
// prerequisites for (*iter_pbody)->getDerivative_MomentumGlobal_Dq(...) and (*iter_pbody)->getDerivative_MomentumGlobal_Ddq(...)
(*iter_pbody)->fwdJointChain.setM(SE3());
(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
(*iter_pbody)->fwdJointChain.update_J();
}
// calculate the derivatives
DHgDq_.SetZero(6, int(pCoordinates_.size()));
DHgDdq_.SetZero(6, int(pCoordinates_.size()));
for (i=0, iter_pcoord = pCoordinates_.begin(); iter_pcoord != pCoordinates_.end(); i++, iter_pcoord++) {
DHDqi = getDerivative_MomentumGlobal_Dq(*iter_pcoord);
DHDdqi = getDerivative_MomentumGlobal_Ddq(*iter_pcoord);
//DHgDq_.Push(0, i, convert_to_RMatrix(DHDqi));
//DHgDdq_.Push(0, i, convert_to_RMatrix(DHDdqi));
matSet(&DHgDq_[6*i], DHDqi.GetArray(), 6);
matSet(&DHgDdq_[6*i], DHDdqi.GetArray(), 6);
}
// restore the previous settings
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
(*iter_pbody)->fwdJointChain.setM(M[i]);
(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
}
}
void GSystem::calcDerivative_PositionCOMGlobal_Dq_2(RMatrix &DpDq_)
{
int i;
std::list<GCoordinate *>::iterator iter_pcoord;
Vec3 DpDqi;
DpDq_.SetZero(3, getNumCoordinates());
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DpDqi = getDerivative_PositionCOMGlobal_Dq_2(*iter_pcoord);
DpDq_(0,i) = DpDqi[0];
DpDq_(1,i) = DpDqi[1];
DpDq_(2,i) = DpDqi[2];
}
}
bool GSystemIK::buildConstrIK_dq(RMatrix &J, RMatrix &V, std::vector<GBody*> pbodies, std::vector<Vec3> pos, std::vector<se3> V_target, std::vector< std::vector<int> > idxC)
{
int i, j, k;
int cnt; // a counter
int nb; // number of bodies
int ncik; // number of IK constraints
std::list<GCoordinate*>::iterator iter_pcoord, iter_pcoord2;
nb = int(pbodies.size());
if ( pos.size() != (size_t)nb || V_target.size() != (size_t)nb || idxC.size() != (size_t)nb ) return false;
// counts the number of IK constraints
ncik = 0;
for (i=0; i<nb; i++) {
for ( j=0; j<int(idxC[i].size()); j++) {
if ( idxC[i][j] < 0 || idxC[i][j] > 5 ) return false;
}
ncik += int(idxC[i].size());
}
J.SetZero(ncik, getNumCoordinates());
V.SetZero(ncik, 1);
// build J, V
cnt = 0;
for (i=0; i<nb; i++) {
// update Jacobian
pbodies[i]->fwdJointChain.setM(SE3(pos[i]));
pbodies[i]->fwdJointChain.setJointLoopConstraintType(GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION);
pbodies[i]->fwdJointChain.update_J();
// transformed Jacobian, Jg = [R 0; 0 R] * J
RMatrix Jg(pbodies[i]->fwdJointChain.J.RowSize(), pbodies[i]->fwdJointChain.J.ColSize());
//RMatrix R = convert_to_RMatrix(pbodies[i]->T_global.GetRotation());
RMatrix R(3,3); matSet(R.GetPtr(), pbodies[i]->T_global.GetRotation().GetArray(), 9);
Jg.Push(0, 0, R * pbodies[i]->fwdJointChain.J.Sub(0, 2, 0, pbodies[i]->fwdJointChain.J.ColSize()-1));
Jg.Push(3, 0, R * pbodies[i]->fwdJointChain.J.Sub(3, 5, 0, pbodies[i]->fwdJointChain.J.ColSize()-1));
// build J
for (j=0, iter_pcoord = pbodies[i]->fwdJointChain.pCoordinates.begin(); iter_pcoord != pbodies[i]->fwdJointChain.pCoordinates.end(); j++, iter_pcoord++) {
// find index of pbodies[i]->fwdJointChain.pCoordinates[j] in pCoordinates
int idx = -1;
for (k=0, iter_pcoord2 = pCoordinates.begin(); iter_pcoord2 != pCoordinates.end(); k++, iter_pcoord2++) {
if ( *iter_pcoord2 == *iter_pcoord ) { idx = k; break; }
}
if ( idx < 0 ) return false;
// insert j-th column of the body Jacobian to the right place
for (k=0; k<int(idxC[i].size()); k++) {
J(cnt+k, idx) = Jg(idxC[i][k], j);
}
}
// build V
for (j=0; j<int(idxC[i].size()); j++) {
V(cnt+j, 0) = V_target[i][idxC[i][j]];
}
cnt += int(idxC[i].size());
}
return true;
}
bool GSystemIK::solveIK_dq(RMatrix &dq, std::vector<GBody*> pbodies_primary, std::vector<GBody*> pbodies_secondary, std::vector<Vec3> p_primary, std::vector<Vec3> p_secondary, std::vector<se3> V_primary, std::vector<se3> V_secondary, std::vector< std::vector<int> > idxC_primary, std::vector< std::vector<int> > idxC_secondary, std::vector<GCoordinate*> pcoords_prescribed, std::ofstream *pfout, gReal alpha_primary, gReal alpha_secondary)
{
if ( pcoords_prescribed.size() == 0 ) {
return solveIK_dq(dq, pbodies_primary, pbodies_secondary, p_primary, p_secondary, V_primary, V_secondary, idxC_primary, idxC_secondary, alpha_primary, alpha_secondary);
}
std::vector<GCoordinate*>::iterator viter_pcoord;
std::list<GCoordinate*>::iterator iter_pcoord;
RMatrix Jp, Js; // Jacobian matrices for primary/secondary constraints
RMatrix Vp, Vs; // the righthand isde of the constraints
if ( !buildConstrIK_dq(Jp, Vp, pbodies_primary, p_primary, V_primary, idxC_primary) ) return false;
if ( !buildConstrIK_dq(Js, Vs, pbodies_secondary, p_secondary, V_secondary, idxC_secondary) ) return false;
int num_prescribed = 0;
std::vector<bool> b_prescribed;
for (iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); iter_pcoord++) {
if ( find(pcoords_prescribed.begin(), pcoords_prescribed.end(), *iter_pcoord) == pcoords_prescribed.end() ) {
b_prescribed.push_back(false);
} else {
b_prescribed.push_back(true);
num_prescribed++;
}
}
std::vector<GCoordinate*> pcoords_all(pCoordinates.begin(), pCoordinates.end());
int nx = getNumCoordinates() - num_prescribed;
RMatrix Jp_(Jp.RowSize(), nx), Js_(Js.RowSize(), nx);
RMatrix Vp_(Vp), Vs_(Vs);
int cnt = 0;
for (int i=0; i<getNumCoordinates(); i++) {
if ( b_prescribed[i] ) {
Vp_ -= pcoords_all[i]->dq * Jp.Sub(0, Jp.RowSize()-1, i, i);
Vs_ -= pcoords_all[i]->dq * Js.Sub(0, Js.RowSize()-1, i, i);
} else {
Jp_.Push(0, cnt, Jp.Sub(0, Jp.RowSize()-1, i, i));
Js_.Push(0, cnt, Js.Sub(0, Js.RowSize()-1, i, i));
cnt++;
}
}
RMatrix x;
x.SetZero(nx,1);
if ( Jp_.RowSize() > 0 ) {
RMatrix x0, N;
x0 = srInv(Jp_, N, alpha_primary) * Vp_;
if ( Js_.RowSize() > 0 ) {
x = x0 + N * ( srInv(Js_ * N, alpha_secondary) * (Vs_ - Js_ * x0) );
} else {
x = x0;
}
} else {
if ( Js_.RowSize() > 0 ) {
x = srInv(Js_, alpha_secondary) * Vs_;
}
}
dq.ReSize(getNumCoordinates(), 1);
cnt = 0;
for (int i=0; i<getNumCoordinates(); i++) {
if ( b_prescribed[i] ) {
dq[i] = pcoords_all[i]->dq;
} else {
dq[i] = x[cnt++];
}
}
if ( pfout != NULL ) {
*pfout << "x = " << x << "Jp_ = " << Jp_ << "Js_ = " << Js_ << "Vp_ = " << Vp_ << "Vs_ = " << Vs_;
*pfout << "Jp = " << Jp << "Js = " << Js << "Vp = " << Vp << "Vs = " << Vs;
*pfout << "dq_prescribed = (";
for (int i=0; i<(int)pcoords_prescribed.size(); i++) {
*pfout << pcoords_prescribed[i]->dq << ", ";
}
*pfout << std::endl;
}
return true;
}
