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

void main_simulation()
#endif
{
    // inputs
    double fitness;

    #ifdef OPTI
    double *optiParams;
    #endif

    Loop_inputs *loop_inputs;

    // initialization
	loop_inputs = init_simulation();

    // optimization init
    #ifdef OPTI

    optiParams = (double*) malloc(NB_PARAMS_TO_OPTIMIZE*sizeof(double));

    get_real_params_to_opt(optiNorms, optiParams);

    erase_for_opti(optiParams, loop_inputs->MBSdata);

    free(optiParams);

    #endif
    
    // -- Simbody -- //

    #ifdef SIMBODY

    // / Create the system. Define all "system" objects - system, matterm forces, tracker, contactForces.
    MultibodySystem           system;
    SimbodyMatterSubsystem    matter(system);
    ContactTrackerSubsystem   tracker(system); 
    CompliantContactSubsystem contactForces(system, tracker);
    system.setUpDirection(+ZAxis); // that is for visualization only. The default direction is +X


    SimbodyVariables simbodyVariables;

    // set all the mechanical parameters of the contact
    
    simbodyVariables.p_system        = &system;
    simbodyVariables.p_matter        = &matter;
    simbodyVariables.p_tracker       = &tracker;
    simbodyVariables.p_contactForces = &contactForces;

    //  cout<<"BoxInd in Main = "<<BoxInd<<" -- should be default \n";
    
    //init_Simbody(&simbodyVariables);

    init_Simbody(&simbodyVariables, loop_inputs->MBSdata->user_IO->simbodyBodies);
    
    //it is "system" commands. We cannot avoid them.    
    system.realizeTopology();
    State state = system.getDefaultState();

    simbodyVariables.p_state = &state;

    //it is "system" command. We cannot avoid them. 
    system.realizeModel(state);

    p_simbodyVariables = &simbodyVariables;

    #endif

	// loop
    fitness = loop_simulation(loop_inputs);

    // end of the simulation
    finish_simulation(loop_inputs);

    #ifdef OPTI

    return fitness;

    #else

    #if defined(PRINT_REPORT)
    printf("fitness: %f\n", fitness);
    #endif

    #endif
}

int main() {
  try
  { // Create the system.
    
    MultibodySystem         system;
    system.setUpDirection(ZAxis);
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity          gravity(forces, matter, -ZAxis, 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-3);

    const Vec3 hdim(.2,.3,.4); // Brick half dimensions
    const Real rad = .1;    // Contact sphere radius
    const Real brickMass = 2;

    #ifdef USE_SHERM_PARAMETERS
        const Real mu_d =.3; // dynamic friction
        const Real mu_s =.3; // static friction
        const Real mu_v = 0;  // viscous friction (1/v)
        const Real dissipation = .1;
        const Real fK = 1e6; // stiffness in pascals
        const Real simDuration = 5.;
    #endif
    #ifdef USE_TOM_PARAMETERS
        const Real mu_d =.3; // dynamic friction
        const Real mu_s =.3; // static friction
        const Real mu_v = 0;  // viscous friction (1/v)
        const Real dissipation = .1756;  //Second impact at 0.685 s.
        const Real fK = 1e6; // stiffness in pascals
        const Real simDuration = 0.5; //3.0; //0.8;
    #endif

    const ContactMaterial material(fK,dissipation,mu_s,mu_d,mu_v);

    // Halfspace floor
    const Rotation R_xdown(Pi/2,YAxis);
    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(), material));

    Body::Rigid brickBody(MassProperties(brickMass, Vec3(0), 
                            UnitInertia::brick(hdim)));
    brickBody.addDecoration(Transform(), 
        DecorativeBrick(hdim).setColor(BrickColor).setOpacity(.7));

    for (int i=-1; i<=1; i+=2)
    for (int j=-1; j<=1; j+=2)
    for (int k=-1; k<=1; k+=2) {
        const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(hdim);
        brickBody.addContactSurface(pt,
            ContactSurface(ContactGeometry::Sphere(rad), material));
        brickBody.addDecoration(pt,
            DecorativeSphere(rad).setColor(SphereColor));
    }

    MobilizedBody::Free brick(matter.Ground(), Transform(),
                              brickBody, Transform());

    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces,
                                                       brick));
    //viz.addFrameController(new BodyWatcher(brick));
    viz.addFrameController(new BodyWatcher(matter.Ground()));
    //viz.setShowSimTime(true);
    //viz.setShowFrameNumber(true);
    viz.setDesiredFrameRate(FrameRate);
    //viz.setShowFrameRate(true);

    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Go", GoItem));
    runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    viz.addMenu("Run", RunMenuId, runMenuItems);

    Array_<std::pair<String,int> > helpMenuItems;
    helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
    viz.addMenu("Help", HelpMenuId, helpMenuItems);

    // Check for a Run->Quit menu pick every 1/4 second.
    //system.addEventHandler(new UserInputHandler(*silo, .25));

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

    // SET INITIAL CONDITIONS
    #ifdef USE_SHERM_PARAMETERS
        brick.setQToFitTranslation(state, Vec3(0,2,.8));
        brick.setQToFitRotation(state, Rotation(BodyRotationSequence, 
                                                Pi/4, XAxis, Pi/6, YAxis));
        brick.setUToFitLinearVelocity(state, Vec3(-5,0,0));
    #endif
    #ifdef USE_TOM_PARAMETERS
