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);
}
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);

}
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);
}
void NestedBoxTestModel::setup(tgWorld& world)
{
    const double edge = 30.0;
    const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
    std::cout << "edge: " << edge << "; height: " << height << std::endl;

    // Create the tetrahedra
    tgStructure tetra;
    addNodes(tetra, edge, height);
    addPairs(tetra);

    // Move the first one so we can create a longer snake.
    // Or you could move the snake at the end, up to you. 
    tetra.move(btVector3(0.0, 2.0, 100.0));

    // Create our snake segments
    tgStructure snake;
    addSegments(snake, tetra, 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...
    const double density = 4.2 / 3000.0; // kg / length^3 - see app for length
    const double radius = 0.5;
    const double h  = 0.5;
    const tgBox::Config rodConfig(radius, density);
    tgBuildSpec spec;
    spec.addBuilder("rod", new tgBoxInfo(rodConfig));
    
    tgSpringCableActuator::Config muscleConfig(1000, 10);
    //spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
    
    const tgSphere::Config sphereConfig(0.5, 0.5);
    spec.addBuilder("light", new tgSphereInfo(sphereConfig));
    
    const tgSphere::Config sphereConfig2(0.5, 2.5);
    spec.addBuilder("light", new tgSphereInfo(sphereConfig2));
    
    // 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);

    trace(structureInfo, *this);

    // Actually setup the children
    tgModel::setup(world);
}
Beispiel #6
0
EXPORT turnoutInfo_t * CreateNewStructure(
		char * scale,
		char * title,
		wIndex_t segCnt,
		trkSeg_p segData,
		BOOL_T updateList )
{
	turnoutInfo_t * to;
#ifdef REORIGSTRUCT
	coOrd orig;
#endif

	if (segCnt == 0)
		return NULL; 
	to = FindCompound( FIND_STRUCT, scale, title );
	if (to == NULL) {
		DYNARR_APPEND( turnoutInfo_t *, structureInfo_da, 10 );
		to = (turnoutInfo_t*)MyMalloc( sizeof *to );
		structureInfo(structureInfo_da.cnt-1) = to;
		to->title = MyStrdup( title );
		to->scaleInx = LookupScale( scale );
	}
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 BigDoxieNoFeet::setup(tgWorld& world)
{
    //Rod and Muscle configuration. 
    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 rod_space2 = 8.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 stiffness = 1000.0;
    const double stiffnessPassive = 5000.0;
    const double stiffnessPassive4 = 3000.0;
    const double damping = .01*stiffness;
    const double pretension = 0.0; //units of pretension is in Newtons.
    const bool   history = true;
    const double maxTens = 7000.0;
    const double maxSpeed = 12.0;

    const double passivePretension = 1000; 
    const double passivePretension2 = 2500;
    const double passivePretension3 = 13000; // was 2500
    const double passivePretension4 = 4000; 

#ifdef USE_KINEMATIC

    const double mRad = 1.0;
    const double motorFriction = 10.0;
    const double motorInertia = 1.0;
    const bool backDrivable = false;
    #ifdef PASSIVE_STRUCTURE
        tgKinematicActuator::Config motorConfigSpine(2*stiffness, damping, pretension,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);

	tgKinematicActuator::Config motorConfigStomach(3*stiffness, damping, passivePretension3,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);

	tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed); 

	tgKinematicActuator::Config motorConfigFeet(stiffnessPassive, damping, passivePretension,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed); 
	tgKinematicActuator::Config motorConfigLegs(stiffnessPassive4, damping, passivePretension4,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);
    #else
        tgKinematicActuator::Config motorConfigSpine(2*stiffness, damping, pretension,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed); 

	tgKinematicActuator::Config motorConfigStomach(3*stiffness, damping, passivePretension3,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);

	tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed); 

	tgKinematicActuator::Config motorConfigFeet(stiffnessPassive, damping, passivePretension,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed); 
	tgKinematicActuator::Config motorConfigLegs(stiffnessPassive4, damping, passivePretension4,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);
    #endif

#else
    
    #ifdef PASSIVE_STRUCTURE
        tgSpringCableActuator::Config muscleConfigSpine(2*stiffness, damping, passivePretension);
	tgSpringCableActuator::Config muscleConfigStomach(2*stiffnessPassive, damping, passivePretension3);
	tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2);
	tgSpringCableActuator::Config muscleConfigFeet(stiffnessPassive, damping, passivePretension); 
	tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive4, damping, passivePretension4);
    #else
        tgSpringCableActuator::Config muscleConfigSpine(2*stiffness, damping, pretension, history, maxTens, 2*maxSpeed);
	tgSpringCableActuator::Config muscleConfigStomach(3*stiffnessPassive, damping, passivePretension3, history, maxTens, 2*maxSpeed); 
	tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2);
	tgSpringCableActuator::Config muscleConfigFeet(stiffnessPassive, damping, passivePretension); 
	tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive4, damping, passivePretension4);
    #endif

