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

/**
* This test verifies the use of BodyActuator for applying spatial forces to a selected
* body. It checks if using a BodyActuator generates equivalent acceleration compared 
* to when applying the forces via mobilityForce.
*
* @author Soha Pouya
*/
void testBodyActuator()
{
    using namespace SimTK;
    // start timing
    std::clock_t startTime = std::clock();

    // Setup OpenSim model
    Model *model = new Model;

    // turn off gravity
    model->setGravity(Vec3(0));

    //OpenSim body 1: Ground
    const Ground& ground = model->getGround();

    // OpenSim body 2: A Block
    // Geometrical/Inertial properties for the block
    double blockMass = 1.0, blockSideLength = 1;
    Vec3 blockMassCenter(0);
    Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2,
        blockSideLength/2, blockSideLength/2); // for the halves see doxygen for brick 

    OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
                                             blockMassCenter, blockInertia);

    // Add display geometry to the block to visualize in the GUI
    block->attachGeometry(new Brick(Vec3(blockSideLength/2,
                                     blockSideLength/2, 
                                     blockSideLength/2)));

    Vec3 locationInParent(0, blockSideLength / 2, 0), orientationInParent(0), 
        locationInBody(0), orientationInBody(0);
    FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundBall", 
        ground, locationInParent, orientationInParent, 
        *block, locationInBody, orientationInBody);
    
    model->addBody(block);
    model->addJoint(blockToGroundFree);
    
    // specify magnitude and direction of applied force and torque
    double forceMag = 1.0;
    Vec3 forceAxis(1, 1, 1);
    Vec3 forceInG = forceMag * forceAxis;

    double torqueMag = 1.0;
    Vec3 torqueAxis(1, 1, 1);
    Vec3 torqueInG = torqueMag*torqueAxis;

    // ---------------------------------------------------------------------------
    // Use MobilityForces to Apply the given Torques and Forces to the body
    // ---------------------------------------------------------------------------
    State& state = model->initSystem();

    model->getMultibodySystem().realize(state, Stage::Dynamics);
    Vector_<SpatialVec>& bodyForces =
        model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
    bodyForces.dump("Body Forces before applying 6D spatial force:");

    model->getMatterSubsystem().addInBodyTorque(state, block->getMobilizedBodyIndex(),
        torqueInG, bodyForces);
    model->getMatterSubsystem().addInStationForce(state, block->getMobilizedBodyIndex(),
        Vec3(0), forceInG, bodyForces);

    bodyForces.dump("Body Forces after applying 6D spatial force to the block");

    model->getMultibodySystem().realize(state, Stage::Acceleration);
    Vector udotBody = state.getUDot();
    udotBody.dump("Accelerations due to body forces");

    // clear body forces
    bodyForces *= 0;

    // update mobility forces
    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[i] = torqueInG[i];
        mobilityForces[i+3] = forceInG[i];
    }
    mobilityForces.dump("Mobility Forces after applying 6D spatial force to the block");


    model->getMultibodySystem().realize(state, Stage::Acceleration);
    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], SimTK::Eps);
//.........这里部分代码省略.........

