void testMcKibbenActuator() { double pressure = 5 * 10e5; // 5 bars double num_turns = 1.5; // 1.5 turns double B = 277.1 * 10e-4; // 277.1 mm using namespace SimTK; std::clock_t startTime = std::clock(); double mass = 1; double ball_radius = 10e-6; Model *model = new Model; model->setGravity(Vec3(0)); Ground& ground = model->updGround(); McKibbenActuator *actuator = new McKibbenActuator("mckibben", num_turns, B); OpenSim::Body* ball = new OpenSim::Body("ball", mass ,Vec3(0), mass*SimTK::Inertia::sphere(0.1)); ball->scale(Vec3(ball_radius), false); actuator->addNewPathPoint("mck_ground", ground, Vec3(0)); actuator->addNewPathPoint("mck_ball", *ball, Vec3(ball_radius)); Vec3 locationInParent(0, ball_radius, 0), orientationInParent(0), locationInBody(0), orientationInBody(0); SliderJoint *ballToGround = new SliderJoint("ballToGround", ground, locationInParent, orientationInParent, *ball, locationInBody, orientationInBody); ballToGround->updCoordinate().setName("ball_d"); ballToGround->updCoordinate().setPrescribedFunction(LinearFunction(20 * 10e-4, 0.5 * 264.1 * 10e-4)); ballToGround->updCoordinate().set_prescribed(true); model->addBody(ball); model->addJoint(ballToGround); model->addForce(actuator); PrescribedController* controller = new PrescribedController(); controller->addActuator(*actuator); controller->prescribeControlForActuator("mckibben", new Constant(pressure)); model->addController(controller); ForceReporter* reporter = new ForceReporter(model); model->addAnalysis(reporter); SimTK::State& si = model->initSystem(); model->getMultibodySystem().realize(si, Stage::Position); double final_t = 10.0; double nsteps = 10; double dt = final_t / nsteps; RungeKuttaMersonIntegrator integrator(model->getMultibodySystem()); integrator.setAccuracy(1e-7); Manager manager(*model, integrator); manager.setInitialTime(0.0); for (int i = 1; i <= nsteps; i++){ manager.setFinalTime(dt*i); manager.integrate(si); model->getMultibodySystem().realize(si, Stage::Velocity); Vec3 pos; model->updSimbodyEngine().getPosition(si, *ball, Vec3(0), pos); double applied = actuator->computeActuation(si);; double theoretical = (pressure / (4* pow(num_turns,2) * SimTK::Pi)) * (3*pow(pos(0), 2) - pow(B, 2)); ASSERT_EQUAL(applied, theoretical, 10.0); manager.setInitialTime(dt*i); } std::cout << " ******** Test McKibbenActuator time = ********" << 1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl; }
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; }