torque-free motion not giving correct output

[Naushad Khan]

Hi,

I want to simulate the torque-free motion of the axis-symmetric(Ix=Iy) body.
Analytical solution of angular velocity
Wx(t) = Wx(0) Cos(lt) - Wy(0)Sin(lt)
Wy(t) = Wx(0) Sin(lt) + Wy(0)Sin(lt)
Wz(t) = Wz(0)
Let's consider Ix = Iy =0.33 and Iz = 0.34 and rest is zero .
Wx(0) = 50 deg/sec
Wy(0) = 50 deg/sec
Wz(0) = 1
Output angular velocity in x and y should be sinusoidal with variation from 70.71 deg to -70.71 deg.



Please find the code to simulate the above.
But I cannot see the system is behaving. All three axis is constant, and there is a slight variation but not with 70.71 deg to -70.71 deg. Would you please help me to find out where I am doing wrong?
Let me know if you need any more information on it.

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

[Michael Sherman]

Hi, Naushad.

Please take a look at the DzhanibekovEffect example, which I think is similar to what you are trying to do. I suggest you run that one to see what it does. They you might want to modify that code to your specific case.

Regards,
Sherm

[Naushad Khan]

Dear Michael,

Thank you for the reply. Yes, the DzhanibekovEffect example and my "Torque free motion of an axisymmetric" is a similar condition.

I have copied the "DzhanibekovEffect example" and removed the "barBody" and just kept the shaftBody and its mass properties is
{ mass=1
com=~[0,0,0]
Uxx,yy,zz=~[0.0002,0.000933333,0.000933333]
Uxy,xz,yz=~[0,0,0]
}

So here Iyy = Izz ,

if we set the initial angular velocity as (0.1, 10 , 10 ) deg/sec then

Wx = 0.1 should stay constant where as
Wy and Wy should vary with +14.14 to -14.14 sinusoidal.
But I Cannot see this variation.

Question

1. Is this way is correct way to get angular velocity

Code: Select all

shaft.getBodyAngularVelocity(integ.getState())

2. I have even changed the accuracy setting to 1e-8?

Looking forward to your reply .

I am attaching code here

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

[Michael Sherman]

Hi, Naushad.

I found a few minor problems with your implementation:
- angular velocity in Simbody is given in radians/sec, not degrees/sec
- angular velocity for a body B is given and reported as w_GB_G, that is, the angular velocity of body B in Ground, expressed in Ground; what you are interested in is w_GB_B, that is, the angular velocity of body B in Ground, expressed in body B! The angular velocity about the long axis (x) is constant in the body frame, but not in Ground since the body is rotating wrt Ground.
- at a small angular velocity like 10 deg/s it takes a very long time for the +/- 14 deg/s cycle to occur.

I'll post some code and results shortly.

Sherm

[Michael Sherman]

Here is a plot of the three components of the angular velocity of the shaft, expressed in the shaft frame. I set the initial conditions to (10, 1000, 1000) degrees/s. The y & z velocities vary between +/- 1414.21 degrees/s.

plot.png (72.58 KiB) Viewed 2515 times

The modified code I used is here:

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

[Naushad Khan]

Thank you for the solution

[Michael Sherman]

You're welcome!