/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact 
 */
int main()
{

    try {
        // Create a new OpenSim model
        Model osimModel;
        osimModel.setName("osimModel");
        osimModel.setAuthors("Matt DeMers");

        double Pi = SimTK::Pi;
            
        // Get the ground body
        Ground& ground = osimModel.updGround();
        ground.addMeshGeometry("checkered_floor.vtp");

        // create linkage body
        double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
        
        Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
        Vec3 linkageMassCenter(0,linkageLength/2,0);
        Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);

        OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);
        
        // Graphical representation
        Cylinder cyl;
        cyl.set_scale_factors(linkageDimensions);
        Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl1Frame->setName("Cyl1_frame");
        osimModel.addFrame(cyl1Frame);
        cyl.setFrameName("Cyl1_frame");
        linkage1->addGeometry(cyl);

        Sphere sphere(0.1);
        linkage1->addGeometry(sphere);
         
        // Creat a second linkage body
        OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
        linkage2->setName("linkage2");
        Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl2Frame->setName("Cyl2_frame");
        osimModel.addFrame(cyl2Frame);
        (linkage2->upd_geometry(0)).setFrameName("Cyl2_frame");
        // Creat a block to be the pelvis
        double blockMass = 20.0, blockSideLength = 0.2;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
        Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
        block->addGeometry(brick);

        // Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block
        Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);

        PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1, 
            locationInChild, orientationInChild);

        PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
            locationInChild, orientationInChild);

        PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
            locationInChild, orientationInChild);
        
        double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
        CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
        ankleCoordinateSet[0].setName("q1");
        ankleCoordinateSet[0].setRange(range);

        CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
        kneeCoordinateSet[0].setName("q2");
        kneeCoordinateSet[0].setRange(range);

        CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
        hipCoordinateSet[0].setName("q3");
        hipCoordinateSet[0].setRange(range);

        // Add the bodies to the model
        osimModel.addBody(linkage1);
        osimModel.addBody(linkage2);
        osimModel.addBody(block);

        // Add the joints to the model
        osimModel.addJoint(ankle);
        osimModel.addJoint(knee);
        osimModel.addJoint(hip);
        // Define contraints on the model
        //  Add a point on line constraint to limit the block to vertical motion

        Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
        PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
        osimModel.addConstraint(lineConstraint);

        // Add PistonActuator between the first linkage and the block
        Vec3 pointOnBodies(0);
        PistonActuator *piston = new PistonActuator();
        piston->setName("piston");
        piston->setBodyA(linkage1);
        piston->setBodyB(block);
        piston->setPointA(pointOnBodies);
        piston->setPointB(pointOnBodies);
        piston->setOptimalForce(200.0);
        piston->setPointsAreGlobal(false);

        osimModel.addForce(piston);
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Added ControllableSpring between the first linkage and the second block
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        ControllableSpring *spring = new ControllableSpring;
        spring->setName("spring");
        spring->setBodyA(block);
        spring->setBodyB(linkage1);
        spring->setPointA(pointOnBodies);
        spring->setPointB(pointOnBodies);
        spring->setOptimalForce(2000.0);
        spring->setPointsAreGlobal(false);
        spring->setRestLength(0.8);

        osimModel.addForce(spring);

        // define the simulation times
        double t0(0.0), tf(15);

        // create a controller to control the piston and spring actuators
        // the prescribed controller sets the controls as functions of time
        PrescribedController *legController = new PrescribedController();
        // give the legController control over all (two) model actuators
        legController->setActuators(osimModel.updActuators());

        // specify some control nodes for spring stiffness control
        double t[] = {0.0, 4.0, 7.0,  10.0, 15.0};
        double x[] = {1.0, 1.0, 0.25,  0.25, 5.0};

        // specify the control function for each actuator
        legController->prescribeControlForActuator("piston", new Constant(0.1));
        legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x));

        // add the controller to the model
        osimModel.addController(legController);     
        
        // define the acceration due to gravity
        osimModel.setGravity(Vec3(0, -9.80665, 0));

        // enable the model visualizer see the model in action, which can be
        // useful for debugging
        osimModel.setUseVisualizer(false);

        // Initialize system
        SimTK::State& si = osimModel.initSystem();
        
        // Pin joint initial states
        double q1_i = -Pi/4;
        double q2_i = - 2*q1_i;
        CoordinateSet &coordinates = osimModel.updCoordinateSet();
        coordinates[0].setValue(si, q1_i, true);
        coordinates[1].setValue(si,q2_i, true);

        // Setup integrator and manager
        SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-3);

        ForceReporter *forces = new ForceReporter(&osimModel);  
        osimModel.updAnalysisSet().adoptAndAppend(forces);
        Manager manager(osimModel, integrator);
    
        //Examine the model
        osimModel.printDetailedInfo(si, std::cout);
        // Save the model
        osimModel.print("toyLeg.osim");
        // Print out the initial position and velocity states
        si.getQ().dump("Initial q's");
        si.getU().dump("Initial u's");
        std::cout << "Initial time: " << si.getTime() << std::endl;

        osimModel.dumpPathName();
        // Integrate
        manager.setInitialTime(t0);
        manager.setFinalTime(tf);
        std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl;
        manager.integrate(si);

        // Save results
        osimModel.printControlStorage("SpringActuatedLeg_controls.sto");
        Storage statesDegrees(manager.getStateStorage());
        osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
        //statesDegrees.print("PistonActuatedLeg_states_degrees.mot");
        statesDegrees.print("SpringActuatedLeg_states_degrees.mot");

        forces->getForceStorage().print("actuator_forces.mot");
        
    }
    catch (const std::exception& ex)
    {
        std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
        return 1;
    }

    std::cout << "Done." << std::endl;
    return 0;
}
Beispiel #2
0
/**
 * Create a model that does nothing.
 */
