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