/**
* This test verifies if using a BodyActuator generates equivalent result in the body
* acceleration compared to when using a combination of PointActuaor, TorqueActuaor 
* and BodyActuator. 
* It therefore also verifies model consistency when user defines and uses a 
* combination of these 3 actuators. 
* 
* @author Soha Pouya
*/
void testActuatorsCombination()
{
    using namespace SimTK;
    // start timing
    std::clock_t startTime = std::clock();

    // Setup OpenSim model
    Model *model = new Model;

    // turn off gravity
    model->setGravity(Vec3(0));

    // OpenSim bodies: 1) The ground
    const Ground& ground = model->getGround();
    //ground.addDisplayGeometry("block.vtp");

    // OpenSim bodies: 2) A Block
    // Geometrical/Inertial properties for the block
    double blockMass = 1.0, blockSideLength = 1.0;
    Vec3 blockMassCenter(0);
    // Brick Inertia: for the halves see doxygen  
    Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2, 
                                    blockSideLength/2, blockSideLength/2);
    std::cout << "blockInertia: " << blockInertia << std::endl;

    OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
                                    blockMassCenter, blockInertia);

    // Add display geometry to the block to visualize in the GUI
    block->attachGeometry(new Brick(Vec3(blockSideLength/2, 
                                     blockSideLength/2, 
                                     blockSideLength/2)));

    // Make a FreeJoint from block to ground
    Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), //locationInParent(0, blockSideLength, 0)
         locationInBody(0), orientationInBody(0);
    FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundFreeJoint", 
                ground, locationInParent, orientationInParent, 
                *block, locationInBody, orientationInBody);

    // Add the body and joint to the model
    model->addBody(block);
    model->addJoint(blockToGroundFree);

    // specify magnitude and direction of desired force and torque vectors to apply
    double forceMag = 1.0;
    Vec3 forceAxis(1, 1, 1);
    SimTK::UnitVec3 forceUnitAxis(forceAxis); // to normalize
    Vec3 forceInG = forceMag * forceUnitAxis;

    double torqueMag = 1.0;
    Vec3 torqueAxis(1, 2, 1);
    SimTK::UnitVec3 torqueUnitAxis(torqueAxis); // to normalize
    Vec3 torqueInG = torqueMag*torqueUnitAxis;

    // needed to be called here once to build controller for body actuator
    /*State& state = */model->initSystem();
    
    // ---------------------------------------------------------------------------
    // Add a set of PointActuator, TorqueActuator and BodyActuator to the model
    // ---------------------------------------------------------------------------
    // Create and add a body actuator to the model
    BodyActuator* bodyActuator1 = new BodyActuator(*block);
    bodyActuator1->setName("BodyAct1");
    bodyActuator1->set_point(Vec3(0, blockSideLength/2, 0));
    model->addForce(bodyActuator1);
    
    // Create and add a torque actuator to the model
    TorqueActuator* torqueActuator =
        new TorqueActuator(*block, ground, torqueUnitAxis, true);
    torqueActuator->setName("torqueAct");
    model->addForce(torqueActuator);

    // Create and add a point actuator to the model
    PointActuator* pointActuator =
        new PointActuator("block");
    pointActuator->setName("pointAct");
    pointActuator->set_direction(forceUnitAxis);
    pointActuator->set_point(Vec3(0, blockSideLength/2,0));
    model->addForce(pointActuator);

    // ------ build the model -----
    model->print("TestActuatorCombinationModel.osim");
    model->setUseVisualizer(false);

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

    // ------------------- Provide control signals for bodyActuator ----------------
    // Get the default control vector of the model
    Vector modelControls = model->getDefaultControls();
//.........这里部分代码省略.........

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

/**
 * Run a simulation of a sliding block being pulled by two muscle 
 */
