C++ PrescribedController::addActuator方法代码示例

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);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    kneeExtensorLeft->set_ignore_tendon_compliance(true);
    model.addForce(kneeExtensorLeft);
    
    // add a back extensor to controll the upper 4-bar linkage
    Millard2012EquilibriumMuscle* backExtensorRight = new Millard2012EquilibriumMuscle(
                                            "back_extensor_right",
                                            back_extensor_F0, back_extensor_lm0,
                                            back_extensor_lts, pennationAngle);
    
    backExtensorRight->addNewPathPoint("back_extensor_right_origin", *chest,
                                      back_extensor_origin);
    backExtensorRight->addNewPathPoint("back_extensor_right_insertion", *back,
                                      back_extensor_insertion);
    backExtensorRight->set_ignore_tendon_compliance(true);
    model.addForce(backExtensorRight);
    
    // copy right back extensor and use to make left extensor
    Millard2012EquilibriumMuscle* backExtensorLeft =
            new Millard2012EquilibriumMuscle(*backExtensorRight);
    
    backExtensorLeft->setName("back_extensor_left");
    
    PathPointSet& pointsLeft = backExtensorLeft->updGeometryPath()
        .updPathPointSet();
    for (int i=0; i<points.getSize(); ++i) {
        pointsLeft[i].setLocationCoord(2, -1*pointsLeft[i].getLocationCoord(2));
    }
    backExtensorLeft->set_ignore_tendon_compliance(true);
    model.addForce(backExtensorLeft);
    
    
    
    // MUSCLE CONTROLLERS
    //________________________________________________________________________
    
    // specify a piecwise linear function for the muscle excitations
    PiecewiseConstantFunction* x_of_t = new PiecewiseConstantFunction(3, times,
                                                                  excitations);
    
    
    PrescribedController* kneeController = new PrescribedController();
    kneeController->addActuator(*kneeExtensorLeft);
    kneeController->addActuator(*kneeExtensorRight);
    kneeController->prescribeControlForActuator(0, x_of_t);
    kneeController->prescribeControlForActuator(1, x_of_t->clone());
    
    model.addController(kneeController);
    
    PrescribedController* backController = new PrescribedController();
    backController->addActuator(*backExtensorLeft);
    backController->addActuator(*backExtensorRight);
    backController->prescribeControlForActuator(0, x_of_t->clone());
    backController->prescribeControlForActuator(1, x_of_t->clone());
    
    model.addController(backController);

    
    /* You'll find that these muscles can make Luxo Myo stand, but not jump.
     * Jumping will require an assistive device. We'll add two frames for
     * attaching a point to point assistive actuator.
     */
    
    
    // add frames for connecting a back assitance device between the chest
    // and pelvis
    PhysicalOffsetFrame* back_assist_origin_frame = new
        PhysicalOffsetFrame("back_assist_origin",
                            *chest,
                            back_assist_origin_transform);
    
    PhysicalOffsetFrame* back_assist_insertion_frame = new
        PhysicalOffsetFrame("back_assist_insertion",
                            *pelvisBracket,
                            back_assist_insertion_transform);
    
    model.addFrame(back_assist_origin_frame);
    model.addFrame(back_assist_insertion_frame);
    
    // add frames for connecting a knee assistance device between the posterior
    // leg and bottom bracket.
    PhysicalOffsetFrame* knee_assist_origin_frame = new
    PhysicalOffsetFrame("knee_assist_origin",
                        *posteriorLegBar,
                        knee_assist_origin_transform);
    
    PhysicalOffsetFrame* knee_assist_insertion_frame = new
    PhysicalOffsetFrame("knee_assist_insertion",
                        *bottom_bracket,
                        knee_assist_insertion_transform);
    
    model.addFrame(knee_assist_origin_frame);
    model.addFrame(knee_assist_insertion_frame);

    // Temporary: make the frame geometry disappear.
    for (auto& c : model.getComponentList<OpenSim::FrameGeometry>()) {
        const_cast<OpenSim::FrameGeometry*>(&c)->set_scale_factors(
                SimTK::Vec3(0.001, 0.001, 0.001));
    }
    
}

