当前位置: 首页>>代码示例>>C++>>正文


C++ MultibodySystem::calcEnergy方法代码示例

本文整理汇总了C++中MultibodySystem::calcEnergy方法的典型用法代码示例。如果您正苦于以下问题:C++ MultibodySystem::calcEnergy方法的具体用法?C++ MultibodySystem::calcEnergy怎么用?C++ MultibodySystem::calcEnergy使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在MultibodySystem的用法示例。


在下文中一共展示了MultibodySystem::calcEnergy方法的12个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: testSpeedCoupler2

void testSpeedCoupler2() {
    
    // Create a system involving a constraint that affects three different 
    // bodies.
    
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    createGimbalSystem(system);
    MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
    std::vector<MobilizedBodyIndex> bodies(3);
    std::vector<MobilizerUIndex> speeds(3);
    bodies[0] = MobilizedBodyIndex(1);
    bodies[1] = MobilizedBodyIndex(3);
    bodies[2] = MobilizedBodyIndex(5);
    speeds[0] = MobilizerUIndex(0);
    speeds[1] = MobilizerUIndex(0);
    speeds[2] = MobilizerUIndex(1);
    Function* function = new CompoundFunction();
    Constraint::SpeedCoupler coupler(matter, function, bodies, speeds);
    State state;
    createState(system, state);
    
    // Make sure the constraint is satisfied.
    
    Vector args(function->getArgumentSize());
    for (int i = 0; i < args.size(); ++i)
        args[i] = matter.getMobilizedBody(bodies[i]).getOneU(state, speeds[i]);
    SimTK_TEST_EQ(0.0, function->calcValue(args));
    
    // Simulate it and make sure the constraint is working correctly and 
    // energy is being conserved. This should be workless and power should
    // always be zero (to the extent that the constraint is satisfied).
    
    Real energy0 = system.calcEnergy(state);
    RungeKuttaMersonIntegrator integ(system);
    integ.setAccuracy(1e-6);
    integ.setReturnEveryInternalStep(true);
    integ.initialize(state);
    while (integ.getTime() < 10.0) {
        integ.stepTo(10.0);
        const State& istate = integ.getState();
        system.realize(istate, Stage::Acceleration);
        const Real energy = system.calcEnergy(istate);
        const Real power = coupler.calcPower(istate);

        for (int i = 0; i < args.size(); ++i)
            args[i] = matter.getMobilizedBody(bodies[i]).getOneU(istate, speeds[i]);
        SimTK_TEST_EQ_TOL(0.0, function->calcValue(args), 
                          integ.getConstraintToleranceInUse());

        SimTK_TEST_EQ_TOL(0.0, power, 10*integ.getConstraintToleranceInUse());

        // Energy conservation depends on global integration accuracy;
        // accuracy returned here is local so we'll fudge at 10X.
        const Real etol = 10*integ.getAccuracyInUse()
                          *std::max(std::abs(energy), std::abs(energy0));        
        SimTK_TEST_EQ_TOL(energy0, energy, etol);
    }
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:59,代码来源:TestCustomConstraints.cpp

示例2: testCoordinateCoupler3

void testCoordinateCoupler3() {
    
    // Create a system involving a constrained body for which qdot != u.
    
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    createBallSystem(system);
    MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
    std::vector<MobilizedBodyIndex> bodies(3);
    std::vector<MobilizerQIndex> coordinates(3);
    bodies[0] = MobilizedBodyIndex(1);
    bodies[1] = MobilizedBodyIndex(1);
    bodies[2] = MobilizedBodyIndex(1);
    coordinates[0] = MobilizerQIndex(0);
    coordinates[1] = MobilizerQIndex(1);
    coordinates[2] = MobilizerQIndex(2);
    Function* function = new CompoundFunction();
    Constraint::CoordinateCoupler coupler(matter, function, bodies, coordinates);
    State state;
    createState(system, state);
    
    // Make sure the constraint is satisfied.
    
    Vector args(function->getArgumentSize());
    for (int i = 0; i < args.size(); ++i)
        args[i] = matter.getMobilizedBody(bodies[i]).getOneQ(state, coordinates[i]);
    SimTK_TEST_EQ(0.0, function->calcValue(args));
    
    // Simulate it and make sure the constraint is working correctly and 
    // energy is being conserved.
    
    const Real energy0 = system.calcEnergy(state);
    RungeKuttaMersonIntegrator integ(system);
    integ.setReturnEveryInternalStep(true);
    integ.initialize(state);
    while (integ.getTime() < 10.0) {
        integ.stepTo(10.0);
        const State& istate = integ.getState();
        const Real energy = system.calcEnergy(istate);

        for (int i = 0; i < args.size(); ++i)
            args[i] = matter.getMobilizedBody(bodies[i])
                            .getOneQ(integ.getState(), coordinates[i]);
        // Constraints are applied to unnormalized quaternions. When they are 
        // normalized, that can increase the constraint error. That is why we 
        // need the factor of 3 in the next line.
        // TODO: Huh? (sherm)
        SimTK_TEST_EQ_TOL(0.0, function->calcValue(args), 
                          3*integ.getConstraintToleranceInUse());
        
         // Energy conservation depends on global integration accuracy;
        // accuracy returned here is local so we'll fudge at 10X.
        const Real etol = 10*integ.getAccuracyInUse()
                          *std::max(std::abs(energy), std::abs(energy0));        
        SimTK_TEST_EQ_TOL(energy0, energy, etol);       
    }
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:57,代码来源:TestCustomConstraints.cpp

示例3: testPrescribedMotion1

void testPrescribedMotion1() {
    
    // Create a system requiring simple linear motion of one Q. This
    // may require that the constraint do work.
    // (The way the cylinder system is structured it only takes work to
    // keep body one at a uniform velocity; the rest are in free fall.)
    
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    createCylinderSystem(system);
    MobilizedBodyIndex body = MobilizedBodyIndex(1);
    MobilizerQIndex coordinate = MobilizerQIndex(1);
    Vector coefficients(2);
    coefficients[0] = 0.1;
    coefficients[1] = 0.0;
    Function* function = new Function::Linear(coefficients);
    Constraint::PrescribedMotion constraint(matter, function, body, coordinate);
    PowerMeasure<Real> powMeas(matter, constraint);
    Measure::Zero zeroMeas(matter);
    Measure::Integrate workMeas(matter, powMeas, zeroMeas);     
    
    State state;
    createState(system, state);
    workMeas.setValue(state, 0); // override createState
    
    // Make sure the constraint is satisfied.
    
    Vector args(1, state.getTime());
    SimTK_TEST_EQ(function->calcValue(args), 
                  matter.getMobilizedBody(body).getOneQ(state, coordinate));
    
    // Simulate it and make sure the constraint is working correctly.
    const Real energy0 = system.calcEnergy(state);   
    RungeKuttaMersonIntegrator integ(system);
    integ.setReturnEveryInternalStep(true);
    integ.initialize(state);
    while (integ.getTime() < 10.0) {
        integ.stepTo(10.0);
        const State& istate = integ.getState();
        system.realize(istate, Stage::Acceleration);
        const Real energy = system.calcEnergy(istate);
        const Real power = powMeas.getValue(istate);
        const Real work =  workMeas.getValue(istate);

        Vector args(1, istate.getTime());
        const Real q = matter.getMobilizedBody(body).getOneQ(istate, coordinate);
        SimTK_TEST_EQ_TOL(function->calcValue(args), q, 
                          integ.getConstraintToleranceInUse());

        // Energy conservation depends on global integration accuracy;
        // accuracy returned here is local so we'll fudge at 10X.
        const Real etol = 10*integ.getAccuracyInUse()
                          *std::max(std::abs(energy-work), std::abs(energy0));        
        SimTK_TEST_EQ_TOL(energy0, energy-work, etol)
    }
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:56,代码来源:TestCustomConstraints.cpp

示例4: main


//.........这里部分代码省略.........

    mbs.realize(s, Stage::Velocity);
    display.report(s);

    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;

    char c;
    cout << "Default configuration shown. 1234 to move on.\n";

    //anAtom.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));

    while (true) {
        Real x;
        cout << "Torsion (deg)? "; cin >> x; if (x==1234) break;
        Vec2 a = anAtom.getAngles(s); a[0]=x*Deg2Rad; anAtom.setAngles(s, a);
        display.report(s);
        cout << "Bend (deg)? "; cin >> x; if (x==1234) break;
        a = anAtom.getAngles(s); a[1]=x*Deg2Rad; anAtom.setAngles(s, a);
        display.report(s);
        cout << "Radius? "; cin >> x; if (x==1234) break;
        anAtom.setRadius(s, x);
        display.report(s);
    }
    anAtom.setUToFitAngularVelocity(s, Vec3(.1,.2,.3));

    //anAtom.setAngle(s, 45*Deg2Rad);
    //anAtom.setTranslation(s, Vec2(.4, .1));

    mbs.realize(s, Stage::Dynamics);
    mbs.realize(s, Stage::Acceleration);

    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;
    cout << "qdot=" << s.getQDot() << endl;
    cout << "udot=" << s.getUDot() << endl;
    cout << "qdotdot=" << s.getQDotDot() << endl;
    display.report(s);

    cout << "Initialized configuration shown. Ready? ";
    cin >> c;

    RungeKuttaMersonIntegrator myStudy(mbs);
    myStudy.setAccuracy(1e-4);

    const Real dt = .02; // output intervals
    const Real finalTime = 20;

    myStudy.setFinalTime(finalTime);

    // Peforms assembly if constraints are violated.
    myStudy.initialize(s);

    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    Integrator::SuccessfulStepStatus status;
    int nextReport = 0;
    while ((status=myStudy.stepTo(nextReport*dt))
           != Integrator::EndOfSimulation) 
    {
        const State& s = myStudy.getState();
        mbs.realize(s);
        printf("%5g %10.4g %10.4g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(), 
            anAtom.getAngles(s)[0], anAtom.getAngles(s)[1], anAtom.getRadius(s),
            //anAtom.getAngle(s), anAtom.getTranslation(s)[0], anAtom.getTranslation(s)[1],
            //anAtom.getQ(s)[0], anAtom.getQ(s)[1], anAtom.getQ(s)[2],
            mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
            myStudy.getPreviousStepSizeTaken(),
            Integrator::getSuccessfulStepStatusString(status).c_str(),
            myStudy.isStateInterpolated()?" (INTERP)":"");

        display.report(s);

        if (status == Integrator::ReachedReportTime)
            ++nextReport;
    }

    printf("Using Integrator %s:\n", myStudy.getMethodName());
    printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());

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

}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:101,代码来源:SphericalCoordsMobilizerTest.cpp

示例5: main


//.........这里部分代码省略.........
    //VerletIntegrator myStudy(mbs);
    //ExplicitEulerIntegrator myStudy(mbs);

    myStudy.setAccuracy(1e-2);
    myStudy.setConstraintTolerance(1e-3); 
    myStudy.setProjectEveryStep(false);

    Visualizer display(mbs);
    display.setBackgroundColor(White);
    display.setBackgroundType(Visualizer::SolidColor);
    display.setMode(Visualizer::RealTime);

    for (MobilizedBodyIndex i(1); i<myRNA.getNumBodies(); ++i)
        myRNA.decorateBody(i, display);
    myRNA.decorateGlobal(display);

    DecorativeLine rbProto; rbProto.setColor(Orange).setLineThickness(3);
    display.addRubberBandLine(GroundIndex, attachPt,MobilizedBodyIndex(myRNA.getNumBodies()-1),Vec3(0), rbProto);
    //display.addRubberBandLine(GroundIndex, -attachPt,myRNA.getNumBodies()-1,Vec3(0), rbProto);

    const Real dt = 1./30; // output intervals

    printf("time  nextStepSize\n");

    s.updTime() = 0;
    for (int i=0; i<50; ++i)
        saveEm.push_back(s);    // delay
    display.report(s);

    myStudy.initialize(s);
    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    saveEm.push_back(myStudy.getState());
    for (int i=0; i<50; ++i)
        saveEm.push_back(myStudy.getState());    // delay
    display.report(myStudy.getState());


    const double startReal = realTime(), startCPU = cpuTime();
    int stepNum = 0;
    for (;;) {
        const State& ss = myStudy.getState();

        mbs.realize(ss);

        if ((stepNum++%100)==0) {
            printf("%5g qerr=%10.4g uerr=%10.4g hNext=%g\n", ss.getTime(), 
                myRNA.getQErr(ss).normRMS(), myRNA.getUErr(ss).normRMS(),
                myStudy.getPredictedNextStepSize());
            printf("      E=%14.8g (pe=%10.4g ke=%10.4g)\n",
                mbs.calcEnergy(ss), mbs.calcPotentialEnergy(ss), mbs.calcKineticEnergy(ss));

            cout << "QERR=" << ss.getQErr() << endl;
            cout << "UERR=" << ss.getUErr() << endl;
        }

        //if (s.getTime() - std::floor(s.getTime()) < 0.2)
        //    display.addEphemeralDecoration( DecorativeSphere(10).setColor(Green) );

        display.report(ss);
        saveEm.push_back(ss);

        if (ss.getTime() >= 10)
            break;

        // TODO: should check for errors or have or teach RKM to throw. 
        myStudy.stepTo(ss.getTime() + dt, Infinity);
    }

    printf("CPU time=%gs, REAL time=%gs\n", cpuTime()-startCPU, realTime()-startReal);
    printf("Using Integrator %s:\n", myStudy.getMethodName());
    printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
    printf("# CONVERGENCE FAILS = %d\n", myStudy.getNumConvergenceTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
    printf("# PROJECTION FAILS = %d\n", myStudy.getNumProjectionFailures());

    display.dumpStats(std::cout);

    while(true) {
        for (int i=0; i < (int)saveEm.size(); ++i) {
            display.report(saveEm[i]);
            //display.report(saveEm[i]); // half speed
        }
        getchar();
    }
  } 
catch (const exception& e)
  {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
  }
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:101,代码来源:CoarseRNA.cpp

示例6: main


//.........这里部分代码省略.........

    myStudy.setFinalTime(finalTime);

    std::vector<State> saveEm;
    saveEm.reserve(2000);

    for (int i=0; i<50; ++i)
        saveEm.push_back(s);    // delay


    // Peforms assembly if constraints are violated.
    myStudy.initialize(s);

    for (int i=0; i<50; ++i)
        saveEm.push_back(s);    // delay

    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    {
        const State& s = myStudy.getState();
        display.report(s);
        cout << "q=" << s.getQ() << endl;
        cout << "u=" << s.getU() << endl;
        cout << "qErr=" << s.getQErr() << endl;
        cout << "uErr=" << s.getUErr() << endl;
        cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
        cout << "PE=" << mbs.calcPotentialEnergy(s) << " KE=" << mbs.calcKineticEnergy(s) << " E=" << mbs.calcEnergy(s) << endl;
        cout << "angle=" << std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1)) << endl;
        cout << "Assembled configuration shown. Ready to simulate? "; getchar();
    }

    Integrator::SuccessfulStepStatus status;
    int nextReport = 0;

    mbs.resetAllCountersToZero();

    int stepNum = 0;
    while ((status=myStudy.stepTo(nextReport*dt))
           != Integrator::EndOfSimulation) 
    {
        const State& s = myStudy.getState();
        mbs.realize(s, Stage::Acceleration);

        if ((stepNum++%10)==0) {
            const Real angle = std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1));
            printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(), 
                angle,
                mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
                myStudy.getPreviousStepSizeTaken(),
                Integrator::getSuccessfulStepStatusString(status).c_str(),
                myStudy.isStateInterpolated()?" (INTERP)":"");
            printf("     qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
                matter.getQErr(s).normRMS(),
                matter.getUErr(s).normRMS(),
                s.getSystemStage() >= Stage::Acceleration ? matter.getUDotErr(s).normRMS() : Real(-1));