int main()
{
    std::clock_t startTime = std::clock();

    try {
        ///////////////////////////////////////////////
        // DEFINE THE SIMULATION START AND END TIMES //
        ///////////////////////////////////////////////
        // Define the initial and final simulation times
        double initialTime = 0.0;
        double finalTime = 10.0;

        ///////////////////////////////////////////
        // DEFINE BODIES AND JOINTS OF THE MODEL //
        ///////////////////////////////////////////
        // Create an OpenSim model and set its name
        Model osimModel;
        osimModel.setName("tugOfWar");

        // GROUND FRAME

        // Get a reference to the model's ground body
        Ground& ground = osimModel.updGround();

        // Add display geometry to the ground to visualize in the GUI
        ground.attachGeometry(new Mesh("ground.vtp"));
        ground.attachGeometry(new Mesh("anchor1.vtp"));
        ground.attachGeometry(new Mesh("anchor2.vtp"));

        // BLOCK BODY

        // Specify properties of a 20 kg, 10cm length block body
        double blockMass = 20.0, blockSideLength = 0.1;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, 
            blockSideLength, blockSideLength);

        // Create a new block body with the specified properties
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
            blockMassCenter, blockInertia);

        // Add display geometry to the block to visualize in the GUI
        block->attachGeometry(new Mesh("block.vtp"));

        // FREE JOINT

        // Create a new free joint with 6 degrees-of-freedom (coordinates) 
        // between the block and ground bodies
        double halfLength = blockSideLength/2.0;
        Vec3 locationInParent(0, halfLength, 0), orientationInParent(0);
        Vec3 locationInBody(0, halfLength, 0), orientationInBody(0);
        FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, 
            locationInParent, orientationInParent, 
            *block, locationInBody, orientationInBody);

        // Set the angle and position ranges for the free (6-degree-of-freedom)
        // joint between the block and ground frames.
        double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
        double positionRange[2] = {-1, 1};
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation1X).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation2Y).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation3Z).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationX).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationY).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationZ).setRange(positionRange);

        // Add the block body to the model
        osimModel.addBody(block);
        osimModel.addJoint(blockToGround);

        ///////////////////////////////////////
        // DEFINE FORCES ACTING ON THE MODEL //
        ///////////////////////////////////////
        // MUSCLE FORCES

        // Create two new muscles
        double maxIsometricForce = 1000.0, optimalFiberLength = 0.2, 
               tendonSlackLength = 0.1,    pennationAngle = 0.0,  
               fatigueFactor = 0.30, recoveryFactor = 0.20;

        // fatigable muscle (Millard2012EquilibriumMuscle with fatigue)
        FatigableMuscle* fatigable = new FatigableMuscle("fatigable",
            maxIsometricForce, optimalFiberLength, tendonSlackLength, 
            pennationAngle, fatigueFactor, recoveryFactor);

        // original muscle model (muscle without fatigue)
        Millard2012EquilibriumMuscle* original = 
            new Millard2012EquilibriumMuscle("original",
                maxIsometricForce, optimalFiberLength, tendonSlackLength,
                pennationAngle);

        // Define the path of the muscles
        fatigable->addNewPathPoint("fatigable-point1", ground, 
            Vec3(0.0, halfLength, -0.35));
        fatigable->addNewPathPoint("fatigable-point2", *block, 
            Vec3(0.0, halfLength, -halfLength));

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

/**
 * Method for building the Luxo Jr articulating model. It sets up the system of
 * rigid bodies and joint articulations to define Luxo Jr lamp geometry.
 */
