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;
}
/* ********************************************************************************************* */
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;
}
