void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy, 
                   const Array_<MobilizedBodyIndex>& originalBodyIxs, 
                   Array_<MobilizedBodyIndex>& copyBodyIxs,
                   bool& hasArtificialBaseBody) 
{
    const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
    SimbodyMatterSubsystem copyMatter(copy);
    Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
    body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
    std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
    hasArtificialBaseBody = false;
    for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
        const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
        MobilizedBody* copyBody;
        if (i == 0) {
            if (originalBody.isGround())
                copyBody = &copyMatter.Ground();
            else {
                hasArtificialBaseBody = true; // not using the original joint here
                MobilizedBody::Free free(copyMatter.Ground(), body);
                copyBody = &copyMatter.updMobilizedBody(free.getMobilizedBodyIndex());
            }
        }
        else {
            MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
            copyBody = &originalBody.cloneForNewParent(parent);
        }
        copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
        idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
    }
    copy.realizeTopology();
    State& s = copy.updDefaultState();
    copyMatter.setUseEulerAngles(s, true);
    copy.realizeModel(s);
}
int main(int argc, char** argv) {
    static const Transform GroundFrame;
    static const Rotation ZUp(UnitVec3(XAxis), XAxis, UnitVec3(YAxis), ZAxis);
    static const Vec3 TestLoc(1,0,0);

  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem             mbs;

    SimbodyMatterSubsystem      matter(mbs);
    GeneralForceSubsystem       forces(mbs);
    DecorationSubsystem         viz(mbs);
    //Force::UniformGravity       gravity(forces, matter, Vec3(0, -g, 0));

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid particle = Body::Rigid(MassProperties(m, Vec3(0), Inertia(0)));
    particle.addDecoration(DecorativeSphere(.1).setColor(Red).setOpacity(.3));

    MobilizedBody::SphericalCoords
        anAtom(matter.Ground(), Transform(ZUp, TestLoc),
               particle, Transform(),
               0*Deg2Rad,  false,   // azimuth offset, negated
               0,          false,   // zenith offset, negated
               ZAxis,      true);  // translation axis, negated

    anAtom.setDefaultRadius(.1);
    anAtom.setDefaultAngles(Vec2(0, 30*Deg2Rad));

    viz.addRubberBandLine(matter.Ground(), TestLoc,
                          anAtom, Vec3(0),
                          DecorativeLine().setColor(Orange).setLineThickness(4));

    Force::MobilityLinearSpring(forces, anAtom, 0, 2, -30*Deg2Rad); // harmonic bend
    Force::MobilityLinearSpring(forces, anAtom, 1, 2, 45*Deg2Rad); // harmonic bend
    Force::MobilityLinearSpring(forces, anAtom, 2, 20, .5); // harmonic stretch

    Force::MobilityLinearDamper(forces, anAtom, 0, .1); // harmonic bend
    Force::MobilityLinearDamper(forces, anAtom, 1, .1); // harmonic bend
    Force::MobilityLinearDamper(forces, anAtom, 2, .1); // harmonic stretch

    
    State s = mbs.realizeTopology(); // returns a reference to the the default state
    mbs.realizeModel(s); // define appropriate states for this System
    mbs.realize(s, Stage::Instance); // instantiate constraints if any

    Visualizer display(mbs);
    display.setBackgroundType(Visualizer::SolidColor);

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

}
Exemple #3
0
int main(int argc, char** argv) {
  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem         mbs;

    SimbodyMatterSubsystem  matter(mbs);
    GeneralForceSubsystem    forces(mbs);
    DecorationSubsystem     viz(mbs);
    Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::brick(hl[0],hl[1],hl[2])));
    body.addDecoration(DecorativeBrick(hl).setOpacity(.5));
    body.addDecoration(DecorativeLine(Vec3(0), Vec3(0,1,0)).setColor(Green));

    MobilizedBody::Free mobilizedBody(matter.Ground(), Transform(), body, Transform());
    MobilizedBody::Free mobilizedBody0(mobilizedBody, Transform(Vec3(1,2,0)), body, Transform(Vec3(0,1,0)));
    MobilizedBody::Free mobilizedBody2(mobilizedBody0, Vec3(-5,0,0), body, Transform());

    Body::Rigid gear1body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(.5, .1)));
    gear1body.addDecoration(DecorativeCircle(.5).setColor(Green).setOpacity(.7));
    gear1body.addDecoration(DecorativeLine(Vec3(0), Vec3(.5,0,0)).setColor(Black).setLineThickness(4));
    Body::Rigid gear2body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(1.5, .1)));
    gear2body.addDecoration(Transform(), DecorativeCircle(1.5).setColor(Blue).setOpacity(.7));  
    gear2body.addDecoration(Transform(), DecorativeLine(Vec3(0), Vec3(1.5,0,0)).setColor(Black).setLineThickness(4));

    MobilizedBody::Pin gear1(mobilizedBody2, Vec3(-1,0,0), gear1body, Transform()); // along z
    MobilizedBody::Pin gear2(mobilizedBody2, Vec3(1,0,0), gear2body, Transform()); // along z
    Constraint::NoSlip1D(mobilizedBody2, Vec3(-.5,0,0), UnitVec3(0,1,0), gear1, gear2);

    Constraint::ConstantSpeed(gear1, 100.);
    
    //Constraint::Ball myc2(matter.Ground(), Vec3(-4,2,0),  mobilizedBody2, Vec3(0,1,0));
    Constraint::Weld myc(matter.Ground(), Vec3(1,2,0),  mobilizedBody, Vec3(0,1,0));
    Constraint::Ball ball1(mobilizedBody, Vec3(2,0,0), mobilizedBody0, Vec3(3,1,1));
    Constraint::Ball ball2(mobilizedBody0, Vec3(2,0,0), mobilizedBody2, Vec3(3,0,0));
    //Constraint::PointInPlane pip(mobilizedBody0, UnitVec3(0,-1,0), 3, mobilizedBody2, Vec3(3,0,0));

    //Constraint::ConstantOrientation ori(mobilizedBody, Rotation(), mobilizedBody0, Rotation());
    //Constraint::ConstantOrientation ori2(mobilizedBody2, Rotation(), mobilizedBody0, Rotation());
    //Constraint::Weld weld(mobilizedBody, Transform(Rotation(Pi/4, ZAxis), Vec3(1,1,0)),
      //                    mobilizedBody2, Transform(Rotation(-Pi/4, ZAxis), Vec3(-1,-1,0)));
    
    //MyConstraint xyz(gear1, 100.);

    viz.addBodyFixedDecoration(mobilizedBody, Transform(Vec3(1,2,3)), DecorativeText("hello world").setScale(.1));



