//==============================================================================
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.
}
else
{
for (int i = 0; i < dof; ++i)
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(J.col(i)(j), numericJ.col(i)(j), JOINT_TOL);
}
//--------------------------------------------------------------------------
// Test first time derivative of analytic Jacobian and numerical Jacobian
// dJ == numerical_dJ
//--------------------------------------------------------------------------
Jacobian numeric_dJ = 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);
Jacobian J_a = _joint->getLocalJacobian();
// b
Eigen::VectorXd q_b = q;
q_b(i) += q_delta;
_joint->setPositions(q_b);
skeleton.computeForwardKinematics(true, false, false);
Jacobian J_b = _joint->getLocalJacobian();
//
Jacobian dJ_dq = (J_b - J_a) / q_delta;
// J(i)
numeric_dJ += dJ_dq * dq(i);
}
if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_joint))
{
// Skip this test for BallJoint and FreeJoint since time derivative of
// Jacobian of BallJoint and FreeJoint is not obtained by the above
// method.
}
else
{
for (int i = 0; i < dof; ++i)
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(dJ.col(i)(j), numeric_dJ.col(i)(j), JOINT_TOL);
}
}
// Forward kinematics test with high joint position
double posMin = -1e+64;
double posMax = +1e+64;
for (int idxTest = 0; idxTest < numTests; ++idxTest)
{
for (int i = 0; i < dof; ++i)
q(i) = random(posMin, posMax);
skeleton.setPositions(q);
skeleton.computeForwardKinematics(true, false, false);
if (_joint->getNumDofs() == 0)
return;
Eigen::Isometry3d T = _joint->getLocalTransform();
EXPECT_TRUE(math::verifyTransform(T));
}
}
//==============================================================================
TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)
{
// -- set up the root BodyNode
BodyNode* root = new BodyNode("root");
WeldJoint* rootjoint = new WeldJoint("base");
root->setParentJoint(rootjoint);
// -- set up the FreeJoint
BodyNode* freejoint_bn = new BodyNode("freejoint_bn");
FreeJoint* freejoint = new FreeJoint("freejoint");
freejoint_bn->setParentJoint(freejoint);
root->addChildBodyNode(freejoint_bn);
freejoint->setTransformFromParentBodyNode(random_transform());
freejoint->setTransformFromChildBodyNode(random_transform());
// -- set up the EulerJoint
BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn");
EulerJoint* eulerjoint = new EulerJoint("eulerjoint");
eulerjoint_bn->setParentJoint(eulerjoint);
root->addChildBodyNode(eulerjoint_bn);
eulerjoint->setTransformFromParentBodyNode(random_transform());
eulerjoint->setTransformFromChildBodyNode(random_transform());
// -- set up the BallJoint
BodyNode* balljoint_bn = new BodyNode("balljoint_bn");
BallJoint* balljoint = new BallJoint("balljoint");
balljoint_bn->setParentJoint(balljoint);
root->addChildBodyNode(balljoint_bn);
balljoint->setTransformFromParentBodyNode(random_transform());
balljoint->setTransformFromChildBodyNode(random_transform());
// -- set up Skeleton and compute forward kinematics
Skeleton* skel = new Skeleton;
skel->addBodyNode(root);
skel->addBodyNode(freejoint_bn);
skel->addBodyNode(eulerjoint_bn);
skel->addBodyNode(balljoint_bn);
skel->init();
// Test a hundred times
for(size_t n=0; n<100; ++n)
{
// -- convert transforms to positions and then positions back to transforms
Eigen::Isometry3d desired_freejoint_tf = random_transform();
freejoint->setPositions(FreeJoint::convertToPositions(desired_freejoint_tf));
Eigen::Isometry3d actual_freejoint_tf = FreeJoint::convertToTransform(
freejoint->getPositions());
Eigen::Isometry3d desired_eulerjoint_tf = random_transform();
desired_eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
eulerjoint->setPositions(
eulerjoint->convertToPositions(desired_eulerjoint_tf.linear()));
Eigen::Isometry3d actual_eulerjoint_tf = eulerjoint->convertToTransform(
eulerjoint->getPositions());
Eigen::Isometry3d desired_balljoint_tf = random_transform();
desired_balljoint_tf.translation() = Eigen::Vector3d::Zero();
balljoint->setPositions(
BallJoint::convertToPositions(desired_balljoint_tf.linear()));
Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform(
balljoint->getPositions());
skel->computeForwardKinematics(true, false, false);
// -- collect everything so we can cycle through the tests
std::vector<Joint*> joints;
std::vector<BodyNode*> bns;
std::vector<Eigen::Isometry3d> desired_tfs;
std::vector<Eigen::Isometry3d> actual_tfs;
joints.push_back(freejoint);
bns.push_back(freejoint_bn);
desired_tfs.push_back(desired_freejoint_tf);
actual_tfs.push_back(actual_freejoint_tf);
joints.push_back(eulerjoint);
bns.push_back(eulerjoint_bn);
desired_tfs.push_back(desired_eulerjoint_tf);
actual_tfs.push_back(actual_eulerjoint_tf);
joints.push_back(balljoint);
bns.push_back(balljoint_bn);
desired_tfs.push_back(desired_balljoint_tf);
actual_tfs.push_back(actual_balljoint_tf);
for(size_t i=0; i<joints.size(); ++i)
{
Joint* joint = joints[i];
BodyNode* bn = bns[i];
Eigen::Isometry3d tf = desired_tfs[i];
bool check_transform_conversion =
equals(predict_joint_transform(joint, tf).matrix(),
get_relative_transform(bn, bn->getParentBodyNode()).matrix());
EXPECT_TRUE(check_transform_conversion);
if(!check_transform_conversion)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
<< "\n\nActual:\n"
<< get_relative_transform(bn, bn->getParentBodyNode()).matrix()
<< "\n\n";
}
bool check_full_cycle = equals(desired_tfs[i].matrix(),
actual_tfs[i].matrix());
EXPECT_TRUE(check_full_cycle);
if(!check_full_cycle)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Desired:\n" << desired_tfs[i].matrix()
<< "\n\nActual:\n" << actual_tfs[i].matrix()
<< "\n\n";
}
}
}
}
