Skeleton * BVHParser::createSkeleton()
{
Skeleton * s = new Skeleton();
// set default pose...
Joint * b = createJoint(_root);
if( !s->setJoints(b) )
{
delete s;
return 0;
}
Pose * pose = new Pose(s->getNumJoints());
for(int i = 0; i < _linearNodes.size(); i++ )
{
BVHNode * n = _linearNodes[i];
pose->transforms[i].rotation.identity();
pose->transforms[i].position(0,0,0);
}
s->pose = pose;
s->init();
return s;
}
Skeleton* Mapper::newSkeleton(string id) {
Skeleton skeleton;
skeleton.init(id);
skeleton.setSmoothing(defaultSmoothing);
skeletons->push_back(skeleton);
return &skeletons->at(skeletons->size()-1);
}
void CollisionInterface::addRigidBody(RigidBody *_rb, const std::string& name) {
Skeleton *skel = new Skeleton();
BodyNode *bn = new BodyNode();
bn->setParentJoint( new dart::dynamics::FreeJoint("freeJoint") );
bn->addCollisionShape(_rb->mShape);
skel->addBodyNode( bn );
skel->setName( name );
skel->disableSelfCollision();
skel->init();
mCollisionChecker->addCollisionSkeletonNode(bn);
mNodeMap[bn] = _rb;
}
//==============================================================================
TEST(InverseKinematics, FittingVelocity)
{
const double TOLERANCE = 1e-4;
#ifdef BUILD_TYPE_RELEASE
const size_t numRandomTests = 100;
#else
const size_t numRandomTests = 10;
#endif
// Create two link robot
const double l1 = 1.5;
const double l2 = 1.0;
Skeleton* robot = createFreeFloatingTwoLinkRobot(
Vector3d(0.3, 0.3, l1),
Vector3d(0.3, 0.3, l2), DOF_ROLL);
robot->init();
BodyNode* body1 = robot->getBodyNode(0);
// BodyNode* body2 = robot->getBodyNode(1);
Joint* joint1 = body1->getParentJoint();
// Joint* joint2 = body2->getParentJoint();
//------------------------- Free joint test ----------------------------------
// The parent joint of body1 is free joint so body1 should be able to
// transform to arbitrary tramsformation.
for (size_t i = 0; i < numRandomTests; ++i)
{
// Test for linear velocity
Vector3d desiredVel = Vector3d::Random();
body1->fitWorldLinearVel(desiredVel);
Vector3d fittedLinVel = body1->getWorldVelocity().tail<3>();
Vector3d fittedAngVel = body1->getWorldVelocity().head<3>();
Vector3d diff = fittedLinVel - desiredVel;
EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE);
EXPECT_NEAR(fittedAngVel.dot(fittedAngVel), 0.0, TOLERANCE);
joint1->setGenVels(Vector6d::Zero(), true, true);
robot->setState(robot->getState(), true, true, false);
// Test for angular velocity
desiredVel = Vector3d::Random();
body1->fitWorldAngularVel(desiredVel);
fittedLinVel = body1->getWorldVelocity().tail<3>();
fittedAngVel = body1->getWorldVelocity().head<3>();
diff = fittedAngVel - desiredVel;
EXPECT_NEAR(fittedLinVel.dot(fittedLinVel), 0.0, TOLERANCE);
EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE);
joint1->setGenVels(Vector6d::Zero(), true, true);
robot->setState(robot->getState(), true, true, false);
}
}
Skeleton* createFreeFloatingTwoLinkRobot(Vector3d dim1,
Vector3d dim2, TypeOfDOF type2,
bool finished = true)
{
Skeleton* robot = new Skeleton();
// Create the first link, the joint with the ground and its shape
double mass = 1.0;
BodyNode* node = new BodyNode("link1");
Joint* joint = new FreeJoint();
joint->setName("joint1");
Shape* shape = new BoxShape(dim1);
node->setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
robot->addBodyNode(node);
// Create the second link, the joint with link1 and its shape
BodyNode* parent_node = robot->getBodyNode("link1");
node = new BodyNode("link2");
joint = create1DOFJoint(0.0, -DART_PI, DART_PI, type2);
joint->setName("joint2");
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(Eigen::Vector3d(0.0, 0.0, dim1(2)));
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(dim2);
node->setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
parent_node->addChildBodyNode(node);
robot->addBodyNode(node);
// If finished, initialize the skeleton
if(finished)
{
addEndEffector(robot, node, dim2);
robot->init();
}
return robot;
}
//==============================================================================
TEST(InverseKinematics, FittingTransformation)
{
const double TOLERANCE = 1e-6;
#ifdef BUILD_TYPE_RELEASE
const size_t numRandomTests = 100;
#else
const size_t numRandomTests = 10;
#endif
// Create two link robot
const double l1 = 1.5;
const double l2 = 1.0;
Skeleton* robot = createFreeFloatingTwoLinkRobot(
Vector3d(0.3, 0.3, l1),
Vector3d(0.3, 0.3, l2), DOF_ROLL);
robot->init();
size_t dof = robot->getNumGenCoords();
VectorXd oldConfig = robot->getConfigs();
BodyNode* body1 = robot->getBodyNode(0);
BodyNode* body2 = robot->getBodyNode(1);
// Joint* joint1 = body1->getParentJoint();
Joint* joint2 = body2->getParentJoint();
//------------------------- Free joint test ----------------------------------
// The parent joint of body1 is free joint so body1 should be able to
// transform to arbitrary tramsformation.
for (size_t i = 0; i < numRandomTests; ++i)
{
// Get desiredT2 by transforming body1 to arbitrary transformation
Isometry3d desiredT1 = math::expMap(Vector6d::Random());
body1->fitWorldTransform(desiredT1);
// Check
Isometry3d newT1 = body1->getWorldTransform();
EXPECT_NEAR(math::logMap(newT1.inverse() * desiredT1).norm(),
0.0, TOLERANCE);
// Set to initial configuration
robot->setConfigs(oldConfig, true, false, false);
}
//----------------------- Revolute joint test ---------------------------------
// The parent joint of body2 is revolute joint so body2 can rotate along the
// axis of the revolute joint.
for (size_t i = 0; i < numRandomTests; ++i)
{
// Store the original transformation and joint angle
Isometry3d oldT2 = body2->getWorldTransform();
VectorXd oldQ2 = joint2->getConfigs();
// Get desiredT2 by rotating the revolute joint with random angle
joint2->setConfigs(VectorXd::Random(1), true, false, false);
Isometry3d desiredT2 = body2->getWorldTransform();
// Transform body2 to the original transofrmation and check if it is done
// well
joint2->setConfigs(oldQ2, true, false, false);
EXPECT_NEAR(
math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(),
0.0, TOLERANCE);
// Try to find optimal joint angle
body2->fitWorldTransform(desiredT2);
// Check
Isometry3d newT2 = body2->getWorldTransform();
EXPECT_NEAR(math::logMap(newT2.inverse() * desiredT2).norm(),
0.0, TOLERANCE);
}
//---------------- Revolute joint test with joint limit ----------------------
for (size_t i = 0; i < numRandomTests; ++i)
{
// Set joint limit
joint2->getGenCoord(0)->setConfigMin(DART_RADIAN * 0.0);
joint2->getGenCoord(0)->setConfigMax(DART_RADIAN * 15.0);
// Store the original transformation and joint angle
Isometry3d oldT2 = body2->getWorldTransform();
VectorXd oldQ2 = joint2->getConfigs();
// Get desiredT2 by rotating the revolute joint with random angle out of
// the joint limit range
joint2->getGenCoord(0)->setConfig(math::random(DART_RADIAN * 15.5, DART_PI));
robot->setConfigs(robot->getConfigs(), true, false, false);
Isometry3d desiredT2 = body2->getWorldTransform();
// Transform body2 to the original transofrmation and check if it is done
// well
joint2->setConfigs(oldQ2, true, false, false);
EXPECT_NEAR(
math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(),
0.0, TOLERANCE);
// Try to find optimal joint angle without joint limit constraint
body2->fitWorldTransform(desiredT2, BodyNode::IKP_PARENT_JOINT, false);
// Check if the optimal body2 transformation is reached to the desired one
Isometry3d newT2 = body2->getWorldTransform();
EXPECT_NEAR(math::logMap(newT2.inverse() * desiredT2).norm(),
0.0, TOLERANCE);
// Try to find optimal joint angle with joint limit constraint
body2->fitWorldTransform(desiredT2, BodyNode::IKP_PARENT_JOINT, true);
// Check if the optimal joint anlge is in the range
double newQ2 = joint2->getGenCoord(0)->getConfig();
EXPECT_GE(newQ2, DART_RADIAN * 0.0);
EXPECT_LE(newQ2, DART_RADIAN * 15.0);
}
}
//==============================================================================
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";
}
}
}
}