//.........这里部分代码省略.........
    Vector& mobilityForces = model->getMultibodySystem()
        .updMobilityForces(state, Stage::Dynamics);

    // Apply torques as mobility forces of the ball joint
    for(int i=0; i<3; ++i){
        mobilityForces[6+i] = torqueInG[i]; 
    }

    model->getMultibodySystem().realize(state, Stage::Acceleration);
    const Vector& udotMobility = state.getUDot();
    udotMobility.dump("Accelerations due to mobility forces");

    // First make sure that accelerations are not zero accidentally
    ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
    // Then check if they are equal
    for(int i=0; i<udotMobility.size(); ++i){
        ASSERT_EQUAL(udotMobility[i], udotBody[i], 1.0e-12);
    }

    // clear the mobility forces
    mobilityForces = 0;

    //Now add the actuator to the model and control it to generate the same
    //torque as applied directly to the multibody system (above)

    // Create and add the torque actuator to the model
    TorqueActuator* actuator =
        new TorqueActuator(*bodyA, *bodyB, torqueAxis, true);
    actuator->setName("torque");
    model->addForce(actuator);

    // Create and add a controller to control the actuator
    PrescribedController* controller =  new PrescribedController();
    controller->addActuator(*actuator);
    // Apply torque about torqueAxis
    controller->prescribeControlForActuator("torque", new Constant(torqueMag));

    model->addController(controller);

    /*
    ActuatorPowerProbe* powerProbe = new ActuatorPowerProbe(Array<string>("torque",1),false, 1); 
    powerProbe->setOperation("integrate");
    powerProbe->setInitialConditions(Vector(1, 0.0));
    */

    //model->addProbe(powerProbe);

    model->print("TestTorqueActuatorModel.osim");
    model->setUseVisualizer(false);

    // get a new system and state to reflect additions to the model
    state = model->initSystem();

    model->computeStateVariableDerivatives(state);

    const Vector &udotTorqueActuator = state.getUDot();

    // First make sure that accelerations are not zero accidentally
    ASSERT(udotMobility.norm() != 0.0 || udotTorqueActuator.norm() != 0.0);

    // Then verify that the TorqueActuator also generates the same acceleration
    // as the equivalent applied mobility force
    for(int i=0; i<udotMobility.size(); ++i){
        ASSERT_EQUAL(udotMobility[i], udotTorqueActuator[i], 1.0e-12);
    }

int main() {
    Model model;
    model.setName("bicep_curl");
#ifdef VISUALIZE
    model.setUseVisualizer(true);
#endif

    // Create two links, each with a mass of 1 kg, center of mass at the body's
    // origin, and moments and products of inertia of zero.
    OpenSim::Body* humerus = new OpenSim::Body("humerus", 1, Vec3(0), Inertia(0));
    OpenSim::Body* radius  = new OpenSim::Body("radius",  1, Vec3(0), Inertia(0));

    // Connect the bodies with pin joints. Assume each body is 1 m long.
    PinJoint* shoulder = new PinJoint("shoulder",
            // Parent body, location in parent, orientation in parent.
            model.getGround(), Vec3(0), Vec3(0),
            // Child body, location in child, orientation in child.
            *humerus, Vec3(0, 1, 0), Vec3(0));
    PinJoint* elbow = new PinJoint("elbow",
            *humerus, Vec3(0), Vec3(0), *radius, Vec3(0, 1, 0), Vec3(0));

    // Add a muscle that flexes the elbow.
    Millard2012EquilibriumMuscle* biceps = new
        Millard2012EquilibriumMuscle("biceps", 200, 0.6, 0.55, 0);
    biceps->addNewPathPoint("origin",    *humerus, Vec3(0, 0.8, 0));
    biceps->addNewPathPoint("insertion", *radius,  Vec3(0, 0.7, 0));

    // Add a controller that specifies the excitation of the muscle.
    PrescribedController* brain = new PrescribedController();
    brain->addActuator(*biceps);
    // Muscle excitation is 0.3 for the first 0.5 seconds, then increases to 1.
    brain->prescribeControlForActuator("biceps",
            new StepFunction(0.5, 3, 0.3, 1));

    // Add components to the model.
    model.addBody(humerus);    model.addBody(radius);
    model.addJoint(shoulder);  model.addJoint(elbow);
    model.addForce(biceps);
    model.addController(brain);

    // Add a console reporter to print the muscle fiber force and elbow angle.
    ConsoleReporter* reporter = new ConsoleReporter();
    reporter->set_report_time_interval(1.0);
    reporter->addToReport(biceps->getOutput("fiber_force"));
    reporter->addToReport(
        elbow->getCoordinate(PinJoint::Coord::RotationZ).getOutput("value"),
        "elbow_angle");
    model.addComponent(reporter);

    // Add display geometry.
    Ellipsoid bodyGeometry(0.1, 0.5, 0.1);
    bodyGeometry.setColor(Gray);
    // Attach an ellipsoid to a frame located at the center of each body.
    PhysicalOffsetFrame* humerusCenter = new PhysicalOffsetFrame(
        "humerusCenter", *humerus, Transform(Vec3(0, 0.5, 0)));
    humerus->addComponent(humerusCenter);
    humerusCenter->attachGeometry(bodyGeometry.clone());
    PhysicalOffsetFrame* radiusCenter = new PhysicalOffsetFrame(
        "radiusCenter", *radius, Transform(Vec3(0, 0.5, 0)));
    radius->addComponent(radiusCenter);
    radiusCenter->attachGeometry(bodyGeometry.clone());

    // Configure the model.
    State& state = model.initSystem();
    // Fix the shoulder at its default angle and begin with the elbow flexed.
    shoulder->getCoordinate().setLocked(state, true);
    elbow->getCoordinate().setValue(state, 0.5 * Pi);
    model.equilibrateMuscles(state);

    // Configure the visualizer.
#ifdef VISUALIZE
    model.updMatterSubsystem().setShowDefaultGeometry(true);
    Visualizer& viz = model.updVisualizer().updSimbodyVisualizer();
    viz.setBackgroundType(viz.SolidColor);
    viz.setBackgroundColor(White);
#endif

    // Simulate.
    simulate(model, state, 10.0);

    return 0;
};