本文整理汇总了C++中BodyNode::getDependentGenCoordIndex方法的典型用法代码示例。如果您正苦于以下问题：C++ BodyNode::getDependentGenCoordIndex方法的具体用法？C++ BodyNode::getDependentGenCoordIndex怎么用？C++ BodyNode::getDependentGenCoordIndex使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类BodyNode的用法示例。
在下文中一共展示了BodyNode::getDependentGenCoordIndex方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: compareVelocities
//==============================================================================
void DynamicsTest::compareVelocities(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-6;
#ifndef NDEBUG // Debug mode
int nRandomItr = 10;
#else
int nRandomItr = 1;
#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);
// load skeleton
World* world = SkelParser::readWorld(_fileName);
assert(world != NULL);
world->setGravity(gravity);
//------------------------------ 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
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);
}
VectorXd state = VectorXd::Zero(dof * 2);
state << q, dq;
skeleton->setState(state, true, true, true);
skeleton->setGenAccs(ddq, true);
skeleton->computeInverseDynamics(false, false);
// For each body node
for (int k = 0; k < skeleton->getNumBodyNodes(); ++k)
{
BodyNode* bn = skeleton->getBodyNode(k);
// Calculation of velocities using recursive method
Vector6d vBody = bn->getBodyVelocity();
Vector6d vWorld = bn->getWorldVelocity();
Vector6d aBody = bn->getBodyAcceleration();
Vector6d aWorld = bn->getWorldAcceleration();
// Calculation of velocities using Jacobian and dq
MatrixXd JBody = bn->getBodyJacobian();
MatrixXd JWorld = bn->getWorldJacobian();
MatrixXd dJBody = bn->getBodyJacobianTimeDeriv();
MatrixXd dJWorld = bn->getWorldJacobianTimeDeriv();
Vector6d vBody2 = Vector6d::Zero();
Vector6d vWorld2 = Vector6d::Zero();
Vector6d aBody2 = Vector6d::Zero();
Vector6d aWorld2 = Vector6d::Zero();
for (int l = 0; l < bn->getNumDependentGenCoords(); ++l)
{
int idx = bn->getDependentGenCoordIndex(l);
vBody2 += JBody.col(l) * dq[idx];
vWorld2 += JWorld.col(l) * dq[idx];
aBody2 += dJBody.col(l) * dq[idx] + JBody.col(l) * ddq[idx];
aWorld2 += dJWorld.col(l) * dq[idx] + JWorld.col(l) * ddq[idx];
}
// Comparing two velocities
EXPECT_TRUE(equals(vBody, vBody2, TOLERANCE));
EXPECT_TRUE(equals(vWorld, vWorld2, TOLERANCE));
EXPECT_TRUE(equals(aBody, aBody2, TOLERANCE));
EXPECT_TRUE(equals(aWorld, aWorld2, TOLERANCE));
// Debugging code
if (!equals(vBody, vBody2, TOLERANCE))
{
cout << "vBody : " << vBody.transpose() << endl;
cout << "vBody2: " << vBody2.transpose() << endl;
}
if (!equals(vWorld, vWorld2, TOLERANCE))
{
cout << "vWorld : " << vWorld.transpose() << endl;
//.........这里部分代码省略.........