int main()
{
    try {

        ///////////////////////////////////////////
        // DEFINE BODIES AND JOINTS OF THE MODEL //
        ///////////////////////////////////////////

        // Create an OpenSim model and set its name
        Model osimModel;
        osimModel.setName("tugOfWar");

        // GROUND BODY

        // Get a reference to the model's ground body
        Ground& ground = osimModel.updGround();

        // Add display geometry to the ground to visualize in the GUI
        ground.addMeshGeometry("ground.vtp");
        ground.addMeshGeometry("anchor1.vtp");
        ground.addMeshGeometry("anchor2.vtp");

        // BLOCK BODY

        // Specify properties of a 20 kg, 0.1 m^3 block body
        double blockMass = 20.0, blockSideLength = 0.1;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);

        // Create a new block body with the specified properties
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);

        // Add display geometry to the block to visualize in the GUI
        Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
        block->addGeometry(brick);

        // FREE JOINT

        // Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies
        Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
        FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);

        // Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
        CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();

        // Set the angle and position ranges for the coordinate set
        double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
        double positionRange[2] = {-1, 1};
        jointCoordinateSet[0].setRange(angleRange);
        jointCoordinateSet[1].setRange(angleRange);
        jointCoordinateSet[2].setRange(angleRange);
        jointCoordinateSet[3].setRange(positionRange);
        jointCoordinateSet[4].setRange(positionRange);
        jointCoordinateSet[5].setRange(positionRange);

        // Add the block body to the model
        osimModel.addBody(block);
        osimModel.addJoint(blockToGround);

        ///////////////////////////////////////////////
        // DEFINE THE SIMULATION START AND END TIMES //
        ///////////////////////////////////////////////

        // Define the initial and final simulation times
        double initialTime = 0.0;
        double finalTime = 3.00;

        /////////////////////////////////////////////
        // DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
        /////////////////////////////////////////////

        // Specify properties of a constant distance constraint to limit the block's motion
        double distance = 0.2;
        Vec3 pointOnGround(0, blockSideLength/2 ,0);
        Vec3 pointOnBlock(0, 0, 0);

        // Create a new constant distance constraint
        ConstantDistanceConstraint *constDist = new ConstantDistanceConstraint(ground,
                pointOnGround, *block, pointOnBlock, distance);

        // Add the new point on a line constraint to the model
        osimModel.addConstraint(constDist);

        ///////////////////////////////////////
        // DEFINE FORCES ACTING ON THE MODEL //
        ///////////////////////////////////////

        // GRAVITY
        // Obtaine the default acceleration due to gravity
        Vec3 gravity = osimModel.getGravity();


        // MUSCLE FORCES
        // Create two new muscles with identical properties
        double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0;
        Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
        Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);

        // Specify the paths for the two muscles
        // Path for muscle 1
        muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35));
        muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0,0.0,-0.05));
        // Path for muscle 2
        muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35));
        muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0,0.0,0.05));

        // Add the two muscles (as forces) to the model
        osimModel.addForce(muscle1);
        osimModel.addForce(muscle2);


        // PRESCRIBED FORCE
        // Create a new prescribed force to be applied to the block
        PrescribedForce *prescribedForce = new PrescribedForce(block);
        prescribedForce->setName("prescribedForce");

        // Specify properties of the force function to be applied to the block
        double time[2] = {0, finalTime};                    // time nodes for linear function
        double fXofT[2] = {0,  -blockMass*gravity[1]*3.0};  // force values at t1 and t2

        // Create linear function for the force components
        PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT);
        // Set the force and point functions for the new prescribed force
        prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));
        prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0));

        // Add the new prescribed force to the model
        osimModel.addForce(prescribedForce);

        ///////////////////////////////////
        // DEFINE CONTROLS FOR THE MODEL //
        ///////////////////////////////////
        // Create a prescribed controller that simply applies controls as function of time
        // For muscles, controls are normalized motor-neuron excitations
        PrescribedController *muscleController = new PrescribedController();
        muscleController->setActuators(osimModel.updActuators());
        // Define linear functions for the control values for the two muscles
        Array<double> slopeAndIntercept1(0.0, 2);  // array of 2 doubles
        Array<double> slopeAndIntercept2(0.0, 2);
        // muscle1 control has slope of -1 starting 1 at t = 0
        slopeAndIntercept1[0] = -1.0/(finalTime-initialTime);
        slopeAndIntercept1[1] = 1.0;
        // muscle2 control has slope of 0.95 starting 0.05 at t = 0
        slopeAndIntercept2[0] = 0.95/(finalTime-initialTime);
        slopeAndIntercept2[1] = 0.05;

        // Set the indiviudal muscle control functions for the prescribed muscle controller
        muscleController->prescribeControlForActuator("muscle1", new LinearFunction(slopeAndIntercept1));
        muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2));

        // Add the muscle controller to the model
        osimModel.addController(muscleController);

        ///////////////////////////////////
        // SPECIFY MODEL DEFAULT STATES  //
        ///////////////////////////////////
        // Define the default states for the two muscles
        // Activation
        muscle1->setDefaultActivation(slopeAndIntercept1[1]);
        muscle2->setDefaultActivation(slopeAndIntercept2[1]);
        // Fiber length
        muscle2->setDefaultFiberLength(optimalFiberLength);
        muscle1->setDefaultFiberLength(optimalFiberLength);

        // Save the model to a file
        osimModel.print("tugOfWar_model.osim");

        //////////////////////////
        // PERFORM A SIMULATION //
        /////////////////////////

        //osimModel.setUseVisualizer(true);

        // Initialize the system and get the default state
        SimTK::State& si = osimModel.initSystem();

        // Define non-zero (defaults are 0) states for the free joint
        CoordinateSet& modelCoordinateSet = osimModel.updCoordinateSet();
        modelCoordinateSet[3].setValue(si, distance); // set x-translation value
        modelCoordinateSet[5].setValue(si, 0.0); // set z-translation value
        modelCoordinateSet[3].setSpeedValue(si, 0.0); // set x-speed value
        double h_start = 0.5;
        modelCoordinateSet[4].setValue(si, h_start); // set y-translation which is height

        std::cout << "Start height = "<< h_start << std::endl;

        osimModel.getMultibodySystem().realize(si, Stage::Velocity);

        // Compute initial conditions for muscles
        osimModel.equilibrateMuscles(si);

        double mfv1 = muscle1->getFiberVelocity(si);
        double mfv2 = muscle2->getFiberVelocity(si);

        // Create the force reporter for obtaining the forces applied to the model
        // during a forward simulation
        ForceReporter* reporter = new ForceReporter(&osimModel);
        osimModel.addAnalysis(reporter);

        // Create the integrator for integrating system dynamics
        SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-6);

        // Create the manager managing the forward integration and its outputs
        Manager manager(osimModel,  integrator);

        // Integrate from initial time to final time
        manager.setInitialTime(initialTime);
        manager.setFinalTime(finalTime);
        std::cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
        manager.integrate(si);

    }
    catch (const std::exception& ex)
    {
        std::cerr << ex.what() << std::endl;
        return 1;
    }
    catch (...)
    {
        std::cerr << "UNRECOGNIZED EXCEPTION" << std::endl;
        return 1;
    }

    std::cout << "OpenSim environment test completed successfully. You should see a block attached to two muscles visualized in a separate window." << std::endl;

    return 0;
}