//.........这里部分代码省略.........

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

    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity   gravity(forces, matter, UnitVec3(-1,0,0), 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-2); // m/s

    // Ground's normal is +x for this model
    system.setUpDirection(+XAxis);

    // Uncomment this if you want a more elegant movie.
    //matter.setShowDefaultGeometry(false);

    const Real ud = .3; // dynamic
    const Real us = .6; // static
    const Real uv = 0;  // viscous (force/velocity)
    const Real k = 1e8; // pascals
    const Real c = 0.01; // dissipation (1/v)


    // Halfspace default is +x, this one occupies -x instead, so flip.
    const Rotation R_xdown(Pi,ZAxis);

    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(),
                       ContactMaterial(k,c,us,ud,uv)));


    const Real ellipsoidMass = 1; // kg
    const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
    const Vec3 comLoc(-1*Cm2m, 0, 0);
    const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
    const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
    Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));

    ellipsoidBody.addDecoration(Transform(),
        DecorativeEllipsoid(halfDims).setColor(Cyan)
         //.setOpacity(.5)
         .setResolution(3));
    ellipsoidBody.addContactSurface(Transform(),
        ContactSurface(ContactGeometry::Ellipsoid(halfDims),
                       ContactMaterial(k,c,us,ud,uv))
                       );
    MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
        ellipsoidBody, Transform(Vec3(0)));


    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    viz.setMode(Visualizer::RealTime);
    viz.setDesiredFrameRate(FrameRate);
    viz.setCameraClippingPlanes(0.1, 10);

    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Go", GoItem));
    runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    viz.addMenu("Run", RunMenuId, runMenuItems);

    Array_<std::pair<String,int> > helpMenuItems;
    helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
    viz.addMenu("Help", HelpMenuId, helpMenuItems);

    system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
    system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));

    // Check for a Run->Quit menu pick every 1/4 second.
    system.addEventHandler(new UserInputHandler(*silo, .25));

    // Initialize the system and state.

    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    ellipsoid.setQToFitTransform(state, Transform(
        Rotation(BodyRotationSequence,  0  *Deg2Rad, XAxis,
                                        0.5*Deg2Rad, YAxis,
                                       -0.5*Deg2Rad, ZAxis),
        Vec3(2.1*Cm2m, 0, 0)));

    ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s

    viz.report(state);
    printf("Default state\n");

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
//.........这里部分代码省略.........