#ifdef HASC
            cout << "CONSTRAINT perr=" << c.getPositionError(s)
                 << " verr=" << c.getVelocityError(s)
                 << " aerr=" << c.getAccelerationError(s)
开发者ID:BrianZ1,项目名称:simbody,代码行数:67,代码来源:MiscConstraints.cpp

示例7: testConservationOfEnergy


//.........这里部分代码省略.........
    MobilizedBody prev=brick2;
    MobilizedBody body25;
    for (int i=0; i<NBod; ++i) {
        MobilizedBody::Ball next(prev, -1*BodyAttach.p(),
                                 brickBody, BodyAttach);
        if (i==25) body25=next;
        //Force::TwoPointLinearSpring(forces,
        //    prev, Vec3(0), next, Vec3(0), 1000, 1);
        prev=next;
    }

    Constraint::Ball(matter.Ground(), Vec3(0,1,0)-2*NBod/3*BodyAttach.p(),
        prev, BodyAttach.p());
    Constraint::Ball(matter.Ground(), Vec3(0,1,0)-1*NBod/3*BodyAttach.p(),
        body25, BodyAttach.p());


    Vec6 k1(1,100,1,100,100,100), c1(0);
    Force::LinearBushing(forces, matter.Ground(), -2*NBod/3*BodyAttach.p(),
                 prev, BodyAttach.p(), k1, c1);
    matter.Ground().addBodyDecoration(-2*NBod/3*BodyAttach.p(),
        DecorativeFrame().setColor(Green));

    Force::Thermostat thermo(forces, matter,
        SimTK_BOLTZMANN_CONSTANT_MD,
        5000,
        .1);

    Vec6 k(1,100,1,100,100,100), c(0);
    Force::LinearBushing bushing1(forces, matter.Ground(), -1*NBod/3*BodyAttach.p(),
        brick1, BodyAttach, k, c);
    Force::LinearBushing bushing2(forces, brick1, Transform(),
        brick2, BodyAttach, k, c);
    matter.Ground().addBodyDecoration(-1*NBod/3*BodyAttach.p(),
        DecorativeFrame().setColor(Green));


    Visualizer viz(system);
    Visualizer::Reporter* reporter = new Visualizer::Reporter(viz, 1./30);
    viz.setBackgroundType(Visualizer::SolidColor);
    system.addEventReporter(reporter);

    ThermoReporter* thermoReport = new ThermoReporter
        (system, thermo, bushing1, bushing2, 1./10);
    system.addEventReporter(thermoReport);

    // Initialize the system and state.

    system.realizeTopology();
    State state = system.getDefaultState();

    viz.report(state);
    printf("Default state -- hit ENTER\n");
    cout << "t=" << state.getTime()
         << " q=" << brick1.getQAsVector(state) << brick2.getQAsVector(state)
         << " u=" << brick1.getUAsVector(state) << brick2.getUAsVector(state)
         << "\nnChains=" << thermo.getNumChains(state)
         << " T="        << thermo.getBathTemperature(state)
         << "\nt_relax=" << thermo.getRelaxationTime(state)
         << " kB="       << thermo.getBoltzmannsConstant()
         << endl;
    getchar();

    state.setTime(0);
    system.realize(state, Stage::Acceleration);
    Vector initU(state.getNU());
    initU = Test::randVector(state.getNU());
    state.updU()=initU;


    RungeKuttaMersonIntegrator integ(system);
    //integ.setMinimumStepSize(1e-1);
    integ.setAccuracy(1e-2);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    const State& istate = integ.getState();

    viz.report(integ.getState());
    viz.zoomCameraToShowAllGeometry();
    printf("After initialize -- hit ENTER\n");
    cout << "t=" << integ.getTime()
         << "\nE=" << system.calcEnergy(istate)
         << "\nEbath=" << thermo.calcBathEnergy(istate)
         << endl;
    thermoReport->handleEvent(istate);
    getchar();

    // Simulate it.
    ts.stepTo(20.0);

    viz.report(integ.getState());
    viz.zoomCameraToShowAllGeometry();
    printf("After simulation:\n");
    cout << "t=" << integ.getTime()
         << "\nE=" << system.calcEnergy(istate)
         << "\nEbath=" << thermo.calcBathEnergy(istate)
         << "\nNsteps=" << integ.getNumStepsTaken()
         << endl;
    thermoReport->handleEvent(istate);
}
开发者ID:thomasklau,项目名称:simbody,代码行数:101,代码来源:TestThermostat.cpp

