bool GSpringDamperBody::applyForce(bool badd_)
{
if ( pLeftBody == NULL || pRightBody == NULL ) return false;
SE3 T = inv_T_left * Inv(pLeftBody->T_global) * pRightBody->T_global * T_right;
//se3 x(Log(T.GetRotation()), T.GetPosition()); // relative location of the right body from the left body
se3 x(Log(T)); // relative location of the right body from the left body
se3 V_left = Ad(inv_T_left, pLeftBody->V) - Ad(T * inv_T_right, pRightBody->V); // relative velocity of the left body
dse3 F_left; // force to be acting on the left body
for (int i=0; i<6; i++) {
F_left[i] = K[i] * x[i] - C[i] * V_left[i];
}
if ( badd_ ) {
pLeftBody->Fe += dAd(inv_T_left, F_left);
pRightBody->Fe += dAd(T * inv_T_right, -F_left);
} else {
pLeftBody->Fe -= dAd(inv_T_left, F_left);
pRightBody->Fe -= dAd(T * inv_T_right, -F_left);
}
return true;
}
void GSystem::calcDerivative_MomentumCOM_Dq_Ddq(RMatrix &DHcDq_, RMatrix &DHcDdq_)
{
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());
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
Vec3 p = getPositionCOMGlobal();
dse3 Hg = getMomentumGlobal();
RMatrix DHgDq(6, getNumCoordinates()), DHgDdq(6, getNumCoordinates());
dse3 DHgDqi, DHgDdqi;
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DHgDqi = getDerivative_MomentumGlobal_Dq(*iter_pcoord);
DHgDdqi = getDerivative_MomentumGlobal_Ddq(*iter_pcoord);
//DHgDq.Push(0, i, convert_to_RMatrix(DHgDqi));
//DHgDdq.Push(0, i, convert_to_RMatrix(DHgDdqi));
matSet(&DHgDq[6*i], DHgDqi.GetArray(), 6);
matSet(&DHgDdq[6*i], DHgDdqi.GetArray(), 6);
}
RMatrix DdAdDq_Hg(6, getNumCoordinates());
Vec3 DpDqi;
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
//DdAdDq_Hg.Push(0, i, convert_to_RMatrix(dAd(SE3(p), dad(se3(Vec3(0,0,0),DpDqi), Hg))));
matSet(&DdAdDq_Hg[6*i], dAd(SE3(p), dad(se3(Vec3(0,0,0),DpDqi), Hg)).GetArray(), 6);
}
DHcDq_ = dAd(SE3(p), DHgDq) + DdAdDq_Hg;
DHcDdq_ = dAd(SE3(p), DHgDdq);
// 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 vpSystem::IDIteration2(void)
{
int i, j;
vpJoint *pCurrent, *pChild;
for ( i = m_pJoint.size() - 1; i >= 0; i-- )
{
pCurrent = m_pJoint[i];
pCurrent->m_sF = pCurrent->m_sI * pCurrent->m_sDV - dad(pCurrent->m_sV, pCurrent->m_sI * pCurrent->m_sV);
for ( j = 0; j < pCurrent->m_pChildJoints.size(); j++ )
{
pChild = pCurrent->m_pChildJoints[j];
pCurrent->m_sF += InvdAd(pChild->m_sRelativeFrame, pChild->m_sF);
}
pCurrent->m_sF -= dAd(pCurrent->m_sRightBodyFrame, pCurrent->m_pRightBody->GetForce());
pCurrent->UpdateTorqueID();
}
if ( m_pRoot->m_bIsGround ) return;
m_pRoot->m_sForce = m_pRoot->m_sI * m_pRoot->m_sDV - dad(m_pRoot->m_sV, m_pRoot->m_sI * m_pRoot->m_sV);
for ( i = 0; i < m_pRoot->m_pJoint.size(); i++ )
{
pChild = m_pRoot->m_pJoint[i];
m_pRoot->m_sForce += InvdAd(pChild->m_sRelativeFrame, pChild->m_sF);
}
m_pRoot->m_sForce -= m_pRoot->GetGravityForce();
}
void vpSystem::HDIteration2(void)
{
int i, j;
vpJoint *pCurrent, *pChild;
AInertia tmpI;
for ( i = m_pJoint.size() - 1; i >= 0; i-- )
{
pCurrent = m_pJoint[i];
pCurrent->m_sJ = pCurrent->m_sI;
pCurrent->m_sB = -dad(pCurrent->m_sV, pCurrent->m_sI * pCurrent->m_sV) - dAd(pCurrent->m_sRightBodyFrame, pCurrent->m_pRightBody->GetForce());
for ( j = 0; j < pCurrent->m_pChildJoints.size(); j++ )
{
pChild = pCurrent->m_pChildJoints[j];
if ( pChild->m_sHDType == VP::KINEMATIC )
{
pCurrent->m_sJ.AddTransform(pChild->m_sJ, Inv(pChild->m_sRelativeFrame));
} else
{
pChild->UpdateAInertia(tmpI);
pCurrent->m_sJ.AddTransform(tmpI, Inv(pChild->m_sRelativeFrame));
}
pCurrent->m_sB += InvdAd(pChild->m_sRelativeFrame, pChild->m_sC + pChild->GetLP());
}
pCurrent->m_sC = pCurrent->m_sB + pCurrent->m_sJ * pCurrent->m_sW;
if ( pCurrent->m_sHDType == VP::KINEMATIC ) pCurrent->UpdateLP();
else pCurrent->UpdateLOTP();
}
if ( m_pRoot->m_bIsGround ) return;
if ( m_pRoot->m_pJoint.size() ) m_sRootInertia = m_pRoot->m_sI;
m_sRootBias = -dad(m_pRoot->m_sV, m_pRoot->m_sI * m_pRoot->m_sV);
if ( m_pRoot->m_sHDType == VP::DYNAMIC ) m_sRootBias -= m_pRoot->GetForce();
else m_sRootBias -= m_pRoot->GetGravityForce();
for ( i = 0; i < m_pRoot->m_pJoint.size(); i++ )
{
pChild = m_pRoot->m_pJoint[i];
if ( pChild->m_sHDType == VP::KINEMATIC )
{
m_sRootInertia.AddTransform(pChild->m_sJ, Inv(pChild->m_sRelativeFrame));
} else
{
pChild->UpdateAInertia(tmpI);
m_sRootInertia.AddTransform(tmpI, Inv(pChild->m_sRelativeFrame));
}
m_sRootBias += InvdAd(pChild->m_sRelativeFrame, pChild->m_sC + pChild->GetLP());
}
}
void vpSystem::FDIteration2s(int idx)
{
int i, j;
vpJoint *pCurrent, *pChild;
for ( i = m_pJoint.size() - 1; i > idx; i-- )
{
pCurrent = m_pJoint[i];
pCurrent->m_sB = SCALAR_0;
pCurrent->ClearTP();
}
for ( i = idx; i >= 0; i-- )
{
pCurrent = m_pJoint[i];
pCurrent->m_sB = -dAd(pCurrent->m_sRightBodyFrame, pCurrent->m_pRightBody->GetImpulse());
for ( j = 0; j < pCurrent->m_pChildJoints.size(); j++ )
{
pChild = pCurrent->m_pChildJoints[j];
pCurrent->m_sB += InvdAd(pChild->m_sRelativeFrame, pChild->m_sB + pChild->GetLP());
}
pCurrent->UpdateTP();
}
if ( m_pRoot->m_bIsGround ) return;
m_sRootBias = SCALAR_0;
for ( i = 0; i < m_pRoot->m_pJoint.size(); i++ )
{
pChild = m_pRoot->m_pJoint[i];
m_sRootBias += InvdAd(pChild->m_sRelativeFrame, pChild->m_sB + pChild->GetLP());
}
m_sRootBias -= m_pRoot->GetImpulse();
}
dse3 GSystem::getMomentumCOM()
{
return dAd(SE3(getPositionCOMGlobal()), getMomentumGlobal());
}