#endif

    //Foot:
    tgStructure foot;
    addNodesFoot(foot,rod_space,rod_space2);
    addRodsFoot(foot);

    //Leg:
    tgStructure leg;
    addNodesLeg(leg,rod_space);
    addRodsLeg(leg);

    //Create the basic unit of the puppy
    tgStructure vertebra;
    addNodesVertebra(vertebra,rod_space);
    addRodsVertebra(vertebra);

    //Create the basic unit for the hips/shoulders. 
    tgStructure hip;
    addNodesHip(hip,rod_space);
    addRodsHip(hip);

    //Build the puppy
    tgStructure puppy;

    const double yOffset_foot = -(2*rod_space+6);

    addSegments(puppy,vertebra,hip,leg,foot,rod_space); //,m_segments,m_hips,m_legs,m_feet

    puppy.move(btVector3(0.0,-yOffset_foot,0.0));

    addMuscles(puppy); //,m_segments,m_hips,m_legs,m_feet

    //Time to add the muscles to the structure. Todo: make this a function; also try to clean this up.
    std::vector<tgStructure*> children = puppy.getChildren();
   
    // 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

    #ifdef PASSIVE_STRUCTURE
        spec.addBuilder("muscleAct1", new tgKinematicContactCableInfo(motorConfigSpine));
	spec.addBuilder("muscleAct2", new tgKinematicContactCableInfo(motorConfigStomach));
	spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
	spec.addBuilder("muscle2 ", new tgKinematicContactCableInfo(motorConfigFeet));
	spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs));
    #else 
	spec.addBuilder("muscleAct1", new tgKinematicContactCableInfo(motorConfigSpine));
	spec.addBuilder("muscleAct2", new tgKinematicContactCableInfo(motorConfigStomach));
	spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
	spec.addBuilder("muscle2 ", new tgKinematicContactCableInfo(motorConfigFeet));
	spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs));
	
    #endif

#else
    #ifdef PASSIVE_STRUCTURE
   	spec.addBuilder("muscleAct1", new tgBasicActuatorInfo(muscleConfigSpine));
	spec.addBuilder("muscleAct2", new tgBasicActuatorInfo(muscleConfigStomach));
	spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
	spec.addBuilder("muscle2 " , new tgBasicActuatorInfo(muscleConfigFeet));
	spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs));
    #else 
	spec.addBuilder("muscleAct1" , new tgBasicActuatorInfo(muscleConfigSpine));
	spec.addBuilder("muscleAct2" , new tgBasicActuatorInfo(muscleConfigStomach));
	spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
	spec.addBuilder("muscle2 " , new tgBasicActuatorInfo(muscleConfigFeet));
	spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs));
    #endif
    
