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 TranslationalJoint::_updateAcceleration()
{
// dS = 0
mdS.setZero();
// dV = dS * dq + S * ddq
mdV = mS * get_ddq();
}
void RevoluteJoint::_updateAcceleration()
{
// dS = 0
mdS.setZero();
// dV = dS * dq + S * ddq
mdV = mS * get_ddq();
}
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;
}
inline void UniversalJoint::_updateAcceleration()
{
// dS
mdS.col(0) = -math::ad(mS.col(1)*mCoordinate[1].get_dq(), math::AdTAngular(mT_ChildBodyToJoint * math::expAngular(-mAxis[1]*mCoordinate[1].get_q()), mAxis[0]));
//mdS.col(1) = setZero();
// dV = dS * dq + S * ddq
mdV = mdS * get_dq() + mS * get_ddq();
}
void GSystem::getEquationsOfMotion(RMatrix &M, RMatrix &b)
{
RMatrix ddq = get_ddq(), tau = get_tau(); // save current ddq and tau
int n = getNumCoordinates();
M.ReNew(n,n);
set_ddq(Zeros(n,1));
GSystem::calcInverseDynamics();
b = get_tau();
for (int i=0; i<n; i++) {
RMatrix unit = Zeros(n,1);
unit[i] = 1;
set_ddq(unit);
GSystem::calcInverseDynamics();
get_tau(&M[i*n]);
for (int j=0; j<n; j++) {
M[i*n+j] -= b[j];
}
}
set_ddq(ddq); set_tau(tau); // restore ddq and tau
}
void FreeJoint::_updateAcceleration()
{
// dS
Eigen::Vector3d q(mCoordinate[0].get_q(),
mCoordinate[1].get_q(),
mCoordinate[2].get_q());
Eigen::Vector3d dq(mCoordinate[0].get_dq(),
mCoordinate[1].get_dq(),
mCoordinate[2].get_dq());
Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
Eigen::Vector6d dJ0;
Eigen::Vector6d dJ1;
Eigen::Vector6d dJ2;
Eigen::Vector6d J3;
Eigen::Vector6d J4;
Eigen::Vector6d J5;
dJ0 << dJ(0,0), dJ(0,1), dJ(0,2), 0, 0, 0;
dJ1 << dJ(1,0), dJ(1,1), dJ(1,2), 0, 0, 0;
dJ2 << dJ(2,0), dJ(2,1), dJ(2,2), 0, 0, 0;
J3 << 0, 0, 0, 1, 0, 0;
J4 << 0, 0, 0, 0, 1, 0;
J5 << 0, 0, 0, 0, 0, 1;
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
mdS.col(3) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3));
mdS.col(4) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4));
mdS.col(5) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5));
// dV = dS * dq + S * ddq
mdV = mdS * get_dq() + mS * get_ddq();
}
inline void EulerJoint::_updateAcceleration()
{
// dS
double q0 = mCoordinate[0].get_q();
double q1 = mCoordinate[1].get_q();
double q2 = mCoordinate[2].get_q();
//double dq0 = mCoordinate[0].get_dq();
double dq1 = mCoordinate[1].get_dq();
double dq2 = mCoordinate[2].get_dq();
//double c0 = cos(q0);
double c1 = cos(q1);
double c2 = cos(q2);
//double s0 = sin(q0);
double s1 = sin(q1);
double s2 = sin(q2);
Eigen::Vector6d dJ0 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ1 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ2 = Eigen::Vector6d::Zero();
switch (mAxisOrder)
{
case AO_XYZ:
{
//--------------------------------------------------------------------------
// dS = [ -(dq1*c2*s1) - dq2*c1*s2, dq2*c2, 0
// -(dq2*c1*c2) + dq1*s1*s2, -(dq2*s2), 0
// dq1*c1, 0, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//--------------------------------------------------------------------------
dJ0 << -(dq1*c2*s1) - dq2*c1*s2, -(dq2*c1*c2) + dq1*s1*s2, dq1*c1, 0, 0, 0;
dJ1 << dq2*c2, -(dq2*s2), 0.0, 0.0, 0.0, 0.0;
dJ2.setConstant(0.0);
break;
}
case AO_ZYX:
{
//--------------------------------------------------------------------------
// dS = [ -c1*dq1, 0, 0
// c2*c1*dq2-s2*s1*dq1, -s2*dq2, 0
// -s1*c2*dq1-c1*s2*dq2, -c2*dq2, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//--------------------------------------------------------------------------
dJ0 << -c1*dq1, c2*c1*dq2 - s2*s1*dq1, -s1*c2*dq1 - c1*s2*dq2, 0.0, 0.0, 0.0;
dJ1 << 0.0, -s2*dq2, -c2*dq2, 0.0, 0.0, 0.0;
dJ2.setConstant(0.0);
break;
}
default:
{
dterr << "Undefined Euler axis order\n";
break;
}
}
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
// dV = dS * dq + S * ddq
mdV = mdS * get_dq() + mS * get_ddq();
}
