simtk.org

Code: Select all

int main() {
    try { // Create the system.

        std::ofstream MyFile("Torque_Free_motion.txt");
        MyFile << "time,ang_x,ang_y,ang_z" << "\n";
        MultibodySystem system;
        SimbodyMatterSubsystem matter(system);
        GeneralForceSubsystem forces(system);
        Force::UniformGravity gravity(forces, matter, Vec3(0, 0, 0));
        Force::DiscreteForces discreteForces(forces, matter);

        Body::Rigid pendulumBody(MassProperties(2.0, Vec3(0.0), Inertia(0.33,0.33,0.34)));


        pendulumBody.addDecoration(Transform(), DecorativeBrick(Vec3(0.1)));

        MobilizedBody masterMobod = matter.Ground();

        MobilizedBody::Free pendulum(masterMobod, Transform(Vec3(0, 0, 0)),
                                     pendulumBody, Transform(Vec3(0, 0, 0)));


        // Set up visualization.
        Visualizer viz(system);
        viz.setGroundHeight(0);
        viz.setShowShadows(true);
        viz.setBackgroundType(Visualizer::SolidColor); // default is white
        system.addEventReporter(new Visualizer::Reporter(viz, 1));



        // Initialize the system and state.
        State state =  system.realizeTopology();


        // Simulate it.
        VerletIntegrator integ(system);
        integ.setAccuracy(1e-6); // ask for *lots* of accuracy here (default is 1e-3)

        TimeStepper ts(system, integ);
        ts.initialize(state);

        // Adding initial angular velocity
        double ang_x = 50 * Pi / 180;
        double ang_y = 50 * Pi / 180;
        double ang_z = 1 * Pi / 180;
        pendulum.setUToFitAngularVelocity(integ.updAdvancedState(), Vec3(ang_x, ang_y, ang_z));
        system.realize(integ.getAdvancedState(), Stage::Acceleration);


        for (int i = 0; i < 1000; ++i) {

            auto angular_velocity = pendulum.getBodyAngularVelocity(integ.getAdvancedState());
            auto simTime = integ.getTime();

            MyFile<<simTime<<','<<180/Pi *angular_velocity[0] <<','<<180/Pi *angular_velocity[1]<<','<<180/Pi *angular_velocity[2]<<"\n";
            std::cout << "angular_velocity\t" << simTime << ',' << 180 / Pi * angular_velocity[0] << ','
                      << 180 / Pi * angular_velocity[1] << ',' << 180 / Pi * angular_velocity[2] << "\n";
            ts.stepTo(i);

        }
        MyFile.close();


    } catch (const std::exception &e) {
        std::printf("EXCEPTION THROWN: %s\n", e.what());
        exit(1);

    } catch (...) {
        std::printf("UNKNOWN EXCEPTION THROWN\n");
        exit(1);
    }

    return 0;
}

Code: Select all

shaft.getBodyAngularVelocity(integ.getState())

Code: Select all

#include "Simbody.h"
#include <iostream>

using namespace SimTK;

//==============================================================================
//                              SHOW ENERGY
//==============================================================================
// Generate text in the scene that displays the total energy, which should be
// conserved to roughly the number of decimal places corresponding to the
// accuracy setting (i.e., acc=1e-5 -> 5 digits).
class ShowEnergy : public DecorationGenerator {
public:
    explicit ShowEnergy(const MultibodySystem& mbs) : m_mbs(mbs) {}
    void generateDecorations(const State&                state,
                             Array_<DecorativeGeometry>& geometry) override;
private:
    const MultibodySystem& m_mbs;
};


//==============================================================================
//                                  MAIN
//==============================================================================
int main() {
    try {
        // Create the system.
        MultibodySystem system; system.setUpDirection(ZAxis);
        SimbodyMatterSubsystem matter(system);
        // No gravity or other forces

        matter.setShowDefaultGeometry(true); // turn off frames and other junk

        Rotation YtoX(-Pi/2, ZAxis);
        Body::Rigid shaftBody(MassProperties(1, Vec3(0),
                                             UnitInertia::cylinderAlongX(.02, .05)));
        shaftBody.addDecoration(YtoX,
                                DecorativeCylinder(.02, .05).setColor(Red));


        MobilizedBody::Free shaft(matter.Ground(), Transform(),
                                  shaftBody, Transform());


        // Visualize a frame every 1/60 s, and include the energy.
        Visualizer viz(system); viz.setDesiredFrameRate(60);
        viz.addDecorationGenerator(new ShowEnergy(system));
        system.addEventReporter(new Visualizer::Reporter(viz, 1./60));

        // Initialize the system and state.
        State state = system.realizeTopology();

        // Set initial conditions. Need a slight perturbation of angular velocity
        // to trigger the instability.
        shaft.setQToFitTranslation(state, Vec3(0,0,.5));
        shaft.setUToFitAngularVelocity(state, Vec3(0.1,10,10)); // 10 rad/s

        // Simulate it.
        RungeKuttaMersonIntegrator integ(system);
        integ.setAccuracy(1e-8);

        TimeStepper ts(system, integ);
        ts.initialize(state);
        std::cout << shaftBody.getDefaultRigidBodyMassProperties() << '\n';
        for (int i = 1; i < 1000; i = i +1) {

            const auto& angular_velocity = shaft.getBodyAngularVelocity(integ.getState());
            std::cout<<ts.getTime()<<'\t'<<angular_velocity<<'\n';
            ts.stepTo(float(i));

        }


    } catch(const std::exception& e) {
        std::cout << "EXCEPTION: " << e.what() << std::endl;
        return 1;
    }
    return 0;
}


