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;
}
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();
}