void testExternalLoad()
{
using namespace SimTK;
Model model("Pendulum.osim");
State &s = model.initSystem();
// Simulate gravity
double init_t =-1e-8;
double final_t = 2.0;
int nsteps = 10;
double dt = final_t/nsteps;
//initial state
double q_init = Pi/4;
model.updCoordinateSet()[0].setValue(s, q_init);
Vector_<double> q_grav(nsteps+1);
// Integrator and integration manager
double integ_accuracy = 1e-6;
RungeKuttaMersonIntegrator integrator(model.getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
Manager manager(model, integrator);
manager.setInitialTime(init_t);
for(int i = 0; i < nsteps+1; i++){
manager.setFinalTime(dt*i);
manager.integrate(s);
q_grav[i] = model.updCoordinateSet()[0].getValue(s);
manager.setInitialTime(dt*i);
}
//q_grav.dump("Coords due to gravity.");
/***************************** CASE 1 ************************************/
// Simulate the same system without gravity but with an equivalent external load
OpenSim::Body &pendulum = model.getBodySet().get(model.getNumBodies()-1);
string pendBodyName = pendulum.getName();
Vec3 comInB = pendulum.getMassCenter();
Storage forceStore;
addLoadToStorage(forceStore, pendulum.getMass()*model.getGravity(), comInB, Vec3(0, 0, 0));
forceStore.setName("test_external_loads.sto");
forceStore.print(forceStore.getName());
// Apply external force with force in ground, point in body, zero torque
ExternalForce xf(forceStore, "force", "point", "torque", pendBodyName, "ground", pendBodyName);
xf.setName("grav");
ExternalLoads* extLoads = new ExternalLoads(model);
extLoads->adoptAndAppend(&xf);
extLoads->print("ExternalLoads_test.xml");
for(int i=0; i<extLoads->getSize(); i++)
model.addForce(&(*extLoads)[i]);
// Create the force reporter
ForceReporter* reporter = new ForceReporter();
model.addAnalysis(reporter);
Kinematics* kin = new Kinematics();
kin->setInDegrees(false);
model.addAnalysis(kin);
PointKinematics* pKin = new PointKinematics();
pKin->setBody(&pendulum);
pKin->setPoint(comInB);
pKin->setPointName(pendulum.getName()+"_com_p");
model.addAnalysis(pKin);
SimTK::State& s2 = model.initSystem();
// Turn-off gravity in the model
model.updGravityForce().disable(s2);
// initial position
model.updCoordinateSet()[0].setValue(s2, q_init);
RungeKuttaMersonIntegrator integrator2(model.getMultibodySystem() );
integrator2.setAccuracy(integ_accuracy);
Manager manager2(model, integrator2);
manager2.setInitialTime(init_t);
// Simulate with the external force applied instead of gravity
Vector_<double> q_xf(nsteps+1);
Vector_<Vec3> pcom_xf(nsteps+1);
for(int i = 0; i < nsteps+1; i++){
manager2.setFinalTime(dt*i);
manager2.integrate(s2);
q_xf[i] = model.updCoordinateSet()[0].getValue(s2);
manager2.setInitialTime(dt*i);
}
//q_xf.dump("Coords due to external force point expressed in pendulum.");
Vector err = q_xf-q_grav;
double norm_err = err.norm();
// kinematics should match to within integ accuracy
ASSERT_EQUAL(0.0, norm_err, integ_accuracy);
/***************************** CASE 2 ************************************/
// Simulate the same system without gravity but with an equivalent external
// force but this time with the point expressed in ground and using
// previous kinematics to transform point to pendulum.
// Construct a new Storage for ExternalForce data source with point
// described in ground
Storage forceStore2 = reporter->getForceStorage();
forceStore2.print("ForcesTest.sto");
Storage *pStore = pKin->getPositionStorage();
pStore->print("PointInGroundTest.sto");
pStore->addToRdStorage(forceStore2, init_t, final_t);
forceStore2.setName("ExternalForcePointInGround.sto");
forceStore2.print(forceStore2.getName());
Storage *qStore = kin->getPositionStorage();
qStore->print("LoadKinematics.sto");
string id_base = pendBodyName+"_"+xf.getName();
string point_id = pKin->getPointName();
ExternalForce xf2(forceStore2, id_base+"_F", point_id, id_base+"_T", pendBodyName, "ground", "ground");
xf2.setName("xf_pInG");
// Empty out existing external forces
extLoads->setMemoryOwner(false);
extLoads->setSize(0);
extLoads->adoptAndAppend(&xf2);
//Ask external loads to transform point expressed in ground to the applied body
extLoads->setDataFileName(forceStore2.getName());
extLoads->invokeConnectToModel(model);
extLoads->transformPointsExpressedInGroundToAppliedBodies(*qStore);
// remove previous external force from the model too
model.disownAllComponents();
model.updForceSet().setSize(0);
// after external loads has transformed the point of the force, then add it the model
for(int i=0; i<extLoads->getSize(); i++)
model.addForce(&(*extLoads)[i]);
// recreate dynamical system to reflect new force
SimTK::State &s3 = model.initSystem();
// Turn-off gravity in the model
model.updGravityForce().disable(s3);
// initial position
model.updCoordinateSet()[0].setValue(s3, q_init);
RungeKuttaMersonIntegrator integrator3(model.getMultibodySystem() );
integrator3.setAccuracy(integ_accuracy);
Manager manager3(model, integrator3);
manager3.setInitialTime(init_t);
// Simulate with the external force applied instead of gravity
Vector_<double> q_xf2(nsteps+1);
for(int i = 0; i < nsteps+1; i++){
manager3.setFinalTime(dt*i);
manager3.integrate(s3);
q_xf2[i] = model.updCoordinateSet()[0].getValue(s3);
manager3.setInitialTime(dt*i);
}
//q_xf2.dump("Coords due to external force point expressed in ground.");
err = q_xf2-q_grav;
//err.dump("Coordinate error after transforming point.");
norm_err = err.norm();
// kinematics should match to within integ accuracy
ASSERT_EQUAL(0.0, norm_err, integ_accuracy);
}
