static void threadFunc(union sigval sv) // thread notification function { ssize_t numRead; mqd_t *mqdp; void *buffer; struct mq_attr attr; mqdp = sv.sival_ptr; if (mq_getattr(*mqdp, &attr) == -1) { errExit("mq_getattr"); } buffer = malloc(attr.mq_msgsize); if (buffer == NULL) { errExit("malloc"); } notifySetup(mqdp); while ((numRead = mq_receive(*mqdp, buffer, attr.mq_msgsize, NULL)) >= 0) { printf("Read %ld bytes\n", (long)numRead); } if (errno != EAGAIN) { errExit("mq_receive"); } free(buffer); }
void TensegrityModel::setup(tgWorld& world) { // create the build spec that uses tags to turn the structure into a model tgBuildSpec spec; // add default rod and string builders that match the tags rods & strings // (these will be overwritten if a different builder is specified for those tags) Yam emptyYam = Yam(); addRodBuilder("tgRodInfo", "rod", emptyYam, spec); addBasicActuatorBuilder("tgBasicActuatorInfo", "string", emptyYam, spec); tgStructure structure; buildStructure(structure, topLvlStructurePath, spec); tgStructureInfo structureInfo(structure, spec); structureInfo.buildInto(*this, world); // use tgCast::filterto pull out the muscles that we want to control allActuators = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants()); // notify controllers that setup has finished notifySetup(); // actually setup the children tgModel::setup(world); }
static void /* Thread notification function */ threadFunc(union sigval sv) { ssize_t numRead; mqd_t *mqdp; void *buffer; struct mq_attr attr; mqdp = sv.sival_ptr; /* Determine mq_msgsize for message queue, and allocate an input buffer of that size */ if (mq_getattr(*mqdp, &attr) == -1) errExit("mq_getattr"); buffer = malloc(attr.mq_msgsize); if (buffer == NULL) errExit("malloc"); /* Reregister for message notification */ notifySetup(mqdp); while ((numRead = mq_receive(*mqdp, buffer, attr.mq_msgsize, NULL)) >= 0) printf("Read %ld bytes\n", (long) numRead); if (errno != EAGAIN) /* Unexpected error */ errExit("mq_receive"); free(buffer); }
static void threadFunc(union sigval sv) { mqd_t *mqdp; mqdp = sv.sival_ptr; notifySetup(mqdp); drainQueue(*mqdp); }
void Escape_T6Model::setup(tgWorld& world) { const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); /// @todo acceleration constraint was removed on 12/10/14 Replace with tgKinematicActuator as appropreate tgBasicActuator::Config activeMuscleConfig(c.stiffness, c.damping, c.hist, c.rotation, c.maxTens, c.targetVelocity); /* tgBasicActuator::Config passiveMuscleConfig(c.stiffness_passive, c.damping, c.hist, c.rotation, c.maxTens, c.targetVelocity, c.maxAcc);*/ // Start creating the structure tgStructure s; addNodes(s); addRods(s); addMuscles(s); s.move(btVector3(0, 10, 0)); // Add a rotation. This is needed if the ground slopes too much, // otherwise glitches put a rod below the ground. btVector3 rotationPoint = btVector3(0, 0, 0); // origin btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis double rotationAngle = M_PI/2; s.addRotation(rotationPoint, rotationAxis, rotationAngle); // Create the build spec that uses tags to turn the structure into a real model tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfig)); #if (0) spec.addBuilder("muscle", new tgBasicActuatorInfo(activeMuscleConfig)); #else spec.addBuilder("muscle", new tgBasicContactCableInfo(activeMuscleConfig)); #endif //spec.addBuilder("active muscle", new tgBasicActuatorInfo(activeMuscleConfig)); //spec.addBuilder("passive muscle", new tgBasicActuatorInfo(passiveMuscleConfig)); // Create your structureInfo tgStructureInfo structureInfo(s, spec); // Use the structureInfo to build ourselves structureInfo.buildInto(*this, world); // We could now use tgCast::filter or similar to pull out the // models (e.g. muscles) that we want to control. allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants()); // call the onSetup methods of all observed things e.g. controllers notifySetup(); // Actually setup the children tgModel::setup(world); }
void ScarrArmModel::setup(tgWorld& world) { const tgRod::Config rodConfig(cRod.radius, cRod.density, cRod.friction, cRod.rollFriction, cRod.restitution); const tgRod::Config rodConfigMassless(cRod.radius, 0.00/*c.density*/, cRod.friction, cRod.rollFriction, cRod.restitution); /// @todo acceleration constraint was removed on 12/10/14 Replace with tgKinematicActuator as appropreate tgBasicActuator::Config olecranonMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_olecranon, cCable.history, cCable.maxTens, cCable.targetVelocity); tgBasicActuator::Config anconeusMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_anconeus, cCable.history, cCable.maxTens, cCable.targetVelocity); tgBasicActuator::Config brachioradialisMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_brachioradialis, cCable.history, cCable.maxTens, cCable.targetVelocity); tgBasicActuator::Config supportstringMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_support, cCable.history, cCable.maxTens, cCable.targetVelocity); // Start creating the structure tgStructure s; addNodes(s); addRods(s); addMuscles(s); // Move the arm out of the ground btVector3 offset(0.0, 50.0, 0.0); s.move(offset); // Create the build spec that uses tags to turn the structure into a real model tgBuildSpec spec; spec.addBuilder("massless", new tgRodInfo(rodConfigMassless)); spec.addBuilder("rod", new tgRodInfo(rodConfig)); spec.addBuilder("olecranon muscle", new tgBasicActuatorInfo(olecranonMuscleConfig)); spec.addBuilder("anconeus muscle", new tgBasicActuatorInfo(anconeusMuscleConfig)); spec.addBuilder("brachioradialis muscle", new tgBasicActuatorInfo(brachioradialisMuscleConfig)); spec.addBuilder("support muscle", new tgBasicActuatorInfo(supportstringMuscleConfig)); // Create your structureInfo tgStructureInfo structureInfo(s, spec); // Use the structureInfo to build ourselves structureInfo.buildInto(*this, world); // We could now use tgCast::filter or similar to pull out the // models (e.g. muscles) that we want to control. allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants()); // call the onSetup methods of all observed things e.g. controllers notifySetup(); // Actually setup the children tgModel::setup(world); //map the rods and add the markers to them //addMarkers(s); }
static void /* Thread notification function */ threadFunc(union sigval sv) { mqd_t *mqdp; mqdp = sv.sival_ptr; /* Reregister for message notification */ notifySetup(mqdp); drainQueue(*mqdp); }
void T12SuperBallPayload::setup(tgWorld& world) { const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); /// @todo acceleration constraint was removed on 12/10/14 Replace with tgKinematicActuator as appropreate tgBasicActuator::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.history, c.maxTens, c.targetVelocity); // Start creating the structure tgStructure s; addNodes(s); addRods(s); addMuscles(s); // // Add a rotation. This is needed if the ground slopes too much, // // otherwise glitches put a rod below the ground. // btVector3 rotationPoint = btVector3(0, 0, 0); // origin // btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis // double rotationAngle = M_PI/2; // s.addRotation(rotationPoint, rotationAxis, rotationAngle); //s.move(btVector3(0,30,0)); // Create the build spec that uses tags to turn the structure into a real model tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfig)); spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig)); // Create your structureInfo tgStructureInfo structureInfo(s, spec); // Use the structureInfo to build ourselves structureInfo.buildInto(*this, world); // We could now use tgCast::filter or similar to pull out the // models (e.g. muscles) that we want to control. allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants()); // call the onSetup methods of all observed things e.g. controllers notifySetup(); // Actually setup the children tgModel::setup(world); //map the rods and add the markers to them addMarkers(s); btVector3 location(0,10.0,0); btVector3 rotation(0.0,0.6,0.8); btVector3 speed(0,20,100); this->moveModel(location,rotation,speed); }
int main(int argc, char *argv[]) { mqd_t mqd; if (argc != 2 || strcmp(argv[1], "--help") == 0) usageErr("%s mq-name\n", argv[0]); mqd = mq_open(argv[1], O_RDONLY | O_NONBLOCK); if (mqd == (mqd_t) -1) errExit("mq_open"); notifySetup(&mqd); pause(); /* Wait for notifications via thread function */ }
int main(int argc, char *argv[]) { mqd_t mqd; if (argc != 2 || strcmp(argv[1], "--help") == 0) usageErr("%s /mq-name\n", argv[0]); mqd = mq_open(argv[1], O_RDONLY | O_NONBLOCK); if (mqd == (mqd_t) -1) errExit("mq_open"); notifySetup(&mqd); drainQueue(mqd); pause(); }
int main(int argc, char *argv[]) { mqd_t mqd; if (argc != 2 || strcmp(argv[1], "--help") == 0) usageErr("%s /mq-name\n", argv[0]); mqd = mq_open(argv[1], O_RDONLY | O_NONBLOCK); if (mqd == (mqd_t) -1) errExit("mq_open"); notifySetup(&mqd); drainQueue(mqd); /* Handle possibility that messages were already queued before we established notification */ pause(); /* Wait for notifications via thread function */ }
int main(int argc, char *argv[]) { mqd_t mqd; if (argc != 2 || strcmp(argv[1], "--help") == 0) { usageErr("%s mq-name\n", argv[0]); } mqd = mq_open(argv[1], O_RDONLY | O_NONBLOCK); if (mqd == (mqd_t)-1) { errExit("mq_open"); } notifySetup(&mqd); pause(); // just pause forever and let the threadFunc() handler take care of all messages exit(EXIT_SUCCESS); }
void tgCraterShallow::setup(tgWorld& world) { const tgBox::Config boxConfig(c.width, c.height, c.density, c.friction, c.rollFriction, c.restitution); // Start creating the structure tgStructure s; addNodes(s); // Create the build spec that uses tags to turn the structure into a real model tgBuildSpec spec; spec.addBuilder("box", new tgBoxInfo(boxConfig)); // Create your structureInfo tgStructureInfo structureInfo(s, spec); // Use the structureInfo to build ourselves structureInfo.buildInto(*this, world); // call the onSetup methods of all observed things e.g. controllers notifySetup(); // Actually setup the children tgModel::setup(world); }
void hillyMuscleNP::setup(tgWorld& world) { const double rodDensity = 1; // Note: This needs to be high enough or things fly apart... const double rodRadius = 0.25; const tgRod::Config rodConfig(rodRadius, rodDensity); const tgRod::Config rodConfig2(rodRadius, 0.0); const tgBox::Config boxConfig(rodRadius, rodRadius, 0.0); tgStructure s; #if (0) s.addNode(-2, 2.1, 0); s.addNode(0, 2.1, 0); s.addNode(10, 2.1, 0); s.addNode(12, 2.1, 0); s.addNode(22, 2.1, 0); s.addNode(24, 2.1, 0); s.addPair(0, 1, "rod2"); s.addPair(2, 3, "rod2"); s.addPair(4, 5, "rod2"); s.addPair(1, 2, "muscle"); s.addPair(3, 4, "muscle"); s.move(btVector3(0, 0, 0)); #else s.addNode(0, 5, 2); s.addNode(0, 5, 0); s.addNode(20, 5, 2); s.addNode(20, 5, 0); s.addNode(5, 2, -5); s.addNode(5, 2, 5); s.addPair(0, 1, "rod"); s.addPair(2, 3, "rod"); s.addPair(0, 2, "muscle"); s.addPair(1, 3, "muscle"); //s.addPair(3, 4, "muscle"); s.move(btVector3(0, 0, 0)); #endif // Move the structure so it doesn't start in the ground s.move(btVector3(0, 0, 0)); //s.addRotation(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 1.0, 0.0), M_PI_2); tgSpringCableActuator::Config muscleConfig(1000, 10, 0.0, false, 600000000); // Create the build spec that uses tags to turn the structure into a real model tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfig)); spec.addBuilder("rod2", new tgRodInfo(rodConfig2)); spec.addBuilder("box", new tgBoxInfo(boxConfig)); spec.addBuilder("muscle", new tgBasicContactCableInfo(muscleConfig)); // Create your structureInfo tgStructureInfo structureInfo(s, spec); // Use the structureInfo to build ourselves structureInfo.buildInto(*this, world); // We could now use tgCast::filter or similar to pull out the // models (e.g. muscles) that we want to control. allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants()); allRods = tgCast::filter<tgModel, tgRod> (getDescendants()); notifySetup(); totalTime = 0.0; tgModel::setup(world); }
void BigPuppy::setup(tgWorld& world) { //Rod and Muscle configuration. Todo: make these into structs in a namespace block! const double density = 4.2/300.0; //Note: this needs to be high enough or things fly apart... const double radius = 0.5; const double rod_space = 10.0; const double friction = 0.5; const double rollFriction = 0.0; const double restitution = 0.0; const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution); const double radius2 = 0.15; const double density2 = 1; // Note: This needs to be high enough or things fly apart... const tgRod::Config rodConfig2(radius2, density2); const double stiffness = 1000.0; const double damping = .01*stiffness; const double pretension = 0.0; const bool history = false; const double maxTens = 7000.0; const double maxSpeed = 12.0; const double passivePretension = 700; // 5 N #ifdef USE_KINEMATIC const double mRad = 1.0; const double motorFriction = 10.0; const double motorInertia = 1.0; const bool backDrivable = false; tgKinematicActuator::Config motorConfig(2000, 20, passivePretension, mRad, motorFriction, motorInertia, backDrivable, history, maxTens, maxSpeed); #else const tgSpringCableActuator::Config stringConfig(stiffness, damping, pretension, false, 7000, 24); tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension); #endif // Calculations for the flemons spine model double v_size = 10.0; //Todo: make separate functions for node, rod, and muscle placement! Do for each type of segment. //Foot: tgStructure foot; //Foot nodes. Todo: make into separate function foot.addNode(8,0,8);//0 foot.addNode(8,0,-8);//1 foot.addNode(-8,0,-8);//2 foot.addNode(-8,0,8);//3 foot.addNode(4,rod_space/2,0);//4 foot.addNode(0,rod_space/2,-4);//5 foot.addNode(-4,rod_space/2,0);//6 foot.addNode(0,rod_space/2,4);//7 //Foot rods. Todo: make into separate function foot.addPair(0,6,"rod"); foot.addPair(1,7,"rod"); foot.addPair(2,4,"rod"); foot.addPair(3,5,"rod"); //Create basic unit for right leg. Todo: make just one basic unit for right and left legs, since they're now the same. tgStructure rightLeg; //Right Leg nodes: rightLeg.addNode(0,0,0); //0: Bottom Center of lower leg segment rightLeg.addNode(0,10,0); //1: Center of lower leg segment rightLeg.addNode(10,10,0); //2: Right of lower leg segment rightLeg.addNode(-10,10,0); //3: Left of lower leg segment rightLeg.addNode(0,20,0); //4: Top of lower leg segment rightLeg.addNode(0,-4,0); //5: was z=3; was y=-3 //rightLeg.addNode(0,-3,-3); //6 //rightLeg.addNode(3,-3,0); //7 //rightLeg.addNode(-3,-3,0); //8 //Add rods for right leg: rightLeg.addPair(0,1,"rod"); rightLeg.addPair(1,2,"rod"); rightLeg.addPair(1,3,"rod"); rightLeg.addPair(1,4,"rod"); rightLeg.addPair(0,5,"rod"); //rightLeg.addPair(0,6,"rod"); //rightLeg.addPair(0,7,"rod"); //rightLeg.addPair(0,8,"rod"); //Create basic unit for left leg tgStructure leftLeg; //Left Leg nodes: leftLeg.addNode(0,0,0); //0: Bottom Center of lower leg segment leftLeg.addNode(0,10,0); //1: Center of lower leg segment leftLeg.addNode(10,10,0); //2: Right of lower leg segment leftLeg.addNode(-10,10,0); //3: Left of lower leg segment leftLeg.addNode(0,20,0); //4: Top of lower leg segment leftLeg.addNode(0,-4,0); //5: was z=3; was y=-3 //leftLeg.addNode(0,-3,-3); //6 //leftLeg.addNode(3,-3,0); //7 //leftLeg.addNode(-3,-3,0); //8 //Add rods for left leg: leftLeg.addPair(0,1,"rod"); leftLeg.addPair(1,2,"rod"); leftLeg.addPair(1,3,"rod"); leftLeg.addPair(1,4,"rod"); leftLeg.addPair(0,5,"rod"); //leftLeg.addPair(0,6,"rod"); //leftLeg.addPair(0,7,"rod"); //leftLeg.addPair(0,8,"rod"); //Create the basic unit of the spine tgStructure tetra; //Add the nodes tetra.addNode(0,0,0); //Node 0 tetra.addNode(v_size, 0, v_size); //Node 1 tetra.addNode(v_size, 0, -v_size); //Node 2 tetra.addNode(-v_size, 0, -v_size); //Node 3 tetra.addNode(-v_size, 0, v_size); //Node 4 tetra.addPair(0,1,"rod"); tetra.addPair(0,2,"rod"); tetra.addPair(0,3,"rod"); tetra.addPair(0,4,"rod"); //Create the basic unit for the hips/shoulders: tgStructure lHip; lHip.addNode(0,0,0); //Node 0 lHip.addNode(0, v_size, v_size); //Node 1 lHip.addNode(0, -v_size, -v_size); //Node 2 lHip.addNode(0, -v_size, v_size); //Node 3 lHip.addPair(0,1,"rod"); lHip.addPair(0,2,"rod"); lHip.addPair(0,3,"rod"); tgStructure rHip; rHip.addNode(0,0,0); //Node 0 rHip.addNode(0, v_size, -v_size); //Node 1 rHip.addNode(0, -v_size, -v_size); //Node 2 rHip.addNode(0, -v_size, v_size); //Node 3 rHip.addPair(0,1,"rod"); rHip.addPair(0,2,"rod"); rHip.addPair(0,3,"rod"); //Build the spine tgStructure spine; const double offsetDist = v_size + 1; //So rod ends don't touch, may need to adjust const double offsetDist2 = v_size*5 + 5 + 3.3; const double offsetDist3 = v_size*6; const double yOffset_leg = -21.0; const double yOffset_foot = -26.0; std::size_t m_segments = 6; std::size_t m_hips = 4; std::size_t m_legs = 4; std::size_t m_feet = 4; //Vertebrae btVector3 offset(offsetDist,0.0,0); //Hips btVector3 offset1(offsetDist*2,0.0,offsetDist); btVector3 offset2(offsetDist2,0.0,offsetDist); btVector3 offset3(offsetDist*2,0.0,-offsetDist); btVector3 offset4(offsetDist2,0.0,-offsetDist); //Lower legs btVector3 offset5(offsetDist3,yOffset_leg,offsetDist); btVector3 offset6(offsetDist3,yOffset_leg,-offsetDist); btVector3 offset7(v_size*2,yOffset_leg,offsetDist); btVector3 offset8(v_size*2,yOffset_leg,-offsetDist); //Feet btVector3 offset9(offsetDist3+1,yOffset_foot,offsetDist); btVector3 offset10(offsetDist3+1,yOffset_foot,-offsetDist); btVector3 offset11(v_size*2+1,yOffset_foot,offsetDist); btVector3 offset12(v_size*2+1,yOffset_foot,-offsetDist); for(std::size_t i = 0; i < m_segments; i++) { //Connect segments for spine tgStructure* t = new tgStructure (tetra); t->addTags(tgString("segment num", i + 1)); t->move((i + 1)*offset); if (i % 2 == 1) { t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), 0.0); } else { t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), M_PI/2.0); } spine.addChild(t); //Add a segment to the spine } for(std::size_t i = m_segments; i < (m_segments + 2); i++) {//deal with right hip and shoulder first tgStructure* t = new tgStructure (rHip); t->addTags(tgString("segment num", i + 1)); if(i % 2 == 0) { t->move(offset1); t->addRotation(btVector3(offsetDist*2, 0.0, offsetDist), btVector3(1, 0, 0), 0.0); } else { t->move(offset2); t->addRotation(btVector3(offsetDist2, 0.0, offsetDist), btVector3(0, 0, 1), M_PI*1/8); } spine.addChild(t); //Add a segment to the spine } for(std::size_t i = (m_segments + 2); i < (m_segments + m_hips); i++) {//deal with left hip and shoulder now tgStructure* t = new tgStructure (lHip); t->addTags(tgString("segment num", i + 1)); if(i % 2 == 0) { t->move(offset3); t->addRotation(btVector3(offsetDist*2, 0.0, -offsetDist), btVector3(1, 0, 0), 0.0); } else { t->move(offset4); t->addRotation(btVector3(offsetDist2, 0.0, -offsetDist), btVector3(0, 0, 1), M_PI*1/8); } spine.addChild(t); //Add a segment to the spine } for(std::size_t i = (m_segments + m_hips); i < (m_segments + m_hips + 2); i++) {//right front and back legs tgStructure* t = new tgStructure (rightLeg); t->addTags(tgString("segment num", i + 1)); if(i % 2 == 0) { t->move(offset7); t->addRotation(btVector3(v_size*2, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI); } else { t->move(offset5); t->addRotation(btVector3(offsetDist3, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI); } spine.addChild(t); //Add a segment to the spine } for(std::size_t i = (m_segments + m_hips + 2); i < (m_segments + m_hips + m_legs); i++) {//left front and back legs tgStructure* t = new tgStructure (leftLeg); t->addTags(tgString("segment num", i + 1)); if(i % 2 == 0) { t->move(offset8); t->addRotation(btVector3(v_size*2, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI); } else { t->move(offset6); t->addRotation(btVector3(offsetDist3, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI); } spine.addChild(t); //Add a segment to the spine } for(std::size_t i = (m_segments + m_hips + m_legs); i < (m_segments + m_hips + m_legs + 2); i++) {//right front and back feet tgStructure* t = new tgStructure (foot); t->addTags(tgString("segment num", i + 1)); if(i % 2 == 0) { t->move(offset11); t->addRotation(btVector3(v_size*2+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0); } else { t->move(offset9); t->addRotation(btVector3(offsetDist3+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0); } spine.addChild(t); //Add a segment to the spine } for(std::size_t i = (m_segments + m_hips + m_legs + 2); i < (m_segments + m_hips + m_legs + m_feet); i++) {//left front and back feet tgStructure* t = new tgStructure (foot); t->addTags(tgString("segment num", i + 1)); if(i % 2 == 0) { t->move(offset12); t->addRotation(btVector3(v_size*2+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0); } else { t->move(offset10); t->addRotation(btVector3(offsetDist3+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0); } spine.addChild(t); //Add a segment to the spine } #ifdef SMALL_HILLS spine.move(btVector3(0.0,-yOffset_foot+5,0.0)); #endif #ifdef LARGE_HILLS spine.move(btVector3(0.0,-yOffset_foot+12,0.0)); #endif #ifdef FLAT_GROUND spine.move(btVector3(0.0,-yOffset_foot,0.0)); #endif #ifdef BLOCKY_GROUND spine.move(btVector3(0.0,10.0,0.0)); #endif #ifdef STAIRS spine.move(btVector3(0.0,0.0,0.0)); #endif std::vector<tgStructure*> children = spine.getChildren(); for(std::size_t i = 2; i < (children.size() - (m_hips + m_legs + m_feet)); i++) { tgNodes n0 = children[i-2]->getNodes(); tgNodes n1 = children[i-1]->getNodes(); tgNodes n2 = children[i]->getNodes(); if(i==2) { //Extra muscles, to keep front vertebra from swinging. spine.addPair(n0[3], n1[3], tgString("spine front upper right muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[3], n1[4], tgString("spine front upper left muscle seg", i-2) + tgString(" seg", i-1)); } //Add muscles to the spine if(i < 3) { if(i % 2 == 0) { //front spine.addPair(n0[1], n1[3], tgString("spine front lower right muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[1], n1[4], tgString("spine front lower left muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[2], n1[3], tgString("spine front upper right muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[2], n1[4], tgString("spine front upper left muscle seg", i-2) + tgString(" seg", i-1)); } else { //rear spine.addPair(n0[1], n1[3], tgString("spine rear upper left muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[1], n1[4], tgString("spine rear lower left muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[2], n1[3], tgString("spine rear upper right muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[2], n1[4], tgString("spine rear lower right muscle seg", i-2) + tgString(" seg", i-1)); } } if(i < 6) { if(i % 2 == 0) { spine.addPair(n0[1], n2[4], tgString("spine bottom muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[2], n2[3], tgString("spine top muscle seg", i-2) + tgString(" seg", i-1)); } else { spine.addPair(n0[1], n2[4], tgString("spine lateral left muscle seg", i-2) + tgString(" seg", i-1)); spine.addPair(n0[2], n2[3], tgString("spine lateral right muscle seg", i-2) + tgString(" seg", i-1)); } } if(i > 0 && i < 5) { if(i % 2 == 0) { //rear spine.addPair(n1[1], n2[3], tgString("spine rear upper left muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[1], n2[4], tgString("spine rear lower left muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[3], tgString("spine rear upper right muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[4], tgString("spine rear lower right muscle seg", i-1) + tgString(" seg", i)); } else { //front spine.addPair(n1[1], n2[3], tgString("spine front lower right muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[1], n2[4], tgString("spine front lower left muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[3], tgString("spine front upper right muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[4], tgString("spine front upper left muscle seg", i-1) + tgString(" seg", i)); } } if(i == 5) { //rear spine.addPair(n1[1], n2[1], tgString("spine rear lower left muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[1], n2[2], tgString("spine rear lower right muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[1], tgString("spine rear upper left muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[2], tgString("spine rear upper right muscle seg", i-1) + tgString(" seg", i)); //front spine.addPair(n1[1], n2[3], tgString("spine front lower right muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[1], n2[4], tgString("spine front lower left muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[3], tgString("spine front upper right muscle seg", i-1) + tgString(" seg", i)); spine.addPair(n1[2], n2[4], tgString("spine front upper left muscle seg", i-1) + tgString(" seg", i)); } } //Now add muscles to hips.... tgNodes n0 = children[0]->getNodes(); tgNodes n1 = children[1]->getNodes(); tgNodes n2 = children[2]->getNodes(); tgNodes n3 = children[3]->getNodes(); tgNodes n4 = children[4]->getNodes(); tgNodes n5 = children[5]->getNodes(); tgNodes n6 = children[6]->getNodes(); tgNodes n7 = children[7]->getNodes(); tgNodes n8 = children[8]->getNodes(); tgNodes n9 = children[9]->getNodes(); tgNodes n10 = children[10]->getNodes(); tgNodes n11 = children[11]->getNodes(); tgNodes n12 = children[12]->getNodes(); tgNodes n13 = children[13]->getNodes(); //Left shoulder muscles spine.addPair(n6[1], n1[1], tgString("left shoulder rear upper muscle seg", 6) + tgString(" seg", 1)); spine.addPair(n6[1], n1[4], tgString("left shoulder front upper muscle seg", 6) + tgString(" seg", 1)); spine.addPair(n6[1], n0[2], tgString("left shoulder front top muscle seg", 6) + tgString(" seg", 0)); spine.addPair(n6[1], n2[3], tgString("left shoulder rear top muscle seg", 6) + tgString(" seg", 2)); spine.addPair(n6[2], n1[1], tgString("left shoulder rear lower muscle seg", 6) + tgString(" seg", 1)); spine.addPair(n6[2], n1[4], tgString("left shoulder front lower muscle seg", 6) + tgString(" seg", 1)); spine.addPair(n6[2], n0[1], tgString("left shoulder front bottom muscle seg", 6) + tgString(" seg", 0)); spine.addPair(n6[2], n2[4], tgString("left shoulder rear bottom muscle seg", 6) + tgString(" seg", 2)); //Extra muscles, to move left shoulder forward and back: spine.addPair(n6[0], n1[1], tgString("left shoulder rear mid muscle seg", 6) + tgString(" seg", 1)); spine.addPair(n6[0], n1[4], tgString("left shoulder front mid muscle seg", 6) + tgString(" seg", 1)); //Left hip muscles spine.addPair(n7[1], n5[1], tgString("left hip rear upper muscle seg", 7) + tgString(" seg", 5)); spine.addPair(n7[1], n5[4], tgString("left hip front upper muscle seg", 7) + tgString(" seg", 5)); spine.addPair(n7[1], n4[2], tgString("left hip rear top muscle seg", 7) + tgString(" seg", 4)); spine.addPair(n7[1], n4[3], tgString("left hip front top muscle seg", 7) + tgString(" seg", 4)); spine.addPair(n7[2], n5[1], tgString("left hip rear lower muscle seg", 7) + tgString(" seg", 5)); spine.addPair(n7[2], n5[4], tgString("left hip front lower muscle seg", 7) + tgString(" seg", 5)); spine.addPair(n7[2], n4[1], tgString("left hip bottom muscle seg", 7) + tgString(" seg", 4)); //Extra muscles, to move left hip forward and back: spine.addPair(n7[0], n3[1], tgString("left hip rear mid muscle seg", 7) + tgString(" seg", 3)); //could also be n3[3] spine.addPair(n7[0], n5[4], tgString("left hip front mid muscle seg", 7) + tgString(" seg", 5)); //Inter-hip connector muscle spine.addPair(n7[2], n9[3], tgString("inter-hip bottom muscle seg", 7) + tgString(" seg", 9)); //inter-hip bottom muscle //Right shoulder muscles spine.addPair(n8[1], n1[2], tgString("right shoulder rear upper muscle seg", 8) + tgString(" seg", 1)); spine.addPair(n8[1], n1[3], tgString("right shoulder front upper muscle seg", 8) + tgString(" seg", 1)); spine.addPair(n8[1], n0[2], tgString("right shoulder front top muscle seg", 8) + tgString(" seg", 0)); spine.addPair(n8[1], n2[3], tgString("right shoulder rear top muscle seg", 8) + tgString(" seg", 2)); spine.addPair(n8[3], n1[2], tgString("right shoulder rear lower muscle seg", 8) + tgString(" seg", 1)); spine.addPair(n8[3], n1[3], tgString("right shoulder front lower muscle seg", 8) + tgString(" seg", 1)); spine.addPair(n8[3], n0[1], tgString("right shoulder front bottom muscle seg", 8) + tgString(" seg", 0)); spine.addPair(n8[3], n2[4], tgString("right shoulder rear bottom muscle seg", 8) + tgString(" seg", 2)); //Extra muscles, to move right shoulder forward and back: spine.addPair(n8[0], n1[2], tgString("right shoulder rear mid muscle seg", 8) + tgString(" seg", 1)); spine.addPair(n8[0], n1[3], tgString("right shoulder front mid muscle seg", 8) + tgString(" seg", 1)); //Right hip muscles spine.addPair(n9[1], n5[2], tgString("right hip rear upper muscle seg", 9) + tgString(" seg", 5)); spine.addPair(n9[1], n5[3], tgString("right hip front upper muscle seg", 9) + tgString(" seg", 5)); spine.addPair(n9[1], n4[2], tgString("right hip rear top muscle seg", 9) + tgString(" seg", 4)); spine.addPair(n9[1], n4[3], tgString("right hip front top muscle seg", 9) + tgString(" seg", 4)); spine.addPair(n9[3], n5[2], tgString("right hip rear lower muscle seg", 9) + tgString(" seg", 5)); spine.addPair(n9[3], n5[3], tgString("right hip front lower muscle seg", 9) + tgString(" seg", 5)); spine.addPair(n9[3], n4[1], tgString("right hip bottom muscle seg", 9) + tgString(" seg", 4)); //Extra muscles, to move right hip forward and back: spine.addPair(n9[0], n3[2], tgString("right hip rear mid muscle seg", 9) + tgString(" seg", 3)); //could also be n3[3] spine.addPair(n9[0], n5[3], tgString("right hip front mid muscle seg", 9) + tgString(" seg", 5)); //Leg/hip connections: //Right front leg/shoulder spine.addPair(n10[4], n6[2], tgString("right outer bicep muscle seg", 10) + tgString(" seg", 6)); spine.addPair(n10[4], n6[3], tgString("right inner bicep muscle seg", 10) + tgString(" seg", 6)); spine.addPair(n10[4], n1[4], tgString("right front abdomen connection muscle seg", 10) + tgString(" seg", 1)); spine.addPair(n10[3], n6[2], tgString("right outer tricep muscle seg", 10) + tgString(" seg", 6)); spine.addPair(n10[3], n6[3], tgString("right inner tricep muscle seg", 10) + tgString(" seg", 6)); spine.addPair(n10[2], n6[2], tgString("right outer front tricep muscle seg", 10) + tgString(" seg", 6)); spine.addPair(n10[2], n6[3], tgString("right inner front tricep muscle seg", 10) + tgString(" seg", 6)); //Adding muscle to pull up on right front leg: spine.addPair(n10[4], n6[1], tgString("right mid bicep muscle seg", 10) + tgString(" seg", 6)); //Left front leg/shoulder spine.addPair(n12[4], n8[2], tgString("left inner bicep muscle seg", 12) + tgString(" seg", 8)); spine.addPair(n12[4], n8[3], tgString("left outer bicep muscle seg", 12) + tgString(" seg", 8)); spine.addPair(n12[4], n1[3], tgString("left front abdomen connection muscle seg", 12) + tgString(" seg", 1)); //Was n1[2] spine.addPair(n12[3], n8[2], tgString("left inner tricep muscle seg", 12) + tgString(" seg", 8)); spine.addPair(n12[3], n8[3], tgString("left outer tricep muscle seg", 12) + tgString(" seg", 8)); spine.addPair(n12[2], n8[2], tgString("left inner front tricep muscle seg", 12) + tgString(" seg", 8)); spine.addPair(n12[2], n8[3], tgString("left outer front tricep muscle seg", 12) + tgString(" seg", 8)); //Adding muscle to pull up on left front leg: spine.addPair(n12[4], n8[1], tgString("left mid bicep muscle seg", 12) + tgString(" seg", 8)); //Right rear leg/hip spine.addPair(n11[4], n7[2], tgString("right outer thigh muscle seg", 11) + tgString(" seg", 7)); spine.addPair(n11[4], n7[3], tgString("right inner thigh muscle seg", 11) + tgString(" seg", 7)); spine.addPair(n11[4], n3[4],tgString("right rear abdomen connection muscle seg", 11) + tgString(" seg", 3)); spine.addPair(n11[3], n5[1],tgString("right rear abdomen connection muscle seg", 11) + tgString(" seg", 5)); spine.addPair(n11[3], n7[2], tgString("right outer calf muscle seg", 11) + tgString(" seg", 7)); spine.addPair(n11[3], n7[3], tgString("right inner calf muscle seg", 11) + tgString(" seg", 7)); spine.addPair(n11[2], n7[2], tgString("right outer front calf muscle seg", 11) + tgString(" seg", 7)); spine.addPair(n11[2], n7[3], tgString("right inner front calf muscle seg", 11) + tgString(" seg", 7)); //Adding muscle to pull rear right leg up: spine.addPair(n11[4], n7[1], tgString("right central thigh muscle seg", 11) + tgString(" seg", 7)); //Left rear leg/hip spine.addPair(n13[4], n9[2], tgString("left inner thigh muscle seg", 13) + tgString(" seg", 9)); spine.addPair(n13[4], n9[3], tgString("left outer thigh muscle seg", 13) + tgString(" seg", 9)); spine.addPair(n13[4], n3[3], tgString("left rear abdomen connection muscle seg", 13) + tgString(" seg", 3)); spine.addPair(n13[3], n5[2], tgString("left rear abdomen connection muscle seg", 13) + tgString(" seg", 5)); spine.addPair(n13[3], n9[2], tgString("left inner calf muscle seg", 13) + tgString(" seg", 9)); spine.addPair(n13[3], n9[3], tgString("left outer calf muscle seg", 13) + tgString(" seg", 9)); spine.addPair(n13[2], n9[2], tgString("left inner front calf muscle seg", 13) + tgString(" seg", 9)); spine.addPair(n13[2], n9[3], tgString("left outer front calf muscle seg", 13) + tgString(" seg", 9)); //Adding muscle to pull rear left leg up: spine.addPair(n13[4], n9[1], tgString("left central thigh muscle seg", 13) + tgString(" seg", 9)); //Populate feet with muscles. Todo: think up names to differentiate each! for(std::size_t i = (m_segments + m_hips + m_legs); i < children.size(); i++) { tgNodes ni = children[i]->getNodes(); tgNodes ni4 = children[i-4]->getNodes(); //Think of a nicer name for this! spine.addPair(ni[0],ni[1],tgString("foot muscle seg", i)); spine.addPair(ni[0],ni[3],tgString("foot muscle seg", i)); spine.addPair(ni[1],ni[2],tgString("foot muscle seg", i)); spine.addPair(ni[2],ni[3],tgString("foot muscle seg", i)); spine.addPair(ni[0],ni[7],tgString("foot muscle seg", i)); spine.addPair(ni[1],ni[4],tgString("foot muscle seg", i)); spine.addPair(ni[2],ni[5],tgString("foot muscle seg", i)); spine.addPair(ni[3],ni[6],tgString("foot muscle seg", i)); spine.addPair(ni[4],ni[5],tgString("foot muscle seg", i)); spine.addPair(ni[4],ni[7],tgString("foot muscle seg", i)); spine.addPair(ni[5],ni[6],tgString("foot muscle seg", i)); spine.addPair(ni[6],ni[7],tgString("foot muscle seg", i)); //Connecting feet to legs: //spine.addPair(ni4[5],ni[1],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[5],ni[2],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[5],ni[5],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[6],ni[0],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[6],ni[3],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[6],ni[7],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[7],ni[0],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[7],ni[1],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[7],ni[4],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[8],ni[2],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[8],ni[3],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //spine.addPair(ni4[8],ni[6],tgString("foot muscle seg", i) + tgString(" seg", i-4)); //Trying out these for foot: spine.addPair(ni4[5],ni[0],tgString("foot muscle seg", i) + tgString(" seg", i-4)); spine.addPair(ni4[5],ni[1],tgString("foot muscle seg", i) + tgString(" seg", i-4)); spine.addPair(ni4[5],ni[2],tgString("foot muscle seg", i) + tgString(" seg", i-4)); spine.addPair(ni4[5],ni[3],tgString("foot muscle seg", i) + tgString(" seg", i-4)); spine.addPair(ni4[0],ni[4],tgString("foot muscle seg", i) + tgString(" seg", i-4)); spine.addPair(ni4[0],ni[5],tgString("foot muscle seg", i) + tgString(" seg", i-4)); spine.addPair(ni4[0],ni[6],tgString("foot muscle seg", i) + tgString(" seg", i-4)); spine.addPair(ni4[0],ni[7],tgString("foot muscle seg", i) + tgString(" seg", i-4)); } //Don't forget muscles connecting hips to feet! // Create the build spec that uses tags to turn the structure into a real model tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfig)); #ifdef USE_KINEMATIC spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig)); #else spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig)); #endif // Create your structureInfo tgStructureInfo structureInfo(spine, spec); // Use the structureInfo to build ourselves structureInfo.buildInto(*this, world); // We could now use tgCast::filter or similar to pull out the // models (e.g. muscles) that we want to control. allActuators = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants()); // Notify controllers that setup has finished. notifySetup(); // Actually setup the children tgModel::setup(world); children.clear(); }
/*************************************** * The primary functions., called from other classes. **************************************/ void VerticalSpineModel::setup(tgWorld& world) { // debugging output: edge and height length //std::cout << "edge: " << c.edge << "; height: " << c.height << std::endl; // Create the first fixed snake segment // @todo move these hard-coded parameters into config tgStructure tetraB; addNodes(tetraB, c.edge, c.height); addPairsB(tetraB); tetraB.move(btVector3(0.0, 2, 0)); // Create our snake segments tgStructure snake; // add 1st child to snake tgStructure* const tB = new tgStructure(tetraB); snake.addChild(tB); tB->addTags(tgString("segment", 1)); // Create the first non-fixed tetrahedra tgStructure tetra; addNodes(tetra, c.edge, c.height); addPairs(tetra); // Move the first tetrahedra // @todo move these hard-coded parameters into config tetra.move(btVector3(0.0, -6, 0)); // add rest of segments using original tetra configuration addSegments(snake, tetra, c.edge, m_segments); addMuscles(snake); // Create the build spec that uses tags to turn the structure into a real model // Note: This needs to be high enough or things fly apart... // length of inner strut = 12.25 cm // m = 1 kg // volume of 1 rod = 9.62 cm^3 // total volume = 38.48 cm^3 //const double density = 1/38.48; = 0.026 // kg / length^3 - see app for length const tgRod::Config rodConfigA(c.radius, c.densityA, c.friction, c.rollFriction, c.restitution); const tgRod::Config rodConfigB(c.radius, c.densityB, c.friction, c.rollFriction, c.restitution); //holder const tgRod::Config rodConfigHA(0.1, c.densityA, c.friction, c.rollFriction, c.restitution); const tgRod::Config rodConfigHB(0.1, c.densityB, c.friction, c.rollFriction, c.restitution); tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfigA)); spec.addBuilder("rodB", new tgRodInfo(rodConfigB)); // set muscle (string) parameters // @todo replace acceleration constraint with tgKinematicActuator if needed... tgSpringCableActuator::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity); spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig)); // Create your structureInfo tgStructureInfo structureInfo(snake, spec); // Use the structureInfo to build ourselves structureInfo.buildInto(*this, world); // We could now use tgCast::filter or similar to pull out the models (e.g. muscles) // that we want to control. allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants()); mapMuscles(muscleMap, *this, m_segments); //trace(structureInfo, *this); // Actually setup the children notifySetup(); tgModel::setup(world); }