示例8: main


//.........这里部分代码省略.........
    // Add a blue sphere around the weight.
    viz.addBodyFixedDecoration(swinger, weight1Location, 
          DecorativeSphere(d/8).setColor(Blue).setOpacity(.2));
    viz.addBodyFixedDecoration(swinger, weight2Location, 
          DecorativeSphere(radiusRatio*d/8).setColor(Green).setOpacity(.2));
    viz.addRubberBandLine(GroundIndex, Vec3(0),
                          swinger, Vec3(0),
                          DecorativeLine().setColor(Blue).setLineThickness(10)
                                          .setRepresentation(DecorativeGeometry::DrawPoints));

    //forces.addMobilityConstantForce(swinger, 0, 10);
    Force::ConstantTorque(forces, swinger, Vec3(0,0,10));
    //forces.addConstantForce(swinger, Vec3(0), Vec3(0,10,0));
    //forces.addConstantForce(swinger, Vec3(0,0,0), Vec3(10,10,0)); // z should do nothing
    //forces.addMobilityConstantForce(swinger, 1, 10);
   // forces.addMobilityConstantForce(swinger, 2, 60-1.2);

    State s = mbs.realizeTopology(); // define appropriate states for this System


    pend.setUseEulerAngles(s, true);
    mbs.realizeModel(s);

    mbs.realize(s);

    // Create a study using the Runge Kutta Merson integrator
    RungeKuttaMersonIntegrator myStudy(mbs);
    myStudy.setAccuracy(1e-6);

    // This will pick up decorative geometry from
    // each subsystem that generates any, including of course the 
    // DecorationSubsystem, but not limited to it.
    Visualizer display(mbs);


    const Real expectedPeriod = 2*Pi*std::sqrt(d/g);
    printf("Expected period: %g seconds\n", expectedPeriod);

    const Real dt = 1./30; // output intervals
    const Real finalTime = 1*expectedPeriod;

    for (Real startAngle = 10; startAngle <= 90; startAngle += 10) {
        s = mbs.getDefaultState();
        connector.setQToFitRotation(s, Rotation(Pi/4, Vec3(1,1,1)) );

        printf("time  theta      energy           *************\n");

        swinger.setQToFitTransform(s, Transform( Rotation( BodyRotationSequence, 0*Pi/2, XAxis, 0*Pi/2, YAxis ), Vec3(0,0,0) ));
        swinger.setUToFitVelocity(s, SpatialVec(0*Vec3(1.1,1.2,1.3),Vec3(0,0,-1)));

        s.updTime() = 0;
        myStudy.initialize(s);

        cout << "MassProperties in B=" << swinger.expressMassPropertiesInAnotherBodyFrame(myStudy.getState(),swinger);
        cout << "MassProperties in G=" << swinger.expressMassPropertiesInGroundFrame(myStudy.getState());
        cout << "Spatial Inertia    =" << swinger.calcBodySpatialInertiaMatrixInGround(myStudy.getState());

        int stepNum = 0;
        for (;;) {
            // Should check for errors and other interesting status returns.
            myStudy.stepTo(myStudy.getTime() + dt);
            const State& s = myStudy.getState(); // s is now the integrator's internal state

            if ((stepNum++%10)==0) {
                // This is so we can calculate potential energy (although logically
                // one should be able to do that at Stage::Position).
                mbs.realize(s, Stage::Dynamics);

                cout << s.getTime() << ": E=" << mbs.calcEnergy(s) 
                     << " connector q=" << connector.getQ(s) 
                     << ": swinger q=" << swinger.getQ(s) << endl;

                // This is so we can look at the UDots.
                mbs.realize(s, Stage::Acceleration);

                cout << "q =" << pend.getQ(s) << endl;
                cout << "u =" << pend.getU(s) << endl;
                cout << "ud=" << pend.getUDot(s) << endl;

                cout << "Connector V=" << connector.getMobilizerVelocity(s) << endl;
                cout << "Swinger V=" << swinger.getMobilizerVelocity(s) << endl;

                const Rotation& R_MbM = swinger.getMobilizerTransform(s).R();
                Vec4 aaMb = R_MbM.convertRotationToAngleAxis();
                cout << "angle=" << aaMb[0] << endl;
                cout << "axisMb=" << aaMb.drop1(0) << endl;
                cout << "axisMb=" << ~R_MbM*aaMb.drop1(0) << endl;
            }

            display.report(s);
            if (s.getTime() >= finalTime)
                break;
        }
    }
} 
catch (const exception& e) {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
}
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:101,代码来源:PendulumExample.cpp

