C++ Body::scale方法代码示例

void testMcKibbenActuator()
{

    double pressure = 5 * 10e5; // 5 bars
    double num_turns = 1.5;     // 1.5 turns
    double B = 277.1 * 10e-4;  // 277.1 mm

    using namespace SimTK;
    std::clock_t startTime = std::clock();

    double mass = 1;
    double ball_radius = 10e-6;

    Model *model = new Model;
    model->setGravity(Vec3(0));

    Ground& ground = model->updGround();

    McKibbenActuator *actuator = new McKibbenActuator("mckibben", num_turns, B);
    
    OpenSim::Body* ball = new OpenSim::Body("ball", mass ,Vec3(0),  mass*SimTK::Inertia::sphere(0.1));
    ball->scale(Vec3(ball_radius), false);

    actuator->addNewPathPoint("mck_ground", ground, Vec3(0));
    actuator->addNewPathPoint("mck_ball", *ball, Vec3(ball_radius));

    Vec3 locationInParent(0, ball_radius, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
    SliderJoint *ballToGround = new SliderJoint("ballToGround", ground, locationInParent, orientationInParent, *ball, locationInBody, orientationInBody);

    ballToGround->updCoordinate().setName("ball_d");
    ballToGround->updCoordinate().setPrescribedFunction(LinearFunction(20 * 10e-4, 0.5 * 264.1 * 10e-4));
    ballToGround->updCoordinate().set_prescribed(true);

    model->addBody(ball);
    model->addJoint(ballToGround);
    model->addForce(actuator);

    PrescribedController* controller =  new PrescribedController();
    controller->addActuator(*actuator);
    controller->prescribeControlForActuator("mckibben", new Constant(pressure));

    model->addController(controller);

    ForceReporter* reporter = new ForceReporter(model);
    model->addAnalysis(reporter);
    
    SimTK::State& si = model->initSystem();

    model->getMultibodySystem().realize(si, Stage::Position);

    double final_t = 10.0;
    double nsteps = 10;
    double dt = final_t / nsteps;

    RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
    integrator.setAccuracy(1e-7);
    Manager manager(*model, integrator);
    manager.setInitialTime(0.0);

    for (int i = 1; i <= nsteps; i++){
        manager.setFinalTime(dt*i);
        manager.integrate(si);
        model->getMultibodySystem().realize(si, Stage::Velocity);
        Vec3 pos;
        model->updSimbodyEngine().getPosition(si, *ball, Vec3(0), pos);

        double applied = actuator->computeActuation(si);;

        double theoretical = (pressure / (4* pow(num_turns,2) * SimTK::Pi)) * (3*pow(pos(0), 2) - pow(B, 2));

        ASSERT_EQUAL(applied, theoretical, 10.0);

        manager.setInitialTime(dt*i);
    }


    std::cout << " ******** Test McKibbenActuator time = ********" <<
        1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}

void testClutchedPathSpring()
{
    using namespace SimTK;

    // start timing
    std::clock_t startTime = std::clock();

    double mass = 1;
    double stiffness = 100;
    double dissipation = 0.3;
    double start_h = 0.5;
    //double ball_radius = 0.25;

    //double omega = sqrt(stiffness/mass);

    // Setup OpenSim model
    Model* model = new Model;
    model->setName("ClutchedPathSpringModel");
    model->setGravity(gravity_vec);

    //OpenSim bodies
    const Ground* ground = &model->getGround();
    
    // body that acts as the pulley that the path wraps over
    OpenSim::Body* pulleyBody =
        new OpenSim::Body("PulleyBody", mass ,Vec3(0),  mass*Inertia::brick(0.1, 0.1, 0.1));
    
    // body the path spring is connected to at both ends
    OpenSim::Body* block =
        new OpenSim::Body("block", mass ,Vec3(0),  mass*Inertia::brick(0.2, 0.1, 0.1));
    block->attachGeometry(new Brick(Vec3(0.2, 0.1, 0.1)));
    block->scale(Vec3(0.2, 0.1, 0.1), false);

    //double dh = mass*gravity_vec(1)/stiffness;
    
    WrapCylinder* pulley = new WrapCylinder();
    pulley->set_radius(0.1);
    pulley->set_length(0.05);

    // Add the wrap object to the body, which takes ownership of it
    pulleyBody->addWrapObject(pulley);

    // Add joints
    WeldJoint* weld = 
        new WeldJoint("weld", *ground, Vec3(0, 1.0, 0), Vec3(0), *pulleyBody, Vec3(0), Vec3(0));
    
    SliderJoint* slider =
        new SliderJoint("slider", *ground, Vec3(0), Vec3(0,0,Pi/2),*block, Vec3(0), Vec3(0,0,Pi/2));

    double positionRange[2] = {-10, 10};
    // Rename coordinates for a slider joint
    slider->updCoordinate().setName("block_h");
    slider->updCoordinate().setRange(positionRange);

    model->addBody(pulleyBody);
    model->addJoint(weld);

    model->addBody(block);
    model->addJoint(slider);

    ClutchedPathSpring* spring = 
        new ClutchedPathSpring("clutch_spring", stiffness, dissipation, 0.01);

    spring->updGeometryPath().appendNewPathPoint("origin", *block, Vec3(-0.1, 0.0 ,0.0));
    
    int N = 10;
    for(int i=1; i<N; ++i){
        double angle = i*Pi/N;
        double x = 0.1*cos(angle);
        double y = 0.1*sin(angle);
        spring->updGeometryPath().appendNewPathPoint("", *pulleyBody, Vec3(-x, y ,0.0));
    }

    spring->updGeometryPath().appendNewPathPoint("insertion", *block, Vec3(0.1, 0.0 ,0.0));

    // BUG in defining wrapping API requires that the Force containing the GeometryPath be
    // connected to the model before the wrap can be added
    model->addForce(spring);

    PrescribedController* controller = new PrescribedController();
    controller->addActuator(*spring);
    
    // Control greater than 1 or less than 0 should be treated as 1 and 0 respectively.
    double     timePts[4] = {0.0,  5.0, 6.0, 10.0};
    double clutchOnPts[4] = {1.5, -2.0, 0.5,  0.5};

    PiecewiseConstantFunction* controlfunc = 
        new PiecewiseConstantFunction(4, timePts, clutchOnPts);

    controller->prescribeControlForActuator("clutch_spring", controlfunc);
    model->addController(controller);

    model->print("ClutchedPathSpringModel.osim");

    //Test deserialization
    delete model;
    model = new Model("ClutchedPathSpringModel.osim");

    // Create the force reporter
    ForceReporter* reporter = new ForceReporter(model);
//.........这里部分代码省略.........