void ShowEnergy::generateDecorations(const State&                state,
                                     Array_<DecorativeGeometry>& geometry)
{
    m_mbs.realize(state, Stage::Dynamics);
    const Real E=m_mbs.calcEnergy(state);
    DecorativeText energy;
    energy.setTransform(Vec3(-.2,0,.5))
            .setText("Energy: " + String(E, "%.6f"))
            .setScale(.09)
            .setColor(Black);
    geometry.push_back(energy);
}

Code: Select all

#include "Simbody.h"
#include <iostream>

using namespace SimTK;

//==============================================================================
//                              SHOW ENERGY
//==============================================================================
// Generate text in the scene that displays the total energy, which should be 
// conserved to roughly the number of decimal places corresponding to the 
// accuracy setting (i.e., acc=1e-5 -> 5 digits).
// Also write to cout the angular velocity w_GS_S for the shaft S.
class ShowEnergy : public DecorationGenerator {
public:
    explicit ShowEnergy(const MultibodySystem& mbs, 
                        const MobilizedBody::Free& shaft)
        : m_mbs(mbs), m_shaft(shaft) {}
    void generateDecorations(const State&                state, 
                             Array_<DecorativeGeometry>& geometry) override;
private:
    const MultibodySystem& m_mbs;
    const MobilizedBody::Free& m_shaft;
};


//==============================================================================
//                                  MAIN
//==============================================================================
int main() {
  try {    
    // Create the system.   
    MultibodySystem system; system.setUpDirection(ZAxis);
    SimbodyMatterSubsystem matter(system);
    // No gravity or other forces

    matter.setShowDefaultGeometry(false); // turn off frames and other junk

    // Construct a single rigid body, a cylindrical shaft.
    Rotation YtoX(-Pi/2, ZAxis);
    Body::Rigid shaftBody(MassProperties(1, Vec3(0),
                            UnitInertia::cylinderAlongX(.02, .05)));
    shaftBody.addDecoration(YtoX,
                            DecorativeCylinder(.02, .05).setColor(Red));

    MobilizedBody::Free shaft(matter.Ground(), Transform(),
                              shaftBody, Transform());

    // Visualize a frame every 1/30 s, and include the energy.
    Visualizer viz(system); viz.setDesiredFrameRate(30);
    viz.addDecorationGenerator(new ShowEnergy(system, shaft));
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
    
    // Initialize the system and state. 
    State state = system.realizeTopology();

    // Set initial conditions.
    shaft.setQToFitTranslation(state, Vec3(0,0,.5));
    shaft.setUToFitAngularVelocity(state, Pi/180 * Vec3(10, 1000, 1000)); // 1000 deg/s

    // Simulate it.
    RungeKuttaMersonIntegrator integ(system);
    integ.setAccuracy(1e-7);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(100.0);

  } catch(const std::exception& e) {
    std::cout << "EXCEPTION: " << e.what() << std::endl;
    return 1;
  }
    return 0;
}


void ShowEnergy::generateDecorations(const State&                state, 
                                     Array_<DecorativeGeometry>& geometry)
{
    m_mbs.realize(state, Stage::Dynamics);
    const Real E=m_mbs.calcEnergy(state);
    DecorativeText energy;
    energy.setTransform(Vec3(-.2,0,.5))
            .setText("Energy: " + String(E, "%.6f"))
            .setScale(.09)
            .setColor(Black);
    geometry.push_back(energy);

    // Shaft S angular velocity w_GS comes back expressed in G; we want to see it in S.
    const Rotation& R_GS = m_shaft.getBodyRotation(state);
    const auto& R_SG = ~R_GS; // Transpose is inverse.
    const auto w_GS_G = 180/Pi * m_shaft.getBodyAngularVelocity(state);
    const auto w_GS_S = R_SG * w_GS_G;
    // Print this out in an Octave or Matlab-friendly format for plotting.
    std::cout << state.getTime() << '\t' << w_GS_S[0] << ' ' << w_GS_S[1] << ' ' << w_GS_S[2] << '\n';
}