/*
    class MyHandler : public ScheduledEventHandler {
    public:
        MyHandler(const Constraint& cons) : c(cons) { }
        Real getNextEventTime(const State&, bool includeCurrentTime) const {
            return .314;
        }
        void handleEvent(State& s, Real acc, const Vector& ywts, const Vector& cwts, Stage& modified,
                         bool& shouldTerminate) const 
        {
            cout << "<<<< TRIGGERED AT T=" << s.getTime() << endl;
            c.enable(s);
            modified = Stage::Model;
        }
    private:
        const Constraint& c;
    };

    mbs.addEventHandler(new MyHandler(xyz));
*/


    State s = mbs.realizeTopology(); // returns a reference to the the default state

    //xyz.disable(s);

    //matter.setUseEulerAngles(s, true);
    mbs.realizeModel(s); // define appropriate states for this System

    //mobilizedBody0.setQ(s, .1);
    //mobilizedBody.setQ(s, .2);

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

    mbs.realize(s, Stage::Velocity);
    display.report(s);
    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;
    cout << "qErr=" << s.getQErr() << endl;
    cout << "uErr=" << s.getUErr() << endl;

    for (ConstraintIndex cid(0); cid < matter.getNumConstraints(); ++cid) {
        const Constraint& c = matter.getConstraint(cid);
        int mp,mv,ma;
        c.getNumConstraintEquationsInUse(s, mp,mv,ma);

        cout << "CONSTRAINT " << cid << (c.isDisabled(s) ? "**DISABLED** " : "")
             << " constrained bodies=" << c.getNumConstrainedBodies();
        if (c.getNumConstrainedBodies()) cout << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex();
        cout << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers() 
                                           << "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
             << " mp,mv,ma=" << mp << "," << mv << "," << ma 
             << endl;
        for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
            cout << "  constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex(); 
            cout << endl;
        }
        for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
            cout << "  constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex() 
                  << ", q(" << c.getNumConstrainedQ(s, cmx) << ")="; 
            for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
                cout << " " << c.getConstrainedQIndex(s, cmx, i);                  
            cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
            for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
                cout << " " << c.getConstrainedUIndex(s, cmx, i);
            cout << endl;
        }
        cout << c.getSubtree();
             
        if (mp) {
            cout << "perr=" << c.getPositionErrorsAsVector(s) << endl;
            cout << "   d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
            cout << "  ~d(Pt lambda)/dlambda=" << ~c.calcPositionConstraintMatrixPt(s);
            cout << "   d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);

            Matrix P = c.calcPositionConstraintMatrixP(s);
            Matrix PQ(mp,matter.getNQ(s));
            Vector out(matter.getNQ(s));
            for (int i=0; i<mp; ++i) {
                Vector in = ~P[i];
                matter.multiplyByNInv(s, true, in, out);
                PQ[i] = ~out;
            }
            cout << " calculated d(perr)/dq=" << PQ;
        }


        if (mv) {
            cout << "verr=" << c.getVelocityErrorsAsVector(s) << endl;
            //cout << "   d(verrdot)/dudot=" << c.calcVelocityConstraintMatrixV(s);
            cout << "  ~d(Vt lambda)/dlambda=" << ~c.calcVelocityConstraintMatrixVt(s);
        }

    }
    const Constraint& c = matter.getConstraint(myc.getConstraintIndex());

    cout << "Default configuration shown. Ready? "; getchar();

    mobilizedBody.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,1,1)),Vec3(.1,.2,.3)));
    mobilizedBody0.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,-1,1)),Vec3(.2,.2,.3)));
    mobilizedBody2.setQToFitTransform (s, Transform(Rotation(.05,Vec3(-1,1,1)),Vec3(.1,.2,.1)));
    mobilizedBody.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
    mobilizedBody0.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
    mobilizedBody2.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));

    //gear1.setUToFitAngularVelocity(s, Vec3(0,0,500)); // these should be opposite directions!
    //gear2.setUToFitAngularVelocity(s, Vec3(0,0,100));

    mbs.realize(s, Stage::Velocity);
    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 << "v_MbM=" << mobilizedBody.getMobilizerVelocity(s)[1] << endl;
    cout << "Unassembled configuration shown. Ready to assemble? "; getchar();

    // These are the SimTK Simmath integrators:
    RungeKuttaMersonIntegrator myStudy(mbs);
    //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::/*Newton*/Functional);
    //myStudy.setOrderLimit(2); // cpodes only
    //VerletIntegrator myStudy(mbs);
   // ExplicitEulerIntegrator myStudy(mbs, .0005); // fixed step
    //ExplicitEulerIntegrator myStudy(mbs); // variable step


    //myStudy.setMaximumStepSize(0.001);
    myStudy.setAccuracy(1e-6); myStudy.setAccuracy(1e-1);
    //myStudy.setProjectEveryStep(true);
    //myStudy.setProjectInterpolatedStates(false);
    myStudy.setConstraintTolerance(1e-7); myStudy.setConstraintTolerance(1e-2);
    //myStudy.setAllowInterpolation(false);
    //myStudy.setMaximumStepSize(.1);

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

    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)
                 << endl;