示例9: main


//.........这里部分代码省略.........
    //crank.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));

    mbs.realize(s, Stage::Velocity);
    display.report(s);

    cout << "q=" << s.getQ() << endl;
    cout << "qErr=" << s.getQErr() << endl;


    // These are the SimTK Simmath integrators:
    RungeKuttaMersonIntegrator myStudy(mbs);
    //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::Newton);


    //myStudy.setMaximumStepSize(0.001);
    myStudy.setAccuracy(1e-3);
    //myStudy.setProjectEveryStep(true);
    //myStudy.setAllowInterpolation(false);
    //myStudy.setMaximumStepSize(.1);

    const Real dt = 1./30; // output intervals
    const Real finalTime = 10;

    myStudy.setFinalTime(finalTime);

    cout << "Hit ENTER in console to continue ...\n";
    getchar();
    display.report(s);

    cout << "Hit ENTER in console to continue ...\n";
    getchar();

    // Peforms assembly if constraints are violated.
    myStudy.initialize(s);
    myStudy.setProjectEveryStep(false);
    myStudy.setConstraintTolerance(.001);
    myStudy.initialize(myStudy.getState());

    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    {
        const State& s = myStudy.getState();
        display.report(s);
        cout << "q=" << s.getQ() << endl;
        cout << "qErr=" << s.getQErr() << endl;
    }


    Integrator::SuccessfulStepStatus status;
    int nextReport = 0;
    while ((status=myStudy.stepTo(nextReport*dt, Infinity))
           != Integrator::EndOfSimulation)
    {
        const State& s = myStudy.getState();
        mbs.realize(s);
        const Real crankAngle = crank.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg;
        printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
            crankAngle,
            mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
            myStudy.getPreviousStepSizeTaken(),
            Integrator::getSuccessfulStepStatusString(status).c_str(),
            myStudy.isStateInterpolated()?" (INTERP)":"");
        printf("     qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
            crankRocker.getQErr(s).normRMS(),
            crankRocker.getUErr(s).normRMS(),
            crankRocker.getUDotErr(s).normRMS());

        display.report(s);


        if (status == Integrator::ReachedReportTime)
            ++nextReport;
    }

    for (int i=0; i<100; ++i)
        display.report(myStudy.getState());

    printf("Using Integrator %s:\n", myStudy.getMethodName());
    printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());

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

}
开发者ID:thomasklau,项目名称:simbody,代码行数:101,代码来源:RadusDrifter.cpp

