C++ SimTK::Inertia方法代码示例

// [Step 2, Task C]
Device* buildDevice() {
    using SimTK::Vec3;
    using SimTK::Inertia;

    // Create the device.
    auto device = new Device();
    device->setName("device");

    // The device's mass is distributed between two identical cuffs that attach
    // to the hopper via WeldJoints (to be added below).
    double deviceMass = 2.0;
    auto cuffA = new Body("cuffA", deviceMass/2., Vec3(0), Inertia(0.5));
    //TODO: Repeat for cuffB.

    //TODO: Add the cuff Components to the device.

    // Attach a sphere to each cuff for visualization.
    auto sphere = new Sphere(0.01);
    sphere->setName("sphere");
    sphere->setColor(SimTK::Red);
    sphere->setFrame(*cuffA); cuffA->attachGeometry(sphere);
    //sphere->setFrame(*cuffB); cuffB->attachGeometry(sphere->clone());

    // Create a WeldJoint to anchor cuffA to the hopper.
    auto anchorA = new WeldJoint();
    anchorA->setName("anchorA");
    //TODO: Connect the "child_frame" (a PhysicalFrame) Connector of anchorA to
    //      cuffA. Note that only the child frame is connected now; the parent
    //      frame will be connected in exampleHopperDevice.cpp.

    //TODO: Add anchorA to the device.

    //TODO: Create a WeldJoint to anchor cuffB to the hopper. Connect the
    //      "child_frame" Connector of anchorB to cuffB and add anchorB to the
    //      device.

    // Attach a PathActuator between the two cuffs.
    auto pathActuator = new PathActuator();
    pathActuator->setName("cableAtoB");
    pathActuator->set_optimal_force(OPTIMAL_FORCE);
    pathActuator->addNewPathPoint("pointA", *cuffA, Vec3(0));
    //pathActuator->addNewPathPoint("pointB", *cuffB, Vec3(0));
    device->addComponent(pathActuator);

    // Create a PropMyoController.
    auto controller = new PropMyoController();
    controller->setName("controller");
    controller->set_gain(GAIN);

    //TODO: Connect the controller's "actuator" Connector to pathActuator.

    //TODO: Add the controller to the device.

    return device;
}

/**
 * Creates two cat models for flipping. The first is simply two segments
 * connected by a 2-degree-of-freedom (no twist) joint. It exhibits the
 * counter-rotation mechanism of flipping. The second adds actuate-able
 * legs to the first model, as well as unlocking the twist degree of
 * freedom between the model's two halves. This model exhibits the variable-
 * inertia mechanism of flipping.
 * */
int main(int argc, char *argv[])
{
    // First model: exhibiting the counter-rotation mechanism of flipping
    // ========================================================================

    // Properties
    // ------------------------------------------------------------------------
    double segmentalLength = 0.175;		                   // m
    double segmentalDiam = 0.15;			               // m
    double segmentalMass = 1;				               // kg
    double segmentalTransverseMomentOfInertia = 1;         // kg-m^2
    
    // Ratio of transverse to axial moment of inertia (Kane and Scher, 1969):
    double JIratio = 0.25;
    
    // For actuators:
	double maxTorque = 40.0;                               // N-m


    // Basics
    // ------------------------------------------------------------------------
    // Create the cat model.
    OpenSim::Model cat;

    // Name the cat after the founder of Stanford University.
    // This model will be able to hunch and wag (see the joints below).
    cat.setName("Leland_hunch_wag");

    // 'Turn off' gravity so it's easier to watch an animation of the flip.
    using SimTK::Vec3;
    cat.setGravity(Vec3(0, 0, 0));


    // Define anterior and posterior halves of the cat
    // ------------------------------------------------------------------------
    // Prepare inertia properties for the 2 primary segments of the cat.
    double segmentalAxialMomentOfInertia = JIratio * segmentalTransverseMomentOfInertia;
    double Ixx = segmentalAxialMomentOfInertia;
    double Iyy = segmentalTransverseMomentOfInertia;
    double Izz = segmentalTransverseMomentOfInertia;
    double Ixy = 0;
    double Ixz = 0;
    double Iyz = 0;
    using SimTK::Inertia;
    Inertia segmentalInertia = Inertia(Ixx, Iyy, Izz, Ixz, Ixz, Iyz);

    // Anterior half of cat.
    using OpenSim::Body;
    Body * anteriorBody = new Body();
    anteriorBody->setName("anteriorBody");
    anteriorBody->setMass(segmentalMass);
    // By choosing the following as the mass center, we choose the origin of
    // the anteriorBody frame to be at the body's positive-X extent. That is,
    // the anterior body sits to the -X direction from its origin.
    anteriorBody->setMassCenter(Vec3(-0.5 * segmentalLength, 0, 0));
    anteriorBody->setInertia(segmentalInertia);

    // Posterior half of cat (same mass properties as anterior half).
    Body * posteriorBody = new Body();
    posteriorBody->setName("posteriorBody");
    posteriorBody->setMass(segmentalMass);
    // Posterior body sits to the +X direction from its origin.
    posteriorBody->setMassCenter(Vec3(0.5 * segmentalLength, 0, 0));
    posteriorBody->setInertia(segmentalInertia);


    // Define joints between bodies (ground and two halves)
    // ------------------------------------------------------------------------
    Body & ground = cat.getGroundBody();

    // Anterior body to the ground via a CustomJoint
    // `````````````````````````````````````````````
    using OpenSim::CustomJoint;
	// Rotation is defined via YZX Euler angles, named yaw, pitch, and
	// roll respectively.

    // To pass to the CustomJoint (farther down), define a SpatialTransform.
    // The SpatialTransfrom has 6 transform axes. The first 3 are rotations,
    // defined about the axes of our choosing. The remaining 3 are translations,
    // which we choose to be along the X, Y, and Z directions of the ground's
    // frame.
    using OpenSim::Array;
    OpenSim::SpatialTransform groundAnteriorST;
    groundAnteriorST.updTransformAxis(0).setCoordinateNames(
            Array<std::string>("yaw", 1));
    groundAnteriorST.updTransformAxis(0).setAxis(Vec3(0, 1, 0));

    groundAnteriorST.updTransformAxis(1).setCoordinateNames(
            Array<std::string>("pitch", 1));
    groundAnteriorST.updTransformAxis(1).setAxis(Vec3(0, 0, 1));

    groundAnteriorST.updTransformAxis(2).setCoordinateNames(
//.........这里部分代码省略.........