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;
}
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;
}
//==============================================================================
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));
}
}
int main(int argc, char* argv[]) {
using dart::dynamics::BodyNode;
using dart::dynamics::FreeJoint;
using dart::dynamics::MeshShape;
using dart::dynamics::Skeleton;
using dart::simulation::World;
using dart::utils::SkelParser;
// Create and initialize the world
World* myWorld = dart::utils::SkelParser::readSkelFile(
DART_DATA_PATH"/skel/mesh_collision.skel");
// Create a skeleton
Skeleton* MeshSkel = new Skeleton("Mesh Skeleton");
// Always set the root node ( 6DOF for rotation and translation )
FreeJoint* joint;
BodyNode* node;
// Set the initial Rootnode that controls the position and orientation of the
// whole robot
node = new BodyNode("rootBodyNode");
joint = new FreeJoint("rootJoint");
// Add joint to the body node
node->setParentJoint(joint);
// Load a Mesh3DTriangle to save in Shape
const aiScene* m3d = MeshShape::loadMesh(DART_DATA_PATH"/obj/foot.obj");
// Create Shape and assign it to node
MeshShape* Shape0 = new MeshShape(Eigen::Vector3d(1.0, 1.0, 1.0), m3d);
node->addVisualizationShape(Shape0);
node->addCollisionShape(Shape0);
node->setInertia(0.000416667, 0.000416667, 0.000416667);
node->setMass(1.0); // 1 Kg according to cube1.skel
// Add node to Skel
MeshSkel->addBodyNode(node);
// Add MeshSkel to the world
myWorld->addSkeleton(MeshSkel);
// Verify that our skeleton has something inside :)
std::printf("Our skeleton has %d nodes \n", MeshSkel->getNumBodyNodes());
// std::printf("Our skeleton has %d joints \n", MeshSkel->getNumJoints());
std::printf("Our skeleton has %d DOFs \n", MeshSkel->getNumGenCoords());
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'s': simulate one step" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1' and '2': programmed interaction" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "meshCollision");
glutMainLoop();
aiReleaseImport(m3d);
return 0;
}
//==============================================================================
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";
}
}
}
}