コード例 #1
0
ファイル: WrapTorus.cpp プロジェクト: ANKELA/opensim-core
/**
 * Calculate the wrapping of one line segment over the torus.
 *
 * @param aPoint1 One end of the line segment
 * @param aPoint2 The other end of the line segment
 * @param aPathWrap An object holding the parameters for this line/torus pairing
 * @param aWrapResult The result of the wrapping (tangent points, etc.)
 * @param aFlag A flag for indicating errors, etc.
 * @return The status, as a WrapAction enum
 */
int WrapTorus::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2,
                                const PathWrap& aPathWrap, WrapResult& aWrapResult, bool& aFlag) const
{
    int i;
    SimTK::Vec3 closestPt;
    //bool constrained = (bool) (_wrapSign != 0);
    //bool far_side_wrap = false;
    aFlag = true;

    if (findClosestPoint(_outerRadius, &aPoint1[0], &aPoint2[0], &closestPt[0], &closestPt[1], &closestPt[2], _wrapSign, _wrapAxis) == 0)
        return noWrap;

    // Now put a cylinder at closestPt and call the cylinder wrap code.
    WrapCylinder cyl;//(rot, trans, quadrant, body, radius, length);
    SimTK::Vec3 cylXaxis, cylYaxis, cylZaxis; // cylinder axes in torus reference frame

    cyl.set_radius(_innerRadius);
    cyl.set_length(CYL_LENGTH);
    cyl.set_quadrant("+x");

    closestPt *= -1;

    cylXaxis = closestPt;
    Mtx::Normalize(3, cylXaxis, cylXaxis);
    cylYaxis[0] = 0.0;
    cylYaxis[1] = 0.0;
    cylYaxis[2] = -1.0;
    Mtx::CrossProduct(cylXaxis, cylYaxis, cylZaxis);
    // Note: you don't need to recalculate Y as Z x X because X and Z are always in the XY plane, so Y will remain 0 0 -1.

    // cylinderToTorus is the transform from the cylinder to the torus.
    // The origin of the cylinder frame in the torus frame is closestPt.
    // closestPtCyl is along the X axis of the cylinder since cylXaxis = closestPt.
    SimTK::Transform cylinderToTorus(SimTK::Rotation(SimTK::Mat33(cylXaxis[0], cylXaxis[1], cylXaxis[2],
        cylYaxis[0], cylYaxis[1], cylYaxis[2], cylZaxis[0], cylZaxis[1], cylZaxis[2])));
    SimTK::Vec3 closestPtCyl = cylinderToTorus.shiftFrameStationToBase(closestPt);
    cylinderToTorus.setP(closestPtCyl);
    Vec3 p1 = cylinderToTorus.shiftFrameStationToBase(aPoint1);
    Vec3 p2 = cylinderToTorus.shiftFrameStationToBase(aPoint2);
    int return_code = cyl.wrapLine(s, p1, p2, aPathWrap, aWrapResult, aFlag);
   if (aFlag == true && return_code > 0) {
        aWrapResult.r1 = cylinderToTorus.shiftBaseStationToFrame(aWrapResult.r1);
        aWrapResult.r2 = cylinderToTorus.shiftBaseStationToFrame(aWrapResult.r2);
        for (i = 0; i < aWrapResult.wrap_pts.getSize(); i++)
            aWrapResult.wrap_pts.updElt(i) = cylinderToTorus.shiftBaseStationToFrame(aWrapResult.wrap_pts.get(i));
    }

    return wrapped;
}
コード例 #2
0
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);
    model->addAnalysis(reporter);

    model->setUseVisualizer(false);
    SimTK::State& state = model->initSystem();

    CoordinateSet& coords = model->updCoordinateSet();
    coords[0].setValue(state, start_h);
    model->getMultibodySystem().realize(state, Stage::Position );

    //==========================================================================
    // Compute the force and torque at the specified times.

    RungeKuttaMersonIntegrator integrator(model->getMultibodySystem() );
    integrator.setAccuracy(integ_accuracy);
    Manager manager(*model,  integrator);
    manager.setWriteToStorage(true);

    manager.setInitialTime(0.0);

    double final_t = 4.99999;

    manager.setFinalTime(final_t);
    manager.integrate(state);

    // tension is dynamics dependent because controls must be computed
    model->getMultibodySystem().realize(state, Stage::Dynamics);

    spring = dynamic_cast<ClutchedPathSpring*>(
                &model->updForceSet().get("clutch_spring"));
    // Now check that the force reported by spring
    double model_force = spring->getTension(state);
    double stretch0 = spring->getStretch(state);

    // the tension should be half the weight of the block
    double analytical_force = -0.5*(gravity_vec(1))*mass;

    cout << "Tension is: " << model_force << " and should be: " << analytical_force << endl;

    // error if the block does not reach equilibrium since spring is clamped
    ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);

    // unclamp and continue integrating
    manager.setInitialTime(final_t);
    final_t = 5.99999;
    manager.setFinalTime(final_t);
    manager.integrate(state);

    // tension is dynamics dependent because controls must be computed
    model->getMultibodySystem().realize(state, Stage::Dynamics);

    // tension should go to zero quickly
    model_force = spring->getTension(state);

    cout << "Tension is: " << model_force << " and should be: 0.0" << endl;
    // is unclamped and block should be in free-fall
    ASSERT_EQUAL(model_force, 0.0, 10*integ_accuracy);

    // spring is reclamped at 7s so keep integrating
    manager.setInitialTime(final_t);
    final_t = 10.0;
    manager.setFinalTime(final_t);
    manager.integrate(state);

    // tension is dynamics dependent because controls must be computed
    model->getMultibodySystem().realize(state, Stage::Dynamics);

    // tension should build to support the block again
    model_force = spring->getTension(state);
    double stretch1 = spring->getStretch(state);

    cout << "Tension is: " << model_force << " and should be: "<< analytical_force << endl;

    // is unclamped and block should be in free-fall
    ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);

    cout << "Steady stretch at control = 1.0 is " << stretch0 << " m." << endl;
    cout << "Steady stretch at control = 0.5 is " << stretch1 << " m." << endl;

    ASSERT_EQUAL(2*stretch0, stretch1, 10*integ_accuracy);

    manager.getStateStorage().print("clutched_path_spring_states.sto");
    model->getControllerSet().printControlStorage("clutched_path_spring_controls.sto");

    // Save the forces
    reporter->getForceStorage().print("clutched_path_spring_forces.mot"); 

    model->disownAllComponents();

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