#endif
            //cout << "   d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
            //cout << "  ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
            //cout << "   d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
            cout << "Q=" << matter.getQ(s) << endl;
            cout << "U=" << matter.getU(s) << endl;
            cout << "Multipliers=" << matter.getMultipliers(s) << endl;
        }

        Vector qdot;
        matter.calcQDot(s, s.getU(), qdot);
       // cout << "===> qdot =" << qdot << endl;

        Vector qdot2;
        matter.multiplyByN(s, false, s.getU(), qdot2);
       // cout << "===> qdot2=" << qdot2 << endl;

        Vector u1,u2;
        matter.multiplyByNInv(s, false, qdot, u1);
        matter.multiplyByNInv(s, false, qdot2, u2);
      //  cout << "===> u =" << s.getU() << endl;
      //  cout << "===> u1=" << u1 << endl;
      //  cout << "===> u2=" << u2 << endl;
       // cout << "     norm=" << (s.getU()-u2).normRMS() << endl;

        display.report(s);
        saveEm.push_back(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());

    printf("System stats: realize %dP %dV %dA, projectQ %d, projectU %d\n",
        mbs.getNumRealizationsOfThisStage(Stage::Position),
        mbs.getNumRealizationsOfThisStage(Stage::Velocity),
        mbs.getNumRealizationsOfThisStage(Stage::Acceleration),
        mbs.getNumProjectQCalls(), mbs.getNumProjectUCalls());


    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);
  }
  catch (...) {
    printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

}
static void testObservedPointFitter(bool useConstraint) {
    int failures = 0;
    for (int iter = 0; iter < ITERATIONS; ++iter) {
        
        // Build a system consisting of a chain of bodies with occasional side chains, and
        // a variety of mobilizers.

        MultibodySystem mbs;
        SimbodyMatterSubsystem matter(mbs);
        Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
        body.addDecoration(Transform(), DecorativeSphere(.1));
        MobilizedBody* lastBody = &matter.Ground();
        MobilizedBody* lastMainChainBody = &matter.Ground();
        vector<MobilizedBody*> bodies;
        Random::Uniform random(0.0, 1.0);
        random.setSeed(iter);

        for (int i = 0; i < NUM_BODIES; ++i) {
            bool mainChain = random.getValue() < 0.5;
            MobilizedBody* parent = (mainChain ? lastMainChainBody : lastBody);
            int type = (int) (random.getValue()*4);
            MobilizedBody* nextBody;
            if (type == 0) {
                MobilizedBody::Cylinder cylinder(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(cylinder.getMobilizedBodyIndex());
            }
            else if (type == 1) {
                MobilizedBody::Slider slider(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(slider.getMobilizedBodyIndex());
            }
            else if (type == 2) {
                MobilizedBody::Ball ball(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(ball.getMobilizedBodyIndex());
            }
            else {
                MobilizedBody::Pin pin(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(pin.getMobilizedBodyIndex());
            }
            bodies.push_back(nextBody);
            if (mainChain)
                lastMainChainBody = nextBody;
            lastBody = nextBody;
        }
        mbs.realizeTopology();
        State s = mbs.getDefaultState();
        matter.setUseEulerAngles(s, true);
        mbs.realizeModel(s);

        // Choose a random initial conformation.

        vector<Real> targetQ(s.getNQ(), Real(0));
        for (MobilizedBodyIndex mbx(1); mbx < matter.getNumBodies(); ++mbx) {
            const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
            for (int i = 0; i < mobod.getNumQ(s); ++i) {
                const QIndex qx0 = mobod.getFirstQIndex(s);
                s.updQ()[qx0+i] = targetQ[qx0+i] = 2.0*random.getValue();
            }
        }
        //cout << "q0=" << s.getQ() << endl;
        mbs.realize(s, Stage::Position);

        // Select some random stations on each body.

        vector<vector<Vec3> > stations(NUM_BODIES);
        vector<vector<Vec3> > targetLocations(NUM_BODIES);
        vector<MobilizedBodyIndex> bodyIxs;
        for (int i = 0; i < NUM_BODIES; ++i) {
            MobilizedBodyIndex id = bodies[i]->getMobilizedBodyIndex();
            bodyIxs.push_back(id);
            int numStations = 1 + (int) (random.getValue()*4);
            for (int j = 0; j < numStations; ++j) {
                Vec3 pos(2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0);
                stations[i].push_back(pos);
                targetLocations[i].push_back(bodies[i]->getBodyTransform(s)*pos);
            }
        }

        Real distance = -1;
        if (useConstraint) {
            // Add a constraint fixing the distance between the first and last bodies.
            Real distance = (bodies[0]->getBodyOriginLocation(s)-bodies[NUM_BODIES-1]->getBodyOriginLocation(s)).norm();
            // (sherm 140506) Without this 1.001 this failed on clang.
            Constraint::Rod(*bodies[0], Vec3(0), *bodies[NUM_BODIES-1], Vec3(0), 1.001*distance);
        }
        s = mbs.realizeTopology();
        matter.setUseEulerAngles(s, true);
        mbs.realizeModel(s);

        // Try fitting it.
        State initState = s;
        // (sherm 140506) I raised this from .02 to .03 to make this more robust.
        if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.0, 0.03, distance))
            failures++;

        //cout << "q1=" << s.getQ() << endl;

        // Now add random noise to the target locations, and see if it can still fit decently.

        Random::Gaussian gaussian(0.0, 0.15);
        for (int i = 0; i < (int) targetLocations.size(); ++i) {
            for (int j = 0; j < (int) targetLocations[i].size(); ++j) {
                targetLocations[i][j] += Vec3(gaussian.getValue(), gaussian.getValue(), gaussian.getValue());
            }
        }

        s = initState; // start from same config as before
        if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.1, 0.5, distance))
            failures++;

        //cout << "q2=" << s.getQ() << endl;

    }

    ASSERT(failures == 0); // It found a reasonable fit every time.
    std::cout << "Done" << std::endl;
}
int main() {
    
    // Build a system consisting of a chain of bodies with every possible mobilizer.
    
    MultibodySystem mbs;
    SimbodyMatterSubsystem matter(mbs);
    Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
    body.addDecoration(DecorativeSphere(.1));
    Random::Uniform random(0.0, 2.0);
    MobilizedBody lastBody = MobilizedBody::Pin(matter.Ground(), Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Slider(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Universal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Cylinder(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::BendStretch(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Planar(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Gimbal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Ball(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Translation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Free(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::LineOrientation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::FreeLine(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Weld(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Screw(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())), 0.5);
    lastBody = MobilizedBody::Ellipsoid(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    mbs.realizeTopology();
    State& s = mbs.updDefaultState();
    mbs.realizeModel(s);
    
    // Choose a random initial conformation.
    
    for (int i = 0; i < s.getNQ(); ++i)
        s.updQ()[i] = random.getValue();
    mbs.realize(s, Stage::Instance);
    // The only constraints are the quaternions -- normalize them.
    Vector temp;
    mbs.project(s, 0.01);
    mbs.realize(s, Stage::Position);
    
    // Convert to Euler angles and make sure the positions are all the same.
    
    State euler = s;
    matter.convertToEulerAngles(s, euler);
    mbs.realize(euler, Stage::Position);
    for (int i = 0; i < matter.getNumBodies(); ++i) {
        const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
        Real dist = (body.getBodyOriginLocation(euler)-body.getBodyOriginLocation(s)).norm();
        ASSERT(dist < 1e-5);
    }

    // Now convert back to quaternions and make sure the positions are still the same.
    
    State quaternions = s;
    matter.convertToQuaternions(euler, quaternions);
    mbs.realize(quaternions, Stage::Position);
    for (int i = 0; i < matter.getNumBodies(); ++i) {
        const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
        Real dist = (body.getBodyOriginLocation(quaternions)-body.getBodyOriginLocation(s)).norm();
        ASSERT(dist < 1e-5);
    }
    
    // Compare the state variables to see if they have been accurately reproduced.
    
    mbs.project(s, 0.01); // Normalize the quaternions
    Real diff = std::sqrt((s.getQ()-quaternions.getQ()).normSqr()/s.getNQ());
    ASSERT(diff < 1e-5);

    std::cout << "Done" << std::endl;
}
void testRiboseMobilizer() 
{
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    DecorationSubsystem decorations(system);

    matter.setShowDefaultGeometry(false);

    // Put some hastily chosen mass there (doesn't help)
    Body::Rigid rigidBody;
    rigidBody.setDefaultRigidBodyMassProperties(MassProperties(
        mass_t(20.0*daltons),
        location_t(Vec3(0,0,0)*nanometers),
        Inertia(20.0)
        ));

    // One body anchored at C4 atom, 
    MobilizedBody::Weld c4Body( 
        matter.updGround(), 
        Rotation(-120*degrees, XAxis),
        rigidBody,
        Transform());
    // sphere for C4 atom
    decorations.addBodyFixedDecoration(
        c4Body.getMobilizedBodyIndex(), 
        Transform(),
        DecorativeSphere( length_t(0.5*angstroms) )
    );
    // sphere for C5 atom
    decorations.addBodyFixedDecoration(
        c4Body.getMobilizedBodyIndex(), 
        location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
        DecorativeSphere( length_t(0.5*angstroms) )
    );

    decorations.addRubberBandLine(
        c4Body.getMobilizedBodyIndex(),
        Vec3(0),
        c4Body.getMobilizedBodyIndex(),
        location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));

    // One body anchored at C3 atom -- works
    // Pin version
    //MobilizedBody::Pin c3Body( 
    //    c4Body, 
    //    Transform(),
    //    rigidBody,
    //    Transform(location_t(Vec3(0,0,1.5)*angstroms))
    //    );

    // Function based pin version -- works
    //TestPinMobilizer c3Body( 
    //    c4Body, 
    //    Transform(),
    //    rigidBody,
    //    Transform(location_t(Vec3(0,0,1.5)*angstroms))
    //    );
    
    PseudorotationMobilizer c3Body( 
        c4Body, 
        Transform(),
        rigidBody,
        Transform(location_t(Vec3(0,0,1.5)*angstroms)),
        angle_t(36.4*degrees), // amplitude
        angle_t(-161.8*degrees) // phase
        );
    // sphere for C3 atom
    decorations.addBodyFixedDecoration(
        c3Body.getMobilizedBodyIndex(), 
        Transform(),
        DecorativeSphere( length_t(0.5*angstroms) )
    );
    // sphere for O3 atom
    decorations.addBodyFixedDecoration(
        c3Body.getMobilizedBodyIndex(), 
        location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
        DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
    );

    decorations.addRubberBandLine(
        c3Body.getMobilizedBodyIndex(),
        Vec3(0),
        c3Body.getMobilizedBodyIndex(),
        location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c4Body.getMobilizedBodyIndex(),
        Vec3(0),
        c3Body.getMobilizedBodyIndex(),
        Vec3(0),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));

    PseudorotationMobilizer c2Body( 
        c3Body, 
        Rotation( angle_t(-80*degrees), YAxis ),
        rigidBody,
        Transform(location_t(Vec3(0,0,1.5)*angstroms)),
        angle_t(35.8*degrees), // amplitude
        angle_t(-91.3*degrees) // phase
        );
    // sphere for C2 atom
    decorations.addBodyFixedDecoration(
        c2Body.getMobilizedBodyIndex(), 
        Transform(),
        DecorativeSphere( length_t(0.5*angstroms) )
    );
    // sphere for O2 atom
    decorations.addBodyFixedDecoration(
        c2Body.getMobilizedBodyIndex(), 
        location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
        DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
    );

    decorations.addRubberBandLine(
        c2Body.getMobilizedBodyIndex(),
        Vec3(0),
        c2Body.getMobilizedBodyIndex(),
        location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c3Body.getMobilizedBodyIndex(),
        Vec3(0),
        c2Body.getMobilizedBodyIndex(),
        Vec3(0),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));

    PseudorotationMobilizer c1Body( 
        c2Body, 
        Rotation( angle_t(-80*degrees), YAxis ),
        rigidBody,
        Transform(location_t(Vec3(0,0,1.5)*angstroms)),
        angle_t(37.6*degrees), // amplitude
        angle_t(52.8*degrees) // phase
        );
    // sphere for C1 atom
    decorations.addBodyFixedDecoration(
        c1Body.getMobilizedBodyIndex(), 
        Transform(),
        DecorativeSphere( length_t(0.5*angstroms) )
    );
    // sphere for N1 atom
    decorations.addBodyFixedDecoration(
        c1Body.getMobilizedBodyIndex(), 
        location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
        DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(0,0,1))
    );
    // sphere for O4 atom
    decorations.addBodyFixedDecoration(
        c1Body.getMobilizedBodyIndex(), 
        location_t(Vec3(1.0,0,-0.5)*angstroms),
        DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
    );

    decorations.addRubberBandLine(
        c2Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        Vec3(0),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c1Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        location_t(Vec3(1.0,0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c1Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c4Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        location_t(Vec3(1.0,0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));

    // Prescribed motion
    Constraint::ConstantSpeed(c3Body, 0.5);

    // Two constraint way works; one constraint way does not
    bool useTwoConstraints = true;

    if (useTwoConstraints) {
        // Constraints to make three generalized coordinates identical
        std::vector<MobilizedBodyIndex> c32bodies(2);
        c32bodies[0] = c3Body.getMobilizedBodyIndex();
        c32bodies[1] = c2Body.getMobilizedBodyIndex();
        std::vector<MobilizerQIndex> coordinates(2, MobilizerQIndex(0));
        Constraint::CoordinateCoupler(matter, new DifferenceFunction, c32bodies, coordinates);

        std::vector<MobilizedBodyIndex> c21bodies(2);
        c21bodies[0] = c2Body.getMobilizedBodyIndex();
        c21bodies[1] = c1Body.getMobilizedBodyIndex();
        Constraint::CoordinateCoupler(matter, new DifferenceFunction, c21bodies, coordinates);
    }
    else { // trying to get single constraint way to work
        // Try one constraint for all three mobilizers
        std::vector<MobilizedBodyIndex> c123Bodies(3);
        c123Bodies[0] = c1Body.getMobilizedBodyIndex();
        c123Bodies[1] = c2Body.getMobilizedBodyIndex();
        c123Bodies[2] = c3Body.getMobilizedBodyIndex();
        std::vector<MobilizerQIndex> coords3(3, MobilizerQIndex(0));
        Constraint::CoordinateCoupler(matter, new ThreeDifferencesFunction, c123Bodies, coords3);
    }


    Visualizer viz(system);
    viz.setBackgroundType(Visualizer::SolidColor);

    system.addEventReporter(new Visualizer::Reporter(viz, 0.10));

    system.realizeTopology();
    State& state = system.updDefaultState();
    
    // Simulate it.
    VerletIntegrator integ(system);
    //RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(50.0);
}
Exemple #7
0
int main(int argc, char** argv) {
  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem         mbs; mbs.setUseUniformBackground(true);

    SimbodyMatterSubsystem  crankRocker(mbs);
    GeneralForceSubsystem   forces(mbs);
    DecorationSubsystem     viz(mbs);
    Force::UniformGravity   gravity(forces, crankRocker, Vec3(0, -g, 0));

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid crankBody  = Body::Rigid(MassProperties(.1, Vec3(0), 0.1*UnitInertia::brick(1,3,.5)));
    crankBody.addDecoration(DecorativeEllipsoid(0.1*Vec3(1,3,.4))
                                .setResolution(10)
                                .setOpacity(.2));
    Body::Rigid sliderBody = Body::Rigid(MassProperties(.2, Vec3(0), 0.2*UnitInertia::brick(1,5,.5)));
    sliderBody.addDecoration(DecorativeEllipsoid(0.2*Vec3(1,5,.4))
                                .setColor(Blue)
                                .setResolution(10)
                                .setOpacity(.2));
    Body::Rigid longBar = Body::Rigid(MassProperties(0.01, Vec3(0), 0.01*UnitInertia::cylinderAlongX(.1, 5)));
    longBar.addDecoration(Rotation(Pi/2,ZAxis), DecorativeCylinder(.01, 1));

    MobilizedBody::Pin
        crank(crankRocker.Ground(), Transform(), crankBody, Transform(Vec3(0, .15, 0)));
    MobilizedBody::Pin
        slider(crankRocker.Ground(), Transform(Vec3(2.5,0,0)), sliderBody, Transform(Vec3(0, 1, 0)));

    MobilizedBody::Universal
        rightConn(crank, Transform(Rotation(-Pi/2,YAxis),Vec3(0,-.3,0)),
                  longBar, Transform(Rotation(-Pi/2,YAxis),Vec3(-1,0,0)));

    Constraint::Ball ball(slider, Vec3(0,-1, 0), rightConn, Vec3(1,0,0));
    ball.setDefaultRadius(0.01);

    //Constraint::Rod rodl(leftConn, Vec3(0), rightConn, Vec3(-2.5,0,0), 2.5);
    //Constraint::Rod rodr(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-1.5,0,0), std::sqrt(2.));
    //Constraint::Rod rodo(leftConn, Vec3(1.5,0,0), rightConn, Vec3(-2.5,1,0), std::sqrt(2.));
    //Constraint::Rod rodl(leftConn, Vec3(-2.5,0,0), rightConn, Vec3(-2.5,0,0), 5);
    //Constraint::Rod rodr(leftConn, Vec3(2.5,0,0), rightConn, Vec3(2.5,0,0), 5);
    //Constraint::Rod rodo(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-2.5,1,0), 2);

    // Add visualization line (orange=spring, black=constraint)
    //viz.addRubberBandLine(crank, Vec3(0,-3,0),
     //                     slider, Vec3(0,-5,0),
       //                   DecorativeLine().setColor(Black).setLineThickness(4));

    //forces.addMobilityConstantForce(leftPendulum, 0, 20);
    //forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
    //forces.addGlobalEnergyDrain(1);

    Force::MobilityConstantForce(forces, crank, 0, 1);
    Force::MobilityLinearDamper(forces, crank, 0, 1.0);


    //Motion::Linear(crank, Vec3(a,b,c)); // crank(t)=at^2+bt+c
    //Motion::Linear lmot(rightConn, Vec3(a,b,c)); // both axes follow
    //lmot.setAxis(1, Vec3(d,e,f));
    //Motion::Orientation(someBall, orientFuncOfT);
    //someBall.prescribeOrientation(orientFunc);
    //Motion::Relax(crank); // acc=vel=0, pos determined by some default relaxation solver

    // Like this, or should this be an Instance-stage mode of every mobilizer?
    //Motion::Lock(crank); // acc=vel=0; pos is discrete or fast
    //Motion::Steady(crank, Vec3(1,2,3)); // acc=0; vel=constant; pos integrated
    //Motion::Steady crankRate(crank, 1); // acc=0; vel=constant, same for each axis; pos integrated
    // or ...
    //crank.lock(state);
    //crank.setMotionType(state, Regular|Discrete|Fast|Prescribed, stage);

    // need a way to register a mobilizer with a particular relaxation solver,
    // switch between dynamic, continuous relaxed, end-of-step relaxed, discrete.
    // what about a "local" (explicit) relaxation, like q=(q1+q2)/2 ?


    State s = mbs.realizeTopology(); // returns a reference to the the default state
    mbs.realizeModel(s); // define appropriate states for this System

    //crankRate.setRate(s, 3);
    crank.setAngle(s, 5); //q
    crank.setRate(s, 3);  //u

    Visualizer display(mbs);

    mbs.realize(s, Stage::Position);
    display.report(s);
    cout << "q=" << s.getQ() << endl;
    cout << "qErr=" << s.getQErr() << endl;

    crank.setAngle(s, -30*Deg2Rad);
    //crank.setQToFitRotation (s, Rotation::aboutZ(-.9*Pi/2));


    //TODO
    //rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));

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

}