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(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->setRadius(0.1);
    pulley->setLength(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
    CoordinateSet &slider_coords = slider->upd_CoordinateSet();
    slider_coords[0].setName("block_h");
    slider_coords[0].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;
}
Пример #2
0
void testPathSpring()
{
	using namespace SimTK;

	double mass = 1;
	double stiffness = 10;
	double restlength = 0.5;
	double dissipation = 0.1;
	double h0 = 0;
	double start_h = 0.5;
	double ball_radius = 0.25;

	double omega = sqrt(stiffness/mass);

	double dh = mass*gravity_vec(1)/stiffness;

	// Setup OpenSim model
	Model *osimModel = new Model;
	osimModel->setName("PathSpring");
	//OpenSim bodies
    OpenSim::Body& ground = osimModel->getGroundBody();
	OpenSim::Body pulleyBody("PulleyBody", mass ,Vec3(0),  mass*SimTK::Inertia::brick(0.1, 0.1, 0.1));
	OpenSim::Body block("block", mass ,Vec3(0),  mass*SimTK::Inertia::brick(0.2, 0.1, 0.1));
	block.addDisplayGeometry("box.vtp");
	block.scale(Vec3(0.2, 0.1, 0.1), false);
	
	WrapCylinder* pulley = new WrapCylinder();
	pulley->setRadius(0.1);
	pulley->setLength(0.05);

	// Add the wrap object to the body, which takes ownership of it
	pulleyBody.addWrapObject(pulley);

	// Add joints
	WeldJoint weld("pulley", ground, Vec3(0, 1.0, 0), Vec3(0), pulleyBody, Vec3(0), Vec3(0));
	SliderJoint slider("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
	CoordinateSet &slider_coords = slider.upd_CoordinateSet();
	slider_coords[0].setName("block_h");
	slider_coords[0].setRange(positionRange);
	slider_coords[0].setMotionType(Coordinate::Translational);

	osimModel->addBody(&block);
	osimModel->addJoint(&weld);

	osimModel->addBody(&pulleyBody);
	osimModel->addJoint(&slider);

	osimModel->setGravity(gravity_vec);

	PathSpring spring("spring", restlength, stiffness, dissipation);
	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
	osimModel->addForce(&spring);

	// Create the force reporter
	ForceReporter* reporter = new ForceReporter(osimModel);
	osimModel->addAnalysis(reporter);

	//osimModel->setUseVisualizer(true);
	SimTK::State& osim_state = osimModel->initSystem();

	slider_coords[0].setValue(osim_state, start_h);
    osimModel->getMultibodySystem().realize(osim_state, Stage::Position );

	//==========================================================================
	// Compute the force and torque at the specified times.
    RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
	integrator.setAccuracy(1e-6);
    Manager manager(*osimModel,  integrator);
    manager.setInitialTime(0.0);

	double final_t = 10.0;

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

	// tension should only be velocity dependent
	osimModel->getMultibodySystem().realize(osim_state, Stage::Velocity);

	// Now check that the force reported by spring
	double model_force = spring.getTension(osim_state);

	// get acceleration of the block
	osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
	double hddot = osimModel->getCoordinateSet().get("block_h").getAccelerationValue(osim_state);

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

	// Save the forces
	reporter->getForceStorage().print("path_spring_forces.mot");  
	
	// something is wrong if the block does not reach equilibrium
	ASSERT_EQUAL(analytical_force, model_force, 1e-3);

	// Before exiting lets see if copying the spring works
	PathSpring *copyOfSpring = spring.clone();
	ASSERT(*copyOfSpring == spring);
	
	osimModel->disownAllComponents();
}