void createLuxoJr(OpenSim::Model &model){
    
    // Create base
    //--------------
    OpenSim::Body* base = new OpenSim::Body("base", baseMass, Vec3(0.0),
                Inertia::cylinderAlongY(0.1, baseHeight));
    
    // Add visible geometry
    base->attachGeometry(new Mesh("Base_meters.obj"));
    
    
    // Define base to float relative to ground via free joint
    FreeJoint* base_ground = new FreeJoint("base_ground",
                // parent body, location in parent body, orientation in parent
                model.getGround(), Vec3(0.0), Vec3(0.0),
                // child body, location in child body, orientation in child
                *base, Vec3(0.0,-baseHeight/2.0,0.0),Vec3(0.0));
    
    // add base to model
    model.addBody(base); model.addJoint(base_ground);
    
    /*for (int i = 0; i<base_ground->get_CoordinateSet().getSize(); ++i) {
        base_ground->upd_CoordinateSet()[i].set_locked(true);
    }*/
    
    // Fix a frame to the base axis for attaching the bottom bracket
    SimTK::Transform* shift_and_rotate = new SimTK::Transform();
    //shift_and_rotate->setToZero();
    shift_and_rotate->set(Rotation(-1*SimTK::Pi/2,
                                   SimTK::CoordinateAxis::XCoordinateAxis()),
                          Vec3(0.0, bracket_location, 0.0));

    PhysicalOffsetFrame pivot_frame_on_base("pivot_frame_on_base",
            *base, *shift_and_rotate);

    // Create bottom bracket
    //-----------------------
    OpenSim::Body* bottom_bracket = new OpenSim::Body("bottom_bracket",
                                            bracket_mass, Vec3(0.0),
                                            Inertia::brick(0.03, 0.03, 0.015));
    // add bottom bracket to model
    model.addBody(bottom_bracket);
    
    // Fix a frame to the bracket for attaching joint
    shift_and_rotate->setP(Vec3(0.0));
    PhysicalOffsetFrame pivot_frame_on_bottom_bracket(
        "pivot_frame_on_bottom_bracket", *bottom_bracket, *shift_and_rotate);
    
    // Add visible geometry
    bottom_bracket->attachGeometry(new Mesh("bottom_bracket_meters.obj"));

    // Make bottom bracket to twist on base with vertical pin joint.
    // You can create a joint from any existing physical frames attached to
    // rigid bodies. One way to reference them is by name, like this...
    PinJoint* base_pivot = new PinJoint("base_pivot", pivot_frame_on_base,
                                        pivot_frame_on_bottom_bracket);
    
    base_pivot->append_frames(pivot_frame_on_base);
    base_pivot->append_frames(pivot_frame_on_bottom_bracket);
    // add base pivot joint to the model
    model.addJoint(base_pivot);
    
    // add some damping to the pivot
    // initialized to zero stiffness and damping
    BushingForce* pivotDamper = new BushingForce("pivot_bushing",
                                         "pivot_frame_on_base",
                                         "pivot_frame_on_bottom_bracket");
    
    pivotDamper->set_rotational_damping(pivot_damping);
    
    model.addForce(pivotDamper);
    
    // Create posterior leg
    //-----------------------
    OpenSim::Body* posteriorLegBar = new OpenSim::Body("posterior_leg_bar",
                                    bar_mass,
                                    Vec3(0.0),
                                    Inertia::brick(leg_bar_dimensions/2.0));
    
    posteriorLegBar->attachGeometry(new Mesh("Leg_meters.obj"));

    PhysicalOffsetFrame posterior_knee_on_bottom_bracket(
        "posterior_knee_on_bottom_bracket",
            *bottom_bracket, Transform(posterior_bracket_hinge_location) );

    PhysicalOffsetFrame posterior_knee_on_posterior_bar(
        "posterior_knee_on_posterior_bar",
            *posteriorLegBar, Transform(inferior_bar_hinge_location) );

    // Attach posterior leg to bottom bracket using another pin joint.
    // Another way to reference physical frames in a joint is by creating them
    // in place, like this...
    OpenSim::PinJoint* posteriorKnee = new OpenSim::PinJoint("posterior_knee",
                                    posterior_knee_on_bottom_bracket,
                                    posterior_knee_on_posterior_bar);
    // posteriorKnee will own and serialize the attachment offset frames 
//.........这里部分代码省略.........

/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact 
 */
int main()
{
    try {
        // Create a new OpenSim model
        Model osimModel;
        osimModel.setName("osimModel");
        osimModel.setAuthors("Matt DeMers");

        double Pi = SimTK::Pi;
            
        // Get the ground body
        Ground& ground = osimModel.updGround();
        ground.attachGeometry(new Mesh("checkered_floor.vtp"));

        // create linkage body
        double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
        
        Vec3 linkageMassCenter(0,linkageLength/2,0);
        Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);

        OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);
        linkage1->attachGeometry(new Sphere(0.1));

        // Graphical representation
        Cylinder cyl(linkageDiameter/2, linkageLength);
        Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1, 
            Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl1Frame->setName("Cyl1_frame");
        cyl1Frame->attachGeometry(cyl.clone());
        osimModel.addFrame(cyl1Frame);

        // Create a second linkage body as a clone of the first
        OpenSim::Body* linkage2 = linkage1->clone();
        linkage2->setName("linkage2");
        Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2,
            Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl2Frame->setName("Cyl2_frame");
        osimModel.addFrame(cyl2Frame);

        // Create a block to be the pelvis
        double blockMass = 20.0, blockSideLength = 0.2;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
        block->attachGeometry(new Brick(SimTK::Vec3(0.05, 0.05, 0.05)));

        // Create 1 degree-of-freedom pin joints between the bodies to create a kinematic chain from ground through the block
        Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);

        PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1, 
            locationInChild, orientationInChild);

        PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
            locationInChild, orientationInChild);

        PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
            locationInChild, orientationInChild);
        
        double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
        ankle->updCoordinate().setName("q1");
        ankle->updCoordinate().setRange(range);

        knee->updCoordinate().setName("q2");
        knee->updCoordinate().setRange(range);

        hip->updCoordinate().setName("q3");
        hip->updCoordinate().setRange(range);

        // Add the bodies to the model
        osimModel.addBody(linkage1);
        osimModel.addBody(linkage2);
        osimModel.addBody(block);

        // Add the joints to the model
        osimModel.addJoint(ankle);
        osimModel.addJoint(knee);
        osimModel.addJoint(hip);
        // Define constraints on the model
        //  Add a point on line constraint to limit the block to vertical motion

        Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
        PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
        osimModel.addConstraint(lineConstraint);

        // Add PistonActuator between the first linkage and the block
        Vec3 pointOnBodies(0);
        PistonActuator *piston = new PistonActuator();
        piston->setName("piston");
        piston->setBodyA(linkage1);
        piston->setBodyB(block);
        piston->setPointA(pointOnBodies);
        piston->setPointB(pointOnBodies);
        piston->setOptimalForce(200.0);
        piston->setPointsAreGlobal(false);

        osimModel.addForce(piston);
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//.........这里部分代码省略.........

