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;
}
SkeletonPtr createGround()
{
SkeletonPtr ground = Skeleton::create("ground");
BodyNode* bn = ground->createJointAndBodyNodePair<WeldJoint>().second;
std::shared_ptr<BoxShape> shape = std::make_shared<BoxShape>(
Eigen::Vector3d(default_ground_width, default_ground_width,
default_wall_thickness));
shape->setColor(Eigen::Vector3d(1.0, 1.0, 1.0));
bn->addCollisionShape(shape);
bn->addVisualizationShape(shape);
return ground;
}
SkeletonPtr createWall()
{
SkeletonPtr wall = Skeleton::create("wall");
BodyNode* bn = wall->createJointAndBodyNodePair<WeldJoint>().second;
std::shared_ptr<BoxShape> shape = std::make_shared<BoxShape>(
Eigen::Vector3d(default_wall_thickness, default_ground_width,
default_wall_height));
shape->setColor(Eigen::Vector3d(0.8, 0.8, 0.8));
bn->addCollisionShape(shape);
bn->addVisualizationShape(shape);
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(
(default_ground_width + default_wall_thickness)/2.0, 0.0,
(default_wall_height - default_wall_thickness)/2.0);
bn->getParentJoint()->setTransformFromParentBodyNode(tf);
bn->setRestitutionCoeff(0.2);
return wall;
}
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;
}
/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
// Create Left Leg skeleton
Skeleton LeftLegSkel;
// Pointers to be used during the Skeleton building
Matrix3d inertiaMatrix;
inertiaMatrix << 0, 0, 0, 0, 0, 0, 0, 0, 0;
double mass = 1.0;
// ***** BodyNode 1: Left Hip Yaw (LHY) ***** *
BodyNode* node = new BodyNode("LHY");
Joint* joint = create1DOFJoint(NULL, node, 0.0, 0.0, DART_PI, DOF_YAW);
joint->setName("LHY");
Shape* shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
LeftLegSkel.addBodyNode(node);
// ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****\
BodyNode* parent_node = LeftLegSkel.getBodyNode("LHY");
node = new BodyNode("LHR");
joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
joint->setName("LHR");
Eigen::Isometry3d T(Eigen::Translation3d(0.0, 0.0, 0.5));
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
LeftLegSkel.addBodyNode(node);
// ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
parent_node = LeftLegSkel.getBodyNode("LHR");
node = new BodyNode("LHP");
joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
joint->setName("LHP");
T = Eigen::Translation3d(0.0, 0.0, 1.0);
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
Shape* shape1 = new EllipsoidShape(Vector3d(0.3, 0.3, 1.0));
shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
node->addVisualizationShape(shape1);
node->addCollisionShape(shape);
LeftLegSkel.addBodyNode(node);
// Initialize the skeleton
LeftLegSkel.initDynamics();
// Window stuff
MyWindow window(&LeftLegSkel);
glutInit(&argc, argv);
window.initWindow(640, 480, "Skeleton example");
glutMainLoop();
return 0;
}