示例10: testPrescribedMotion2

void testPrescribedMotion2() {
    
    // Create a system prescribing the motion of two Qs.
    
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    createCylinderSystem(system);
    MobilizedBodyIndex body1 = MobilizedBodyIndex(2);
    MobilizerQIndex coordinate1 = MobilizerQIndex(1);
    Vector coefficients1(2);
    coefficients1[0] = 0.1;
    coefficients1[1] = 0.0;
    Function* function1 = new Function::Linear(coefficients1);
    Constraint::PrescribedMotion constraint1(matter, function1, body1, coordinate1);
    MobilizedBodyIndex body2 = MobilizedBodyIndex(2);
    MobilizerQIndex coordinate2 = MobilizerQIndex(0);
    Vector coefficients2(3);
    coefficients2[0] = 0.5;
    coefficients2[1] = -0.2;
    coefficients2[2] = 1.1;
    Function* function2 = new Function::Polynomial(coefficients2);
    Constraint::PrescribedMotion constraint2(matter, function2, body2, coordinate2);
    
    // Must track work done by the constraints in order to check that
    // energy is conserved.
    Measure::Zero zeroMeas(matter);
    PowerMeasure<Real> powMeas1(matter, constraint1);
    Measure::Integrate workMeas1(matter, powMeas1, zeroMeas);     
    PowerMeasure<Real> powMeas2(matter, constraint2);
    Measure::Integrate workMeas2(matter, powMeas2, zeroMeas);    
    
    State state;
    createState(system, state);
    workMeas1.setValue(state, 0); // override createState
    workMeas2.setValue(state, 0); // override createState
    
    // Make sure the constraint is satisfied.
    
    Vector args(1, state.getTime());
    SimTK_TEST_EQ(function1->calcValue(args), 
        matter.getMobilizedBody(body1).getOneQ(state, coordinate1));
    SimTK_TEST_EQ(function2->calcValue(args), 
        matter.getMobilizedBody(body2).getOneQ(state, coordinate2));
    
    // Simulate it and make sure the constraint is working correctly and energy is being conserved.
    const Real energy0 = system.calcEnergy(state);   
    
    RungeKuttaMersonIntegrator integ(system);
    integ.setReturnEveryInternalStep(true);
    integ.initialize(state);
    while (integ.getTime() < 10.0) {
        integ.stepTo(10.0);
        const State& istate = integ.getState();
        system.realize(istate, Stage::Acceleration);
        const Real energy = system.calcEnergy(istate);
        const Real power1 = powMeas1.getValue(istate);
        const Real work1 =  workMeas1.getValue(istate);
        const Real power2 = powMeas2.getValue(istate);
        const Real work2 =  workMeas2.getValue(istate);

        Vector args(1, istate.getTime());
        SimTK_TEST_EQ_TOL(function1->calcValue(args), 
            matter.getMobilizedBody(body1).getOneQ(istate, coordinate1), 
            integ.getConstraintToleranceInUse());
        SimTK_TEST_EQ_TOL(function2->calcValue(args), 
            matter.getMobilizedBody(body2).getOneQ(istate, coordinate2), 
            integ.getConstraintToleranceInUse());

        // Energy conservation depends on global integration accuracy;
        // accuracy returned here is local so we'll fudge at 10X.
        const Real etol = 10*integ.getAccuracyInUse()
                          *std::max(std::abs(energy-(work1+work2)), std::abs(energy0));        
        SimTK_TEST_EQ_TOL(energy0, energy-(work1+work2), etol)
    }
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:75,代码来源:TestCustomConstraints.cpp

示例11: testSpeedCoupler3

void testSpeedCoupler3() {
    
    // Create a system with a constraint that uses both u's and q's.
    // This will not be workless in general.
    
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    createCylinderSystem(system);
    MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
    std::vector<MobilizedBodyIndex> ubody(2), qbody(1);
    std::vector<MobilizerUIndex> uindex(2);
    std::vector<MobilizerQIndex> qindex(1);
    ubody[0] = MobilizedBodyIndex(1);
    ubody[1] = MobilizedBodyIndex(3);
    qbody[0] = MobilizedBodyIndex(5);
    uindex[0] = MobilizerUIndex(0);
    uindex[1] = MobilizerUIndex(1);
    qindex[0] = MobilizerQIndex(1);
    Function* function = new CompoundFunction();
    Constraint::SpeedCoupler coupler(matter, function, ubody, uindex, 
                                     qbody, qindex);
    PowerMeasure<Real> powMeas(matter, coupler);
    Measure::Zero zeroMeas(matter);
    Measure::Integrate workMeas(matter, powMeas, zeroMeas); 

    State state;
    createState(system, state);
    workMeas.setValue(state, 0); // override createState
    
    // Make sure the constraint is satisfied.
    
    Vector args(function->getArgumentSize());
    args[0] = matter.getMobilizedBody(ubody[0]).getOneU(state, uindex[0]);
    args[1] = matter.getMobilizedBody(ubody[1]).getOneU(state, uindex[1]);
    args[2] = matter.getMobilizedBody(qbody[0]).getOneQ(state, qindex[0]);
    SimTK_TEST_EQ(0.0, function->calcValue(args));
    
    // Simulate it and make sure the constraint is working correctly.
    // We don't expect energy to be conserved here but energy minus the
    // work done by the constraint should be conserved.
    Real energy0 = system.calcEnergy(state);

    RungeKuttaMersonIntegrator integ(system);
    integ.setAccuracy(1e-6);
    integ.setReturnEveryInternalStep(true);
    integ.initialize(state);

    while (integ.getTime() < 10.0) {
        integ.stepTo(10.0);
        const State& istate = integ.getState();
        system.realize(istate, Stage::Acceleration);
        const Real energy = system.calcEnergy(istate);
        const Real power = powMeas.getValue(istate);
        const Real work =  workMeas.getValue(istate);

        args[0] = matter.getMobilizedBody(ubody[0]).getOneU(state, uindex[0]);
        args[1] = matter.getMobilizedBody(ubody[1]).getOneU(state, uindex[1]);
        args[2] = matter.getMobilizedBody(qbody[0]).getOneQ(state, qindex[0]);
        SimTK_TEST_EQ_TOL(0.0, function->calcValue(args), 
                          integ.getConstraintToleranceInUse());

        // Energy conservation depends on global integration accuracy;
        // accuracy returned here is local so we'll fudge at 10X.
        const Real etol = 10*integ.getAccuracyInUse()
                          *std::max(std::abs(energy-work), std::abs(energy0));        
        SimTK_TEST_EQ_TOL(energy0, energy-work, etol)

    }
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:69,代码来源:TestCustomConstraints.cpp

示例12: testCoordinateCoupler2

void testCoordinateCoupler2() {
    
    // Create a system involving a constraint that affects multiple mobilizers.
    
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    createCylinderSystem(system);
    MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
    std::vector<MobilizedBodyIndex> mobilizers(3);
    std::vector<MobilizerQIndex>    coordinates(3);
    mobilizers[0]  = MobilizedBodyIndex(1);
    mobilizers[1]  = MobilizedBodyIndex(1);
    mobilizers[2]  = MobilizedBodyIndex(5);
    coordinates[0] = MobilizerQIndex(0);
    coordinates[1] = MobilizerQIndex(1);
    coordinates[2] = MobilizerQIndex(1);
    Function* function = new CompoundFunction();
    Constraint::CoordinateCoupler coupler(matter, function, 
                                          mobilizers, coordinates);
    State state;
    createState(system, state);
    
    // Make sure the constraint is satisfied.
    
    Vector cq(function->getArgumentSize());
    for (int i = 0; i < cq.size(); ++i)
        cq[i] = matter.getMobilizedBody(mobilizers[i])
                      .getOneQ(state, coordinates[i]);
    SimTK_TEST_EQ(0.0, function->calcValue(cq));
    
    // Simulate it and make sure the constraint is working correctly and
    // energy is being conserved. This is a workless constraint so the
    // power should be zer
    system.realize(state, Stage::Acceleration);
    Real energy0 = system.calcEnergy(state);

    RungeKuttaMersonIntegrator integ(system);
    integ.setReturnEveryInternalStep(true);
    integ.initialize(state);
    while (integ.getTime() < 10.0) {
        integ.stepTo(10.0);
        const State& istate = integ.getState();
        system.realize(istate, Stage::Acceleration);
        const Vector& u = istate.getU();
        const Real energy = system.calcEnergy(istate);
        const Real power  = coupler.calcPower(istate);


        for (int i = 0; i < cq.size(); ++i)
            cq[i] = matter.getMobilizedBody(mobilizers[i])
                          .getOneQ(istate, coordinates[i]);
        SimTK_TEST_EQ_TOL(0.0, function->calcValue(cq), 
                          integ.getConstraintToleranceInUse());

        // Power output should always be zero to machine precision
        // with some slop for calculation of multipliers.
        SimTK_TEST_EQ_SIZE(0.0, power, istate.getNU());

        // Energy conservation depends on global integration accuracy;
        // accuracy returned here is local so we'll fudge at 10X.
        const Real etol = 10*integ.getAccuracyInUse()
                          *std::max(std::abs(energy), std::abs(energy0));
        SimTK_TEST_EQ_TOL(energy0, energy, etol);
    }
}
开发者ID:AyMaN-GhOsT,项目名称:simbody,代码行数:65,代码来源:TestCustomConstraints.cpp


注:本文中的MultibodySystem::calcEnergy方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。