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

/**
 * 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.addMeshGeometry("checkered_floor.vtp");

        // create linkage body
        double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
        
        Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
        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);
        
        // Graphical representation
        Cylinder cyl;
        cyl.set_scale_factors(linkageDimensions);
        Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl1Frame->setName("Cyl1_frame");
        osimModel.addFrame(cyl1Frame);
        cyl.setFrameName("Cyl1_frame");
        linkage1->addGeometry(cyl);

        Sphere sphere(0.1);
        linkage1->addGeometry(sphere);
         
        // Creat a second linkage body
        OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
        linkage2->setName("linkage2");
        Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl2Frame->setName("Cyl2_frame");
        osimModel.addFrame(cyl2Frame);
        (linkage2->upd_geometry(0)).setFrameName("Cyl2_frame");
        // Creat 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);
        Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
        block->addGeometry(brick);

        // Create 1 degree-of-freedom pin joints between the bodies to creat 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};
        CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
        ankleCoordinateSet[0].setName("q1");
        ankleCoordinateSet[0].setRange(range);

        CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
        kneeCoordinateSet[0].setName("q2");
        kneeCoordinateSet[0].setRange(range);

        CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
        hipCoordinateSet[0].setName("q3");
        hipCoordinateSet[0].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 contraints 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);
//.........这里部分代码省略.........

/**
 * 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 BODY

        // 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.addMeshGeometry("ground.vtp");
        ground.addMeshGeometry("anchor1.vtp");
        ground.addMeshGeometry("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
        Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
        block->addGeometry(brick);

        // FREE JOINT

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

        // Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
        CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();

        // Set the angle and position ranges for the coordinate set
        double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
        double positionRange[2] = {-1, 1};
        jointCoordinateSet[0].setRange(angleRange);
        jointCoordinateSet[1].setRange(angleRange);
        jointCoordinateSet[2].setRange(angleRange);
        jointCoordinateSet[3].setRange(positionRange);
        jointCoordinateSet[4].setRange(positionRange);
        jointCoordinateSet[5].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
        // Obtaine 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);
//.........这里部分代码省略.........