void MyWindow::initIK()
{
// create variables
for (int i = 0; i < mSkel->getNumDofs(); i++) {
Var *v = new Var(mSkel->getDof(i)->getValue(), mSkel->getDof(i)->getMin(), mSkel->getDof(i)->getMax());
mVariables.push_back(v);
}
// create objBox
mObjBox = new ObjectiveBox(mVariables.size());
// create constraints
BodyNode *node = mSkel->getNode("fullbody1_h_heel_left");
Vector3d offset(0, 0, 0);
Vector3d target = xformHom(node->getWorldTransform(), offset);
PositionConstraint* pos = new PositionConstraint(mVariables, mSkel, node, offset, target);
mObjBox->add(pos);
node = mSkel->getNode("fullbody1_h_heel_right");
target = xformHom(node->getWorldTransform(), offset);
pos = new PositionConstraint(mVariables, mSkel, node, offset, target);
mObjBox->add(pos);
}
//==============================================================================
void DynamicsTest::compareAccelerations(const std::string& _fileName)
{
using namespace std;
using namespace Eigen;
using namespace dart;
using namespace math;
using namespace dynamics;
using namespace simulation;
using namespace utils;
//----------------------------- Settings -------------------------------------
const double TOLERANCE = 1.0e-2;
#ifndef NDEBUG // Debug mode
int nRandomItr = 2;
#else
int nRandomItr = 10;
#endif
double qLB = -0.5 * DART_PI;
double qUB = 0.5 * DART_PI;
double dqLB = -0.5 * DART_PI;
double dqUB = 0.5 * DART_PI;
double ddqLB = -0.5 * DART_PI;
double ddqUB = 0.5 * DART_PI;
Vector3d gravity(0.0, -9.81, 0.0);
double timeStep = 1.0e-6;
// load skeleton
World* world = SkelParser::readWorld(_fileName);
assert(world != NULL);
world->setGravity(gravity);
world->setTimeStep(timeStep);
//------------------------------ Tests ---------------------------------------
for (int i = 0; i < world->getNumSkeletons(); ++i)
{
Skeleton* skeleton = world->getSkeleton(i);
assert(skeleton != NULL);
int dof = skeleton->getNumGenCoords();
for (int j = 0; j < nRandomItr; ++j)
{
// Generate a random state and ddq
VectorXd q = VectorXd(dof);
VectorXd dq = VectorXd(dof);
VectorXd ddq = VectorXd(dof);
for (int k = 0; k < dof; ++k)
{
q[k] = math::random(qLB, qUB);
dq[k] = math::random(dqLB, dqUB);
ddq[k] = math::random(ddqLB, ddqUB);
// q[k] = 0.0;
// dq[k] = 0.0;
// ddq[k] = 0.0;
}
VectorXd x = VectorXd::Zero(dof * 2);
x << q, dq;
skeleton->setState(x, true, true, false);
skeleton->setGenAccs(ddq, true);
// Integrate state
skeleton->integrateConfigs(timeStep);
skeleton->integrateGenVels(timeStep);
VectorXd qNext = skeleton->getConfigs();
VectorXd dqNext = skeleton->getGenVels();
VectorXd xNext = VectorXd::Zero(dof * 2);
xNext << qNext, dqNext;
// For each body node
for (int k = 0; k < skeleton->getNumBodyNodes(); ++k)
{
BodyNode* bn = skeleton->getBodyNode(k);
int nDepGenCoord = bn->getNumDependentGenCoords();
// Calculation of velocities and Jacobian at k-th time step
skeleton->setState(x, true, true, false);
skeleton->setGenAccs(ddq, true);
Vector6d vBody1 = bn->getBodyVelocity();
Vector6d vWorld1 = bn->getWorldVelocity();
MatrixXd JBody1 = bn->getBodyJacobian();
MatrixXd JWorld1 = bn->getWorldJacobian();
Isometry3d T1 = bn->getWorldTransform();
// Get accelerations and time derivatives of Jacobians at k-th time step
Vector6d aBody1 = bn->getBodyAcceleration();
Vector6d aWorld1 = bn->getWorldAcceleration();
MatrixXd dJBody1 = bn->getBodyJacobianTimeDeriv();
MatrixXd dJWorld1 = bn->getWorldJacobianTimeDeriv();
// Calculation of velocities and Jacobian at (k+1)-th time step
skeleton->setState(xNext, true, true, false);
skeleton->setGenAccs(ddq, true);
Vector6d vBody2 = bn->getBodyVelocity();
Vector6d vWorld2 = bn->getWorldVelocity();
MatrixXd JBody2 = bn->getBodyJacobian();
MatrixXd JWorld2 = bn->getWorldJacobian();
Isometry3d T2 = bn->getWorldTransform();
// Get accelerations and time derivatives of Jacobians at k-th time step
Vector6d aBody2 = bn->getBodyAcceleration();
Vector6d aWorld2 = bn->getWorldAcceleration();
MatrixXd dJBody2 = bn->getBodyJacobianTimeDeriv();
MatrixXd dJWorld2 = bn->getWorldJacobianTimeDeriv();
// Calculation of approximated accelerations and time derivatives of
// Jacobians
Vector6d aBodyApprox = (vBody2 - vBody1) / timeStep;
Vector6d aWorldApprox = (vWorld2 - vWorld1) / timeStep;
// TODO(JS): Finite difference of Jacobian test is not implemented yet.
// MatrixXd dJBodyApprox = (JBody2 - JBody1) / timeStep;
// MatrixXd dJWorldApprox = (JWorld2 - JWorld1) / timeStep;
// MatrixXd dJBodyApprox = MatrixXd::Zero(6, nDepGenCoord);
// MatrixXd dJWorldApprox = MatrixXd::Zero(6, nDepGenCoord);
// for (int l = 0; l < nDepGenCoord; ++l)
// {
// skeleton->setConfig(q);
// Jacobian JBody_a = bn->getBodyJacobian();
// int idx = bn->getDependentGenCoordIndex(l);
// VectorXd qGrad = q;
// qGrad[idx] = qNext[idx];
// skeleton->setConfig(qGrad);
// Jacobian JBody_b = bn->getBodyJacobian();
// Jacobian dJBody_dq = (JBody_b - JBody_a) / (qNext[idx] - q[idx]);
// dJBodyApprox += dJBody_dq * dq[idx];
// }
// Comparing two velocities
EXPECT_TRUE(equals(aBody1, aBodyApprox, TOLERANCE));
EXPECT_TRUE(equals(aBody2, aBodyApprox, TOLERANCE));
EXPECT_TRUE(equals(aWorld1, aWorldApprox, TOLERANCE));
EXPECT_TRUE(equals(aWorld2, aWorldApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJBody1, dJBodyApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJBody2, dJBodyApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJWorld1, dJWorldApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJWorld2, dJWorldApprox, TOLERANCE));
// Debugging code
if (!equals(aBody1, aBodyApprox, TOLERANCE))
{
cout << "aBody1 :" << aBody1.transpose() << endl;
cout << "aBodyApprox:" << aBodyApprox.transpose() << endl;
}
if (!equals(aBody2, aBodyApprox, TOLERANCE))
{
cout << "aBody2 :" << aBody2.transpose() << endl;
cout << "aBodyApprox:" << aBodyApprox.transpose() << endl;
}
if (!equals(aWorld1, aWorldApprox, TOLERANCE))
{
cout << "aWorld1 :" << aWorld1.transpose() << endl;
cout << "aWorldApprox:" << aWorldApprox.transpose() << endl;
}
if (!equals(aWorld2, aWorldApprox, TOLERANCE))
{
cout << "aWorld2 :" << aWorld2.transpose() << endl;
cout << "aWorldApprox:" << aWorldApprox.transpose() << endl;
}
// if (!equals(dJBody1, dJBodyApprox, TOLERANCE))
// {
// cout << "Name :" << bn->getName() << endl;
// cout << "dJBody1 :" << endl << dJBody1 << endl;
// cout << "dJBodyApprox:" << endl << dJBodyApprox << endl;
// }
// if (!equals(dJBody2, dJBodyApprox, TOLERANCE))
// {
// cout << "dJBody2:" << endl << dJBody2.transpose() << endl;
// cout << "dJBodyApprox:" << endl << dJBodyApprox.transpose() << endl;
// }
// if (!equals(dJWorld1, dJWorldApprox, TOLERANCE))
// {
// cout << "dJWorld1 :" << endl << dJWorld1 << endl;
// cout << "dJWorldApprox:" << endl << dJWorldApprox << endl;
// }
// if (!equals(dJWorld2, dJWorldApprox, TOLERANCE))
// {
// cout << "dJWorld2 :" << endl << dJWorld2 << endl;
// cout << "dJWorldApprox:" << endl << dJWorldApprox << endl;
// }
}
}
}
delete world;
}
/* ********************************************************************************************* */
TEST(KINEMATICS, TRANS_AND_DERIV) {
using namespace Eigen;
using namespace kinematics;
FileInfoSkel<Skeleton> modelFile;
bool loadModelResult = modelFile.loadFile(DART_DATA_PATH"skel/SehoonVSK3.vsk");
ASSERT_TRUE(loadModelResult);
Skeleton* skel = modelFile.getSkel();
EXPECT_TRUE(skel != NULL);
/* LOG(INFO) << "# Dofs = " << skel->getNumDofs(); */
/* LOG(INFO) << "# Nodes = " << skel->getNumNodes(); */
FileInfoDof dofFile(skel);
bool loadDofResult = dofFile.loadFile(DART_DATA_PATH"dof/init_Tpose.dof");
ASSERT_TRUE(loadDofResult);
/* LOG(INFO) << "# frames = " << dofFile.getNumFrames(); */
VectorXd pose = dofFile.getPoseAtFrame(0);
skel->setPose(pose, true, true);
const int nodeIndex = 1;
BodyNode* node = skel->getNode(nodeIndex);
EXPECT_TRUE(node != NULL);
const double TOLERANCE = 0.000001;
// Check the one global transform matrix
const Matrix4d& W = skel->getNode(20)->getWorldTransform();
Matrix4d W_truth;
W_truth << -0.110662, 0.991044, 0.074734, 0.642841
, -0.987564, -0.118099, 0.103775, 0.327496
, 0.111672, -0.0623207, 0.991789, -0.12855
, 0, 0, 0, 1;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
EXPECT_NEAR(W(i, j), W_truth(i, j), TOLERANCE);
}
}
// Check the derivative of one global transform matrix
// const Matrix4d& Wq = skel->getNode(10)->mWq.at(4);
const Matrix4d& Wq = skel->getNode(10)->getDerivWorldTransform(4);
Matrix4d Wq_truth;
Wq_truth << 0.121382, 0.413015, 0.902378, 0.161838
,-0.0175714, 0.00698451, 0.0149899, 0.00571836
,-0.992336, 0.0556535, 0.107702, 0.175059
,0, 0, 0, 0;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
EXPECT_NEAR(Wq(i, j), Wq_truth(i, j), TOLERANCE);
}
}
// Check Jacobians
BodyNode *nodecheck = skel->getNode(23);
// Linear Jacobian
const MatrixXd Jv = nodecheck->getJacobianLinear();
MatrixXd Jv_truth = MatrixXd::Zero(Jv.rows(), Jv.cols());
Jv_truth<<1, 0, 0, 0.013694, -0.0194371, -0.422612, 0.0157281, -0.00961473, -0.421925, 0.0026645, -0.0048767, -0.179914, -0.0013539, -0.00132969, -0.00885535,
0, 1, 0, 0.0321939, 0.00226492, -0.135736, 0.0330525, 0.00462613, -0.135448, 0.0181491, 0.00758086, -0.122187, 0.00532694, 0.0049221, -0.128917,
0, 0, 1, 0.423948, 0.130694, 0.0166546, 0.42642, 0.118176, 0.0221309, 0.180296, 0.120584, 0.0105059, 0.0838259, 0.081304, 0.00719314;
for (int i = 0; i < Jv.rows(); i++) {
for (int j = 0; j < Jv.cols(); j++) {
EXPECT_NEAR(Jv(i, j), Jv_truth(i, j), TOLERANCE);
}
}
// Angular Jacobian
const MatrixXd Jwbody = nodecheck->getWorldTransform().topLeftCorner<3,3>().transpose()
* nodecheck->getJacobianAngular();
MatrixXd Jwbody_truth = MatrixXd::Zero(Jwbody.rows(), Jwbody.cols());
Jwbody_truth << 0, 0, 0, 0.0818662, 0.996527, 0.000504051, 0.110263, 0.993552, 0.0249018, 0.0772274, 0.995683, 0.0423836, 0.648386, 0.628838, 0.0338389,
0, 0, 0, -0.995079, 0.082001, -0.0518665, -0.992054, 0.111479, -0.0554717, -0.996033, 0.078601, -0.0313861, -0.629201, 0.649145, -0.00994729,
0, 0, 0, -0.0518265, 0.00391001, 0.998406, -0.0579139, -0.0187244, 0.998021, -0.0343359, -0.0398718, 0.998564, -0.0262454, -0.0235626, 0.999159;
for (int i = 0; i < Jwbody.rows(); i++) {
for (int j = 0; j < Jwbody.cols(); j++) {
EXPECT_NEAR(Jwbody(i, j), Jwbody_truth(i, j), TOLERANCE);
}
}
}