#endif


    
    // Create your structureInfo
    tgStructureInfo structureInfo(puppy, 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. 
    m_allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());

    m_allSegments = this->find<tgModel> ("spine segment");
    
    // Actually setup the children, notify controller that the setup has finished
    BaseSpineModelLearning::setup(world);

    children.clear();
}
// This is basically a manual setup of a model.
// There are things that do this for us (@todo: reference the things that do this for us)
void TetraSpineGoal::setup(tgWorld& world)
{
    const double edge = 3.8 * scaleFactor;
    const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
    std::cout << "edge: " << edge << "; height: " << height << std::endl;
	
    // Create the tetrahedra
    tgStructure tetra;
    addNodes(tetra, edge, height, scaleFactor);
    addPairs(tetra);

    // Move the first one so we can create a longer snake.
    // Or you could move the snake at the end, up to you. 
    tetra.move(btVector3(0.0, 8.0, 10.0));

    // Create our snake segments
    tgStructure snake;
    addSegments(snake, tetra, -2.30 * scaleFactor, 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...
    

    // Params for In Won
    const double radius  = 0.635 * scaleFactor / 10.0;
    const double sphereRadius  = 0.635 * scaleFactor / (10.0);
    const double density = 2.0 *.0201 / (pow(radius, 2) * M_PI * edge); // Mass divided by volume... should there be a way to set this automatically??
    const double friction = 0.5;
    const tgRod::Config rodConfig(radius, density, friction);
    tgBuildSpec spec;
    spec.addBuilder("rod", new tgRodInfo(rodConfig));
    
    // 1000 is so the units below can be in grams
    const double sphereVolume1 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
    const double sphereVolume2 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
    
    const double baseCornerMidD = 180.0 / sphereVolume1;
    const tgSphere::Config baseCornerMidConfig(sphereRadius, baseCornerMidD, friction);
    spec.addBuilder("base", new tgSphereInfo(baseCornerMidConfig));

    const double tipCornerMidD = 120.0 / sphereVolume1;
    const tgSphere::Config tipCornerMidConfig(sphereRadius, tipCornerMidD, friction);
    spec.addBuilder("tip", new tgSphereInfo(tipCornerMidConfig));
    
    const double PCBD = 70.0 / sphereVolume2;
    const tgSphere::Config PCB_1_Config(radius, PCBD, friction);
    spec.addBuilder("PCB", new tgSphereInfo(PCB_1_Config));
    
    
        const double elasticity = 1000.0;
    const double damping = 10.0;
    const double pretension = 0.0;
    const bool   history = false;
    const double maxTens = 7000.0;
    const double maxSpeed = 12.0;

    const double mRad = 1.0;
    const double motorFriction = 10.0;
    const double motorInertia = 1.0;
    const bool backDrivable = false;
    
    tgKinematicActuator::Config motorConfig(elasticity, damping, pretension,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);
    
    spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig));

    // 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.    
    m_allMuscles = this->find<tgSpringCableActuator> ("muscle");
    m_allSegments = this->find<tgModel> ("segment");
    mapMuscles(m_muscleMap, *this);
    
    //addMarkers(snake, *this);
    
    #if (0)
    trace(structureInfo, *this);
    #endif
    
    // Actually setup the children
    BaseSpineModelGoal::setup(world);
}
void FlemonsSpineModelGoal::setup(tgWorld& world)
{
    // This is basically a manual setup of a model.
    // There are things that do this for us
    /// @todo: reference the things that do this for us

    // Rod and Muscle configuration
    // Note: This needs to be high enough or things fly apart...
    const double density = 4.2/300.0;
    const double radius  = 0.5;
    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 elasticity = 1000.0;
    const double damping = 10.0;
    const double pretension = 0.0;
    const bool   history = true;
    const double maxTens = 7000.0;
    const double maxSpeed = 12.0;

    const double mRad = 1.0;
    const double motorFriction = 10.0;
    const double motorInertia = 1.0;
    const bool backDrivable = false;
    tgKinematicActuator::Config motorConfig(elasticity, damping, pretension,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);
    
    // Calculations for the flemons spine model
    double v_size = 10.0;
    
    // Create the tetrahedra
    tgStructure tetra;

    tetra.addNode(0.0, 0.0, 0.0);  // center
    tetra.addNode( v_size,  v_size,  v_size); // front
    tetra.addNode( v_size, -v_size, -v_size); // right
    tetra.addNode(-v_size,  v_size, -v_size); // back
    tetra.addNode(-v_size, -v_size,  v_size); // left
    
    tetra.addPair(0, 1, "front rod");
    tetra.addPair(0, 2, "right rod");
    tetra.addPair(0, 3, "back rod");
    tetra.addPair(0, 4, "left rod");

    // Move the first one so we can create a longer snake.
    // Or you could move the snake at the end, up to you. 
    tetra.move(btVector3(0.0,15.0,50.0));

    // Create our snake segments
    tgStructure snake;
    /// @todo: there seems to be an issue with Muscle2P connections if the front
    /// of a tetra is inside the next one.
    btVector3 offset(0.0, 0.0, -v_size * 1.15);
    for (std::size_t i = 0; i < m_segments; i++)
    {
        /// @todo: the snake is a temporary variable -- 
        /// will its destructor be called?
        /// If not, where do we delete its children?
        tgStructure* const p = new tgStructure(tetra);
        p->addTags(tgString("segment num", i + 1));
        p->move((i + 1.0) * offset);
        snake.addChild(p); // Add a child to the snake
    }
    //conditionally compile for debugging 
#if (1)
    // Add muscles that connect the segments
    // Tag the muscles with their segment numbers so CPGs can find
    // them.
    std::vector<tgStructure*> children = snake.getChildren();
    for (std::size_t i = 1; i < children.size(); i++)
    {
        tgNodes n0 = children[i - 1]->getNodes();
        tgNodes n1 = children[i]->getNodes();
        
        snake.addPair(n0[1], n1[1],
              tgString("outer front muscle seg", i));
        snake.addPair(n0[2], n1[2],
              tgString("outer right muscle seg", i));
        snake.addPair(n0[3], n1[3],
              tgString("outer back muscle seg",  i));
        snake.addPair(n0[4], n1[4],
              tgString("outer top muscle seg",   i));
        
        snake.addPair(n0[2], n1[1],
              tgString("inner front muscle seg", i));
        snake.addPair(n0[2], n1[4],
              tgString("inner right muscle seg",  i));
        snake.addPair(n0[3], n1[1],
              tgString("inner left muscle seg",   i));
        snake.addPair(n0[3], n1[4],
              tgString("inner back muscle seg",   i));

    }
#endif
    
    btVector3 fixedPoint(0.0, 0.0, 0.0);
    btVector3 axis(0.0, 1.0, 0.0);
    
    snake.addRotation(fixedPoint, axis, m_startAngle);
    
    // Create the build spec that uses tags to turn the structure into a real model
    tgBuildSpec spec;
    spec.addBuilder("rod", new tgRodInfo(rodConfig));
    
#if (1)
    spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig));