/**
 * Create a model that does nothing. 
 */
int main()
{
    try {

        ///////////////////////////////////////////
        // DEFINE BODIES AND JOINTS OF THE MODEL //
        ///////////////////////////////////////////

        // Create an OpenSim model and set its name
        Model osimModel;
        osimModel.setName("tugOfWar");

        // GROUND FRAME

        // Get a reference to the model's ground frame
        Ground& ground = osimModel.updGround();

        // Attach geometry to the ground to visualize in the GUI
        ground.attachGeometry(new Mesh("ground.vtp"));
        ground.attachGeometry(new Mesh("anchor1.vtp"));
        ground.attachGeometry(new Mesh("anchor2.vtp"));

        // BLOCK BODY

        // Specify properties of a 20 kg, 0.1 m^3 block body
        double blockMass = 20.0, blockSideLength = 0.1;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);

        // Create a new block body with the specified properties
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);

        // Add display geometry to the block to visualize in the GUI
        block->attachGeometry(new Brick(SimTK::Vec3(0.05, 0.05, 0.05)));

        // FREE JOINT

        // Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground frames
        Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
        FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
        
        // Set the angle and position ranges for the free (6-degree-of-freedom)
        // joint between the block and ground frames.
        double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
        double positionRange[2] = {-1, 1};
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation1X).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation2Y).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::Rotation3Z).setRange(angleRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationX).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationY).setRange(positionRange);
        blockToGround->updCoordinate(FreeJoint::Coord::TranslationZ).setRange(positionRange);

        // Add the block body to the model
        osimModel.addBody(block);
        osimModel.addJoint(blockToGround);

        ///////////////////////////////////////////////
        // DEFINE THE SIMULATION START AND END TIMES //
        ///////////////////////////////////////////////

        // Define the initial and final simulation times
        double initialTime = 0.0;
        double finalTime = 3.00;

        /////////////////////////////////////////////
        // DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
        /////////////////////////////////////////////

        // Specify properties of a constant distance constraint to limit the block's motion
        double distance = 0.2;
        Vec3 pointOnGround(0, blockSideLength/2 ,0);
        Vec3 pointOnBlock(0, 0, 0);

        // Create a new constant distance constraint
        ConstantDistanceConstraint *constDist = new ConstantDistanceConstraint(ground, 
                                        pointOnGround, *block, pointOnBlock, distance);

        // Add the new point on a line constraint to the model
        osimModel.addConstraint(constDist);

        ///////////////////////////////////////
        // DEFINE FORCES ACTING ON THE MODEL //
        ///////////////////////////////////////

        // GRAVITY
        // Obtain the default acceleration due to gravity
        Vec3 gravity = osimModel.getGravity();
    

        // MUSCLE FORCES
        // Create two new muscles with identical properties
        double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0; 
        Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
        Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);

        // Specify the paths for the two muscles
        // Path for muscle 1
//.........这里部分代码省略.........