/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
// Create Left Leg skeleton
SkeletonDynamics 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) ***** *
BodyNodeDynamics* node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHY");
Joint* joint = new Joint(NULL, node, "LHY");
add_XyzRpy(joint, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_YAW);
Shape* shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
node->setMass(mass);
LeftLegSkel.addNode(node);
// ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****
BodyNodeDynamics* parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHY");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHR");
joint = new Joint(parent_node, node, "LHR");
add_XyzRpy(joint, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHR");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHP");
joint = new Joint(parent_node, node, "LHP");
add_XyzRpy(joint, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(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 ShapeEllipsoid(Vector3d(0.3, 0.3, 1.0));
shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setVisualizationShape(shape1);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// Initialize the skeleton
LeftLegSkel.initSkel();
// Window stuff
MyWindow window(&LeftLegSkel);
glutInit(&argc, argv);
window.initWindow(640, 480, "Skeleton example");
glutMainLoop();
return 0;
}