#else    
    spec.addBuilder("muscle", new tgBasicContactCableInfo(motorConfig));
#endif
    
    // Create your structureInfo
    tgStructureInfo structureInfo(snake, spec);

    // Use the structureInfo to build ourselves
    structureInfo.buildInto(*this, world);
    
    // Setup vectors for control
    m_allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
     
    m_allSegments = this->find<tgModel> ("segment");
    
    //addMarkers(snake, *this, m_segments);
    
#if (0)
    // Debug printing
    std::cout << "StructureInfo:" << std::endl;
    std::cout << structureInfo << std::endl;
    
    std::cout << "Model: " << std::endl;
    std::cout << *this << std::endl;
    
    std::cout << "Spine Length: " << getSpineLength() << std::endl;
#endif

    std::vector<tgBaseRigid*> myRigids = this->getAllRigids();
#if (1)
		for (int i =0; i < myRigids.size(); i++)
		{
			std::cout << myRigids[i]->mass() << " " <<myRigids[i]->getPRigidBody() << std::endl;
		}
#endif

    children.clear();
    
    // Actually setup the children, notify controller
    BaseSpineModelGoal::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 RibModelMixedContact::setup(tgWorld& world)
{
    double v_size = 3.0;
    
    // Create the spinal processes
    tgStructure vertebrae;
    addNodes(vertebrae, v_size);
    addPairs(vertebrae);

    // Move the first one so we can create a longer snake.
    // Or you could move the snake at the end, up to you. 
    vertebrae.move(btVector3(0.0, 2 * v_size, v_size * m_segments));
    
    // Create ribs and add them to the vertebrae
    double majorAxis  = 6.0;
    double minorAxis  = 4.0;
    double startTheta = M_PI / 2.0;
    double endTheta   = 5.0 * M_PI / 4.0;
    size_t segs       = 15;
    
    tgStructure ribs;
    ellipseNodes(ribs, majorAxis, minorAxis, startTheta, endTheta, segs);
    makePairs(ribs);
    
    #if (0) // Attempt at compliant rib attachments
    ribs.move(btVector3(v_size / 3.0, 2 * v_size - minorAxis, v_size * m_segments));
    #else
    ribs.move(btVector3(0.0, 2 * v_size - minorAxis -.3, v_size * m_segments));
    #endif
    // Create our snake segments
    tgStructure snake;
    addSegments(snake, vertebrae, ribs, v_size, m_segments);
    
    snake.move(btVector3(0.0, majorAxis, 0.0));
    
    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...
    const double density = 4.2 / 300.0;
    const double radius  = 0.5;
    const double friction = 0.5; // Default is 0.5
    const double rollFriction  = 0.5; // Default is 0.0
    const double restitution  = 0.0; // Default
    
    const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
    tgBuildSpec spec;
    spec.addBuilder("rod", new tgRodInfo(rodConfig));  
    
    const double elasticity = 500.0;
    const double elasticityAct = 1000.0;
    const double damping = 5.0;
    const double dampingAct = 10.0;
    const double pretension = 0.0;
    const bool	 history = true;
    const double maxTens = 1000.0;
    const double maxTensAct = 7000.0;
    const double maxSpeed = 100.0;
    const double maxSpeedAct = 24.0;
    
    const double mRad = 1.0;
    const double motorFriction = 10.0;
    const double motorInertia = 1.0; 
    const bool backDrivable = false;
    

    #if (0) //Replacing with tgKinematicActuator, leaving option to turn it off. 9/9/15.
    tgKinematicActuator::Config muscleConfig(elasticity, damping, pretension,
                                            mRad, motorFriction, motorInertia, backDrivable,
                                            history, maxTens, maxSpeed);
    spec.addBuilder("muscle", new tgKinematicContactCableInfo(muscleConfig));

    tgKinematicActuator::Config muscleConfigAct(elasticityAct, dampingAct, pretension,
						mRad, motorFriction, motorInertia, backDrivable,
                                                history, maxTensAct, maxSpeedAct);
    spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(muscleConfigAct));
    #else
    tgSpringCableActuator::Config muscleConfig(elasticity, damping, pretension, history);
    spec.addBuilder("muscle", new tgBasicContactCableInfo(muscleConfig));
    /// @todo acceleration constraint was removed on 12/10/14 Replace with tgKinematicActuator as appropreate
    tgSpringCableActuator::Config muscleConfigAct(elasticityAct, dampingAct, pretension, history, 7000, 24);
    spec.addBuilder("muscleAct", new tgBasicContactCableInfo(muscleConfigAct));
    #endif
    
    #if (0) // Compliant Rib Attachments
    const double stiffness = 1000;
    const double damping = .01 * stiffness;
    
    tgSpringCableActuator::Config muscleConfig1(stiffness, damping, -M_PI / 2.0);
    tgSpringCableActuator::Config muscleConfig2(stiffness, damping, M_PI / 2.0);
    tgSpringCableActuator::Config muscleConfig3(stiffness, damping, M_PI);
    tgSpringCableActuator::Config muscleConfig4(stiffness, damping, 0);
    
    spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig1));
    spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig2));
    spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig3));
    spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig4));
    #endif
    // 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.    
    m_allMuscles = find<tgSpringCableActuator> ("muscleAct");
    m_allSegments = find<tgModel> ("segment");

    #if (0)
    trace(structureInfo, *this);
    #endif
   
    // Actually setup the children
    BaseSpineModelLearning::setup(world);
}
Beispiel #13
0
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();
}
// This is basically a manual setup of a model.
// There are things that do this for us (@todo: reference the things that do this for us)
void TetraSpineStaticModel_hf::setup(tgWorld& world)
{
    const double edge = 38;
    const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
    std::cout << "edge: " << edge << "; height: " << height << std::endl;

    // Create the tetrahedra
    tgStructure tetra;
    addNodes(tetra, edge, height);
    addPairs(tetra);

    // Move the first one so we can create a longer snake.
    // Or you could move the snake at the end, up to you.
    tetra.move(btVector3(0.0, 2.0, 100.0));

    // Create our snake segments
    tgStructure snake;
    addSegments(snake, tetra, 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...


    // Params for In Won
    const double oldDensity = .00311;
    const double radius  = 0.635 / 2.0;
    const double density = 0.0201 / (pow(radius, 2) * M_PI * edge); // Mass divided by volume... should there be a way to set this automatically??
    const double friction = 0.15;
    const tgRod::Config rodConfig(radius, density, friction);
    tgBuildSpec spec;
    spec.addBuilder("rod", new tgRodInfo(rodConfig));

    // 1000 is so the units below can be in grams
    const double sphereVolume = 1000.0 * 4.0 / 3.0 * M_PI * pow(radius, 3);

    const double baseCornerFrontD = 140.0 / sphereVolume;
    const tgSphere::Config baseCornerFrontConfig(radius, baseCornerFrontD, friction);
    spec.addBuilder("num0 base", new tgSphereInfo(baseCornerFrontConfig));

    const double baseCornerMidD = 180.0 / sphereVolume;
    const tgSphere::Config baseCornerMidConfig(radius, baseCornerMidD, friction);
    spec.addBuilder("num1 base", new tgSphereInfo(baseCornerMidConfig));

    const double baseCornerRearD = 100.0 / sphereVolume;
    const tgSphere::Config baseCornerRearConfig(radius, baseCornerRearD, friction);
    spec.addBuilder("num2 base", new tgSphereInfo(baseCornerRearConfig));

    const double tipCornerFrontD = 40.0 / sphereVolume;
    const tgSphere::Config tipCornerFrontConfig(radius, tipCornerFrontD, friction);
    spec.addBuilder("num0 tip", new tgSphereInfo(tipCornerFrontConfig));

    const double tipCornerMidD = 120.0 / sphereVolume;
    const tgSphere::Config tipCornerMidConfig(radius, tipCornerMidD, friction);
    spec.addBuilder("num1 tip", new tgSphereInfo(tipCornerMidConfig));
    spec.addBuilder("num2 tip", new tgSphereInfo(tipCornerMidConfig));

    const double PCBD = 70.0 / sphereVolume;
    const tgSphere::Config PCB_1_Config(radius, PCBD, friction);
    spec.addBuilder("PCB num0", new tgSphereInfo(PCB_1_Config));
    spec.addBuilder("PCB num1", new tgSphereInfo(PCB_1_Config));

    const double PCB2D = 95.0 / sphereVolume;
    const tgSphere::Config PCB_2_Config(radius, PCB2D, friction);
    spec.addBuilder("PCB num2", new tgSphereInfo(PCB_2_Config));

    // Two different string configs
    /// @todo acceleration constraint was removed on 12/10/14 Replace with tgKinematicActuator as appropreate
    tgSpringCableActuator::Config muscleConfig(229.16, 20, 2000.0, true, 5000, 7.0, 1.0, 1.0);
    tgSpringCableActuator::Config muscleConfig2(229.16, 20, 700.0, true, 5000, 7.0, 1.0, 1.0);
    spec.addBuilder("top muscle", new tgBasicActuatorInfo(muscleConfig));
    spec.addBuilder("left muscle", new tgBasicActuatorInfo(muscleConfig2));
    spec.addBuilder("right muscle", new tgBasicActuatorInfo(muscleConfig2));

    // 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.
    m_allMuscles = this->find<tgSpringCableActuator> ("muscle");
    m_allSegments = this->find<tgModel> ("segment");
    mapMuscles(m_muscleMap, *this);

    addMarkers(snake, *this);

#if (0)
    trace(structureInfo, *this);
#endif

    // Actually setup the children
    BaseSpineModelLearning::setup(world);
}
/***************************************
 * 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);
}