本文整理汇总了C++中Jacobian::col方法的典型用法代码示例。如果您正苦于以下问题：C++ Jacobian::col方法的具体用法？C++ Jacobian::col怎么用？C++ Jacobian::col使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Jacobian的用法示例。
在下文中一共展示了Jacobian::col方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: kinematicsTest
//==============================================================================
void JOINTS::kinematicsTest(Joint* _joint)
{
assert(_joint);
int numTests = 1;
_joint->setTransformFromChildBodyNode(
math::expMap(Eigen::Vector6d::Random()));
_joint->setTransformFromParentBodyNode(
math::expMap(Eigen::Vector6d::Random()));
BodyNode* bodyNode = new BodyNode();
bodyNode->setParentJoint(_joint);
Skeleton skeleton;
skeleton.addBodyNode(bodyNode);
skeleton.init();
int dof = _joint->getNumDofs();
//--------------------------------------------------------------------------
//
//--------------------------------------------------------------------------
VectorXd q = VectorXd::Zero(dof);
VectorXd dq = VectorXd::Zero(dof);
for (int idxTest = 0; idxTest < numTests; ++idxTest)
{
double q_delta = 0.000001;
for (int i = 0; i < dof; ++i)
{
q(i) = random(-DART_PI*1.0, DART_PI*1.0);
dq(i) = random(-DART_PI*1.0, DART_PI*1.0);
}
skeleton.setPositions(q);
skeleton.setVelocities(dq);
skeleton.computeForwardKinematics(true, true, false);
if (_joint->getNumDofs() == 0)
return;
Eigen::Isometry3d T = _joint->getLocalTransform();
Jacobian J = _joint->getLocalJacobian();
Jacobian dJ = _joint->getLocalJacobianTimeDeriv();
//--------------------------------------------------------------------------
// Test T
//--------------------------------------------------------------------------
EXPECT_TRUE(math::verifyTransform(T));
//--------------------------------------------------------------------------
// Test analytic Jacobian and numerical Jacobian
// J == numericalJ
//--------------------------------------------------------------------------
Jacobian numericJ = Jacobian::Zero(6,dof);
for (int i = 0; i < dof; ++i)
{
// a
Eigen::VectorXd q_a = q;
_joint->setPositions(q_a);
skeleton.computeForwardKinematics(true, false, false);
Eigen::Isometry3d T_a = _joint->getLocalTransform();
// b
Eigen::VectorXd q_b = q;
q_b(i) += q_delta;
_joint->setPositions(q_b);
skeleton.computeForwardKinematics(true, false, false);
Eigen::Isometry3d T_b = _joint->getLocalTransform();
//
Eigen::Isometry3d Tinv_a = T_a.inverse();
Eigen::Matrix4d Tinv_a_eigen = Tinv_a.matrix();
// dTdq
Eigen::Matrix4d T_a_eigen = T_a.matrix();
Eigen::Matrix4d T_b_eigen = T_b.matrix();
Eigen::Matrix4d dTdq_eigen = (T_b_eigen - T_a_eigen) / q_delta;
//Matrix4d dTdq_eigen = (T_b_eigen * T_a_eigen.inverse()) / dt;
// J(i)
Eigen::Matrix4d Ji_4x4matrix_eigen = Tinv_a_eigen * dTdq_eigen;
Eigen::Vector6d Ji;
Ji[0] = Ji_4x4matrix_eigen(2,1);
Ji[1] = Ji_4x4matrix_eigen(0,2);
Ji[2] = Ji_4x4matrix_eigen(1,0);
Ji[3] = Ji_4x4matrix_eigen(0,3);
Ji[4] = Ji_4x4matrix_eigen(1,3);
Ji[5] = Ji_4x4matrix_eigen(2,3);
numericJ.col(i) = Ji;
}
if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_joint))
{
// Skip this test for BallJoint and FreeJoint since Jacobian of BallJoint
// and FreeJoint is not obtained by the above method.
}
//.........这里部分代码省略.........