void performMCFD_flexion(Model model, int iterations) { std::default_random_engine gen; random_device rd; gen.seed(rd()); //std::uniform_real_distribution<double> random_stiff(5*1.e8, 5*1.e9); //std::uniform_real_distribution<double> random_diss(0.0, 20.0); //std::uniform_real_distribution<double> random_aPCL_force(4800, 6200); std::uniform_real_distribution<double> random_PCL_length_range(0, 0.01); // length diff // flexion controller addFlexionController(model); // init system std::time_t result = std::time(nullptr); std::cout << "\nBefore initSystem() " << std::asctime(std::localtime(&result)) << endl; SimTK::State& si = model.initSystem(); result = std::time(nullptr); std::cout << "\nAfter initSystem() " << std::asctime(std::localtime(&result)) << endl; // disable muscles string muscle_name; for (int i=0; i<model.getActuators().getSize(); i++) { muscle_name = model.getActuators().get(i).getName(); model.getActuators().get(i).setDisabled(si, true); if (muscle_name == "bifemlh_r" || muscle_name == "bifemsh_r" || muscle_name == "grac_r" \ || muscle_name == "lat_gas_r" || muscle_name == "med_gas_r" || muscle_name == "sar_r" \ || muscle_name == "semimem_r" || muscle_name == "semiten_r" \ || muscle_name == "rect_fem_r" || muscle_name == "vas_med_r" || muscle_name == "vas_int_r" || muscle_name == "vas_lat_r" ) model.getActuators().get(i).setDisabled(si, false); } model.equilibrateMuscles( si); Array<double> samplingArray1; Array<double> samplingArray2; for (int i = 50; i < iterations; i++) { // Add reporters ForceReporter* forceReporter = new ForceReporter(&model); model.addAnalysis(forceReporter); CustomAnalysis* customReporter = new CustomAnalysis(&model, "r"); model.addAnalysis(customReporter); //string outputFile = changeToString(i) + "_fd_.sto"; //double this_random_stiff = random_stiff(gen); //double this_random_diss = random_diss(gen); //double this_random_aPCL = random_aPCL_force(gen); //double this_random_pPCL = this_random_aPCL * 0.8363; double this_random_PCL_diff = random_PCL_length_range(gen); double this_random_aPCL_length = (0.031 - 0.005) + this_random_PCL_diff; double this_random_pPCL_length = (0.030 - 0.005) + this_random_PCL_diff; //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setStiffness(this_random_stiff); //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setStiffness(this_random_stiff); //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setDissipation(this_random_diss); //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setDissipation(this_random_diss); //static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aPCL_R")).set_stiffness(this_random_aPCL); //static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pPCL_R")).set_stiffness(this_random_pPCL); static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aPCL_R")).setRestingLength(this_random_aPCL_length); static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pPCL_R")).setRestingLength(this_random_pPCL_length); samplingArray1.append(this_random_aPCL_length); samplingArray2.append(this_random_pPCL_length); setHipAngle(model, si, 90); setKneeAngle(model, si, 0, false, false); si = model.initSystem(); // Create the integrator and manager for the simulation. SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); //integrator.setAccuracy(1.0e-3); //integrator.setFixedStepSize(0.0001); Manager manager(model, integrator); // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 0.25; // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); std::cout<<"\n\nIntegrating from "<<initialTime<<" to " <<finalTime<<". "<< i <<std::endl; result = std::time(nullptr); std::cout << "\nBefore integrate(si) " << std::asctime(std::localtime(&result)) << endl; manager.integrate(si); result = std::time(nullptr); std::cout << "\nAfter integrate(si) " << std::asctime(std::localtime(&result)) << endl; // Save the simulation results Storage statesDegrees(manager.getStateStorage()); //statesDegrees.print("../outputs/MonteCarlo/states_rads/" + changeToString(i) + "_states_flexion.sto"); model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees); statesDegrees.setWriteSIMMHeader(true); statesDegrees.print("../outputs/MC_flexion/MC_PclLength_Flexion_v6/states/" + changeToString(i) + "_states_degrees_flexion.mot"); // force reporter results forceReporter->getForceStorage().print("../outputs/MC_flexion/MC_PclLength_Flexion_v6/ForceReporter/" + changeToString(i) + "_force_reporter_flexion.mot"); customReporter->print( "../outputs/MC_flexion/MC_PclLength_Flexion_v6/CustomReporter/" + changeToString(i) + "_custom_reporter_flexion.mot"); model.removeAnalysis(forceReporter); model.removeAnalysis(customReporter); writeArrayToFile("../outputs/MC_flexion/MC_PclLength_Flexion_v6/aPCL_length.txt", samplingArray1); writeArrayToFile("../outputs/MC_flexion/MC_PclLength_Flexion_v6/pPCL_length.txt", samplingArray2); } }
/** * Run a simulation of block sliding with contact on by two muscles sliding with contact */ int main() { try { // Create a new OpenSim model Model osimModel; osimModel.setName("osimModel"); osimModel.setAuthors("Matt DeMers"); double Pi = SimTK::Pi; // Get the ground body OpenSim::Body& ground = osimModel.getGroundBody(); ground.addDisplayGeometry("checkered_floor.vtp"); // create linkage body double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06; Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter); Vec3 linkageMassCenter(0,linkageLength/2,0); Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0); OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia); // Graphical representation linkage1->addDisplayGeometry("cylinder.vtp"); //This cylinder.vtp geometry is 1 meter tall, 1 meter diameter. Scale and shift it to look pretty GeometrySet& geometry = linkage1->updDisplayer()->updGeometrySet(); DisplayGeometry& thinCylinder = geometry[0]; thinCylinder.setScaleFactors(linkageDimensions); thinCylinder.setTransform(Transform(Vec3(0.0,linkageLength/2.0,0.0))); linkage1->addDisplayGeometry("sphere.vtp"); //This sphere.vtp is 1 meter in diameter. Scale it. geometry[1].setScaleFactors(Vec3(0.1)); // Creat a second linkage body OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1); linkage2->setName("linkage2"); // Creat a block to be the pelvis double blockMass = 20.0, blockSideLength = 0.2; Vec3 blockMassCenter(0); Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength); OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia); block->addDisplayGeometry("block.vtp"); //This block.vtp is 0.1x0.1x0.1 meters. scale its appearance block->updDisplayer()->updGeometrySet()[0].setScaleFactors(Vec3(2.0)); // Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0); PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1, locationInChild, orientationInChild); PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2, locationInChild, orientationInChild); PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block, locationInChild, orientationInChild); double range[2] = {-SimTK::Pi*2, SimTK::Pi*2}; CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet(); ankleCoordinateSet[0].setName("q1"); ankleCoordinateSet[0].setRange(range); CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet(); kneeCoordinateSet[0].setName("q2"); kneeCoordinateSet[0].setRange(range); CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet(); hipCoordinateSet[0].setName("q3"); hipCoordinateSet[0].setRange(range); // Add the bodies to the model osimModel.addBody(linkage1); osimModel.addBody(linkage2); osimModel.addBody(block); // Define contraints on the model // Add a point on line constraint to limit the block to vertical motion Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0); PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock); osimModel.addConstraint(lineConstraint); // Add PistonActuator between the first linkage and the block Vec3 pointOnBodies(0); PistonActuator *piston = new PistonActuator(); piston->setName("piston"); piston->setBodyA(linkage1); piston->setBodyB(block); piston->setPointA(pointOnBodies); piston->setPointB(pointOnBodies); piston->setOptimalForce(200.0); piston->setPointsAreGlobal(false); osimModel.addForce(piston); //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Added ControllableSpring between the first linkage and the second block //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ControllableSpring *spring = new ControllableSpring; spring->setName("spring"); spring->setBodyA(block); spring->setBodyB(linkage1); spring->setPointA(pointOnBodies); spring->setPointB(pointOnBodies); spring->setOptimalForce(2000.0); spring->setPointsAreGlobal(false); spring->setRestLength(0.8); osimModel.addForce(spring); // define the simulation times double t0(0.0), tf(15); // create a controller to control the piston and spring actuators // the prescribed controller sets the controls as functions of time PrescribedController *legController = new PrescribedController(); // give the legController control over all (two) model actuators legController->setActuators(osimModel.updActuators()); // specify some control nodes for spring stiffness control double t[] = {0.0, 4.0, 7.0, 10.0, 15.0}; double x[] = {1.0, 1.0, 0.25, 0.25, 5.0}; // specify the control function for each actuator legController->prescribeControlForActuator("piston", new Constant(0.1)); legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x)); // add the controller to the model osimModel.addController(legController); // define the acceration due to gravity osimModel.setGravity(Vec3(0, -9.80665, 0)); // enable the model visualizer see the model in action, which can be // useful for debugging osimModel.setUseVisualizer(false); // Initialize system SimTK::State& si = osimModel.initSystem(); // Pin joint initial states double q1_i = -Pi/4; double q2_i = - 2*q1_i; CoordinateSet &coordinates = osimModel.updCoordinateSet(); coordinates[0].setValue(si, q1_i, true); coordinates[1].setValue(si,q2_i, true); // Setup integrator and manager SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem()); integrator.setAccuracy(1.0e-3); ForceReporter *forces = new ForceReporter(&osimModel); osimModel.updAnalysisSet().adoptAndAppend(forces); Manager manager(osimModel, integrator); //Examine the model osimModel.printDetailedInfo(si, std::cout); // Save the model osimModel.print("toyLeg.osim"); // Print out the initial position and velocity states si.getQ().dump("Initial q's"); si.getU().dump("Initial u's"); std::cout << "Initial time: " << si.getTime() << std::endl; // Integrate manager.setInitialTime(t0); manager.setFinalTime(tf); std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl; manager.integrate(si); // Save results osimModel.printControlStorage("SpringActuatedLeg_controls.sto"); Storage statesDegrees(manager.getStateStorage()); osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees); //statesDegrees.print("PistonActuatedLeg_states_degrees.mot"); statesDegrees.print("SpringActuatedLeg_states_degrees.mot"); forces->getForceStorage().print("actuator_forces.mot"); } catch (const std::exception& ex) { std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl; return 1; } std::cout << "Exiting" << std::endl; return 0; }
void performMCFD_atl(Model model, int iterations) { // gaussian distribution setup std::default_random_engine gen; random_device rd; gen.seed(rd()); //std::uniform_real_distribution<double> random_stiff(5*1.e8, 5*1.e9); //std::uniform_real_distribution<double> random_diss(0.0, 20.0); //std::uniform_real_distribution<double> random_aACL_force(1300,1700); std::uniform_real_distribution<double> random_ACL_length_range(0, 0.005); // length diff // init system std::time_t result = std::time(nullptr); std::cout << "\nBefore initSystem() " << std::asctime(std::localtime(&result)) << endl; SimTK::State& si = model.initSystem(); result = std::time(nullptr); std::cout << "\nAfter initSystem() " << std::asctime(std::localtime(&result)) << endl; // set gravity model.updGravityForce().setGravityVector(si, Vec3(0,0,0)); model.equilibrateMuscles( si); Array<double> samplingArray1; Array<double> samplingArray2; for (int i = 20; i < iterations; i++) { double kneeAngle [6] = {0, -15, -30, -60, -90}; //double this_random_stiff = random_stiff(gen); //double this_random_diss = random_diss(gen); //double this_random_aACL = random_aACL_force(gen); //double this_random_pACL = this_random_aACL * 1.266; double this_random_ACL_diff = random_ACL_length_range(gen); double this_random_aACL_length = (0.031 - 0.0025) + this_random_ACL_diff; double this_random_pACL_length = (0.0253 - 0.0025) + this_random_ACL_diff; //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setStiffness(this_random_stiff); //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setStiffness(this_random_stiff); //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setDissipation(this_random_diss); //static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setDissipation(this_random_diss); //static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aACL_R")).set_stiffness(this_random_aACL); //static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pACL_R")).set_stiffness(this_random_pACL); static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aACL_R")).setRestingLength(this_random_aACL_length); static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pACL_R")).setRestingLength(this_random_pACL_length); samplingArray1.append(this_random_aACL_length); samplingArray2.append(this_random_pACL_length); for (int j=3; j<4; j++) { // Add reporters ForceReporter* forceReporter = new ForceReporter(&model); model.addAnalysis(forceReporter); CustomAnalysis* customReporter = new CustomAnalysis(&model, "r"); model.addAnalysis(customReporter); //string outputFile = changeToString(i) + "_fd_.sto"; // edit prescribed force for anterior tibial load addTibialLoads(model, kneeAngle[j]); si = model.initSystem(); setKneeAngle(model, si, kneeAngle[j], true, true); // Create the integrator and manager for the simulation. SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); //integrator.setAccuracy(1.0e-3); //integrator.setFixedStepSize(0.0001); Manager manager(model, integrator); // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 0.8; // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); std::cout<<"\n\nIntegrating from "<<initialTime<<" to " <<finalTime<<". "<< i <<std::endl; result = std::time(nullptr); std::cout << "\nBefore integrate(si) " << std::asctime(std::localtime(&result)) << endl; manager.integrate(si); result = std::time(nullptr); std::cout << "\nAfter integrate(si) " << std::asctime(std::localtime(&result)) << endl; // Save the simulation results Storage statesDegrees(manager.getStateStorage()); //statesDegrees.print("../outputs/MonteCarlo/states_rads/" + changeToString(i) + "_states_flexion.sto"); model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees); statesDegrees.setWriteSIMMHeader(true); statesDegrees.print("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/states/" + changeToString(i) + "_states_degrees_atl_" + changeToString(abs(kneeAngle[j])) + ".mot" ); // force reporter results forceReporter->getForceStorage().print("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/ForceReporter/" + changeToString(i) + "_force_reporter_atl_" + changeToString(abs(kneeAngle[j])) + ".mot"); customReporter->print( "../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/CustomReporter/" + changeToString(i) + "_custom_reporter_atl_" + changeToString(abs(kneeAngle[j])) + ".mot"); model.removeAnalysis(forceReporter); model.removeAnalysis(customReporter); writeArrayToFile("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/aACL_length.txt", samplingArray1); writeArrayToFile("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/pACL_length.txt", samplingArray2); } } }
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(comInB); 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); model.addAnalysis(reporter); Kinematics* kin = new Kinematics(&model); kin->setInDegrees(false); model.addAnalysis(kin); PointKinematics* pKin = new PointKinematics(&model); 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); }
/*============================================================================== Main test driver to be used on any muscle model (derived from Muscle) so new cases should be easy to add currently, the test only verifies that the work done by the muscle corresponds to the change in system energy. TODO: Test will fail wih prescribe motion until the work done by this constraint is accounted for. ================================================================================ */ void simulateMuscle( const Muscle &aMuscModel, double startX, double act0, const Function *motion, // prescribe motion of free end of muscle const Function *control, // prescribed excitation signal to the muscle double integrationAccuracy, int testType, double testTolerance, bool printResults) { string prescribed = (motion == NULL) ? "." : " with Prescribed Motion."; cout << "\n******************************************************" << endl; cout << "Test " << aMuscModel.getConcreteClassName() << " Model" << prescribed << endl; cout << "******************************************************" << endl; using SimTK::Vec3; //========================================================================== // 0. SIMULATION SETUP: Create the block and ground //========================================================================== // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 4.0; //Physical properties of the model double ballMass = 10; double ballRadius = 0.05; double anchorWidth = 0.1; // Create an OpenSim model Model model; double optimalFiberLength = aMuscModel.getOptimalFiberLength(); double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength(); double tendonSlackLength = aMuscModel.getTendonSlackLength(); // Use a copy of the muscle model passed in to add path points later PathActuator *aMuscle = aMuscModel.clone(); // Get a reference to the model's ground body Ground& ground = model.updGround(); ground.addDisplayGeometry("box.vtp"); ground.updDisplayer() ->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2 * anchorWidth)); OpenSim::Body * ball = new OpenSim::Body("ball", ballMass, Vec3(0), ballMass*SimTK::Inertia::sphere(ballRadius)); ball->addDisplayGeometry("sphere.vtp"); ball->updDisplayer()->setScaleFactors(Vec3(2 * ballRadius)); // ball connected to ground via a slider along X double xSinG = optimalFiberLength*cos(pennationAngle) + tendonSlackLength; SliderJoint* slider = new SliderJoint("slider", ground, Vec3(anchorWidth / 2 + xSinG, 0, 0), Vec3(0), *ball, Vec3(0), Vec3(0)); CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet(); jointCoordinateSet[0].setName("tx"); jointCoordinateSet[0].setDefaultValue(1.0); jointCoordinateSet[0].setRangeMin(0); jointCoordinateSet[0].setRangeMax(1.0); if (motion != NULL){ jointCoordinateSet[0].setPrescribedFunction(*motion); jointCoordinateSet[0].setDefaultIsPrescribed(true); } // add ball to model model.addBody(ball); model.addJoint(slider); //========================================================================== // 1. SIMULATION SETUP: Add the muscle //========================================================================== //Attach the muscle const string &actuatorType = aMuscle->getConcreteClassName(); aMuscle->setName("muscle"); aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth / 2, 0, 0)); aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius, 0, 0)); ActivationFiberLengthMuscle_Deprecated *aflMuscle = dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle); if (aflMuscle){ // Define the default states for the muscle that has //activation and fiber-length states aflMuscle->setDefaultActivation(act0); aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength()); } else{ ActivationFiberLengthMuscle *aflMuscle2 = dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle); if (aflMuscle2){ // Define the default states for the muscle //that has activation and fiber-length states aflMuscle2->setDefaultActivation(act0); aflMuscle2->setDefaultFiberLength(aflMuscle2 ->getOptimalFiberLength()); } } model.addForce(aMuscle); // Create a prescribed controller that simply //applies controls as function of time PrescribedController * muscleController = new PrescribedController(); if (control != NULL){ muscleController->setActuators(model.updActuators()); // Set the indiviudal muscle control functions //for the prescribed muscle controller muscleController->prescribeControlForActuator("muscle", control->clone()); // Add the control set controller to the model model.addController(muscleController); } // Set names for muscles / joints. Array<string> muscNames; muscNames.append(aMuscle->getName()); Array<string> jointNames; jointNames.append("slider"); //========================================================================== // 2. SIMULATION SETUP: Instrument the test with probes //========================================================================== Array<string> muscNamesTwice = muscNames; muscNamesTwice.append(muscNames.get(0)); cout << "------------\nPROBES\n------------" << endl; int probeCounter = 1; // Add ActuatorPowerProbe to measure work done by the muscle ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1); //muscWorkProbe->setName("ActuatorWork"); muscWorkProbe->setOperation("integrate"); SimTK::Vector ic1(1); ic1 = 9.0; // some arbitary initial condition. muscWorkProbe->setInitialConditions(ic1); model.addProbe(muscWorkProbe); model.setup(); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl; // Add ActuatorPowerProbe to measure power generated by the muscle ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor muscPowerProbe->setName("ActuatorPower"); muscPowerProbe->setOperation("value"); model.addProbe(muscPowerProbe); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl; // Add ActuatorPowerProbe to report the muscle power MINIMUM ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMinimum->setName("ActuatorPowerMinimum"); powerProbeMinimum->setOperation("minimum"); model.addProbe(powerProbeMinimum); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl; // Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMinAbs->setName("ActuatorPowerMinAbs"); powerProbeMinAbs->setOperation("minabs"); model.addProbe(powerProbeMinAbs); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl; // Add ActuatorPowerProbe to report the muscle power MAXIMUM ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMaximum->setName("ActuatorPowerMaximum"); powerProbeMaximum->setOperation("maximum"); model.addProbe(powerProbeMaximum); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl; // Add ActuatorPowerProbe to report the muscle power MAXABS ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMaxAbs->setName("ActuatorPowerMaxAbs"); powerProbeMaxAbs->setOperation("maxabs"); model.addProbe(powerProbeMaxAbs); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl; // Add ActuatorPowerProbe to measure the square of the power generated by the muscle ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor muscPowerSquaredProbe->setName("ActuatorPowerSquared"); muscPowerSquaredProbe->setExponent(2.0); model.addProbe(muscPowerSquaredProbe); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl; // Add JointInternalPowerProbe to measure work done by the joint JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1); jointWorkProbe->setName("JointWork"); jointWorkProbe->setOperation("integrate"); jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0)); model.addProbe(jointWorkProbe); cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl; // Add JointPowerProbe to measure power generated by the joint JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor jointPowerProbe->setName("JointPower"); jointPowerProbe->setOperation("value"); model.addProbe(jointPowerProbe); cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl; // Add ActuatorForceProbe to measure the impulse of the muscle force ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1); impulseProbe->setName("ActuatorImpulse"); impulseProbe->setOperation("integrate"); impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0)); model.addProbe(impulseProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl; // Add ActuatorForceProbe to report the muscle force ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbe->setName("ActuatorForce"); forceProbe->setOperation("value"); model.addProbe(forceProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl; // Add ActuatorForceProbe to report the square of the muscle force ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor forceSquaredProbe->setName("ActuatorForceSquared"); forceSquaredProbe->setExponent(2.0); model.addProbe(forceSquaredProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl; // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice"); forceSquaredProbeTwice->setSumForcesTogether(true); forceSquaredProbeTwice->setActuatorNames(muscNamesTwice); model.addProbe(forceSquaredProbeTwice); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl; // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5 ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved"); double gain1 = 0.5; forceSquaredProbeTwiceScaled->setGain(gain1); model.addProbe(forceSquaredProbeTwiceScaled); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl; // Add ActuatorForceProbe to report -3.5X the muscle force double gain2 = -3.50; ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbeScale->setName("ScaleActuatorForce"); forceProbeScale->setOperation("value"); forceProbeScale->setGain(gain2); model.addProbe(forceProbeScale); cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl; // Add ActuatorForceProbe to report the differentiated muscle force ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbeDiff->setName("DifferentiateActuatorForce"); forceProbeDiff->setOperation("differentiate"); model.addProbe(forceProbeDiff); cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl; // Add SystemEnergyProbe to measure the system KE+PE SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true); sysEnergyProbe->setName("SystemEnergy"); sysEnergyProbe->setOperation("value"); sysEnergyProbe->setComputeKineticEnergy(true); sysEnergyProbe->setComputePotentialEnergy(true); model.addProbe(sysEnergyProbe); cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl; // Add SystemEnergyProbe to measure system power (d/dt system KE+PE) SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor sysPowerProbe->setName("SystemPower"); sysPowerProbe->setDisabled(false); sysPowerProbe->setOperation("differentiate"); model.addProbe(sysPowerProbe); cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl; // Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR"); forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice); //cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl; forceSquaredProbeTwiceReportedIndividually1->setOperation("value"); model.addProbe(forceSquaredProbeTwiceReportedIndividually1); cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl; // Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR"); forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate"); model.addProbe(forceSquaredProbeTwiceReportedIndividually2); cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; // Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR"); forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate"); SimTK::Vector initCondVec(2); initCondVec(0) = 0; initCondVec(1) = 10; forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec); model.addProbe(forceSquaredProbeTwiceReportedIndividually3); cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; cout << "initCondVec = " << initCondVec << endl; /* Since all components are allocated on the stack don't have model own them (and try to free)*/ // model.disownAllComponents(); model.setName("testProbesModel"); cout << "Saving model... " << endl; model.print("testProbesModel.osim"); cout << "Re-loading model... " << endl; Model reloadedModel = Model("testProbesModel.osim"); /* Setup analyses and reporters. */ ProbeReporter* probeReporter = new ProbeReporter(&model); model.addAnalysis(probeReporter); ForceReporter* forceReporter = new ForceReporter(&model); model.addAnalysis(forceReporter); MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model); model.addAnalysis(muscleReporter); model.print("testProbesModel.osim"); model.printBasicInfo(cout); //========================================================================== // 3. SIMULATION Initialization //========================================================================== // Initialize the system and get the default state SimTK::State& si = model.initSystem(); SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si); model.getMultibodySystem().realize(si, SimTK::Stage::Dynamics); model.equilibrateMuscles(si); CoordinateSet& modelCoordinateSet = model.updCoordinateSet(); // Define non-zero (defaults are 0) states for the free joint // set x-translation value modelCoordinateSet[0].setValue(si, startX, true); //Copy the initial state SimTK::State initialState(si); // Check muscle is setup correctly const PathActuator &muscle = dynamic_cast<const PathActuator&>(model.updActuators().get("muscle")); double length = muscle.getLength(si); double trueLength = startX + xSinG - anchorWidth / 2; ASSERT_EQUAL(length / trueLength, 1.0, testTolerance, __FILE__, __LINE__, "testMuscles: path failed to initialize to correct length."); model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0); //cout << "Muscle initial energy = " << Emuscle0 << endl; double Esys0 = model.getMultibodySystem().calcEnergy(si); Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0)); double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si); //cout << "Total initial system energy = " << Esys0 << endl; //========================================================================== // 4. SIMULATION Integration //========================================================================== // Create the integrator SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); integrator.setAccuracy(integrationAccuracy); // Create the manager Manager manager(model, integrator); // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); cout << "\nIntegrating from " << initialTime << " to " << finalTime << endl; // Start timing the simulation const clock_t start = clock(); // simulate manager.integrate(si); // how long did it take? double comp_time = (double)(clock() - start) / CLOCKS_PER_SEC; //========================================================================== // 5. SIMULATION Reporting //========================================================================== double realTimeMultiplier = ((finalTime - initialTime) / comp_time); printf("testMuscles: Realtime Multiplier: %f\n" " : simulation duration / clock duration\n" " > 1 : faster than real time\n" " = 1 : real time\n" " < 1 : slower than real time\n", realTimeMultiplier); /* ASSERT(comp_time <= (finalTime-initialTime)); printf("testMuscles: PASSED Realtime test\n" " %s simulation time: %f with accuracy %f\n\n", actuatorType.c_str(), comp_time , accuracy); */ //An analysis only writes to a dir that exists, so create here. if (printResults == true){ Storage states(manager.getStateStorage()); states.print("testProbes_states.sto"); probeReporter->getProbeStorage().print("testProbes_probes.sto"); forceReporter->getForceStorage().print("testProbes_forces.sto"); muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto"); cout << "\nDone with printing results..." << endl; } double muscleWork = muscWorkProbe->getProbeOutputs(si)(0); cout << "Muscle work = " << muscleWork << endl; // Test the resetting of probes cout << "Resetting muscle work probe..." << endl; muscWorkProbe->reset(si); muscleWork = muscWorkProbe->getProbeOutputs(si)(0); cout << "Muscle work = " << muscleWork << endl; ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe."); //========================================================================== // 6. SIMULATION Tests //========================================================================== model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe."); ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe."); ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2 * pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe."); for (int i = 0; i<initCondVec.size(); ++i) { stringstream myError; //myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl; //ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str()); //if (testRealInitConditions(i) != initCondVec(i)) // cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl; } }
// test sphere to sphere contact using elastic foundation with and without // meshes and their combination int testBallToBallContact(bool useElasticFoundation, bool useMesh1, bool useMesh2) { // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity, Vec3(0), Inertia()); osimModel->addBody(&ground); OpenSim::Body ball; ball.setName("ball"); ball.setMass(mass); ball.setMassCenter(Vec3(0)); ball.setInertia(Inertia(1.0)); // Add joints FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0)); osimModel->addBody(&ball); osimModel->addJoint(&free); // Create ContactGeometry. OpenSim::ContactGeometry *ball1, *ball2; if (useElasticFoundation && useMesh1) ball1 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ground, "ball1"); else ball1 = new ContactSphere(radius, Vec3(0), ground, "ball1"); if (useElasticFoundation && useMesh2) ball2 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball2"); else ball2 = new ContactSphere(radius, Vec3(0), ball, "ball2"); osimModel->addContactGeometry(ball1); osimModel->addContactGeometry(ball2); OpenSim::Force* force; std::string prefix; if (useElasticFoundation){ } else{ } if (useElasticFoundation) { // Add an ElasticFoundationForce. OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/(2*radius), 0.001, 0.0, 0.0, 0.0); contactParams->addGeometry("ball1"); contactParams->addGeometry("ball2"); force = new OpenSim::ElasticFoundationForce(contactParams); prefix = "EF_"; prefix += useMesh1 ?"Mesh":"noMesh"; prefix += useMesh2 ? "_to_Mesh":"_to_noMesh"; } else { // Add a Hertz HuntCrossleyForce. OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 0.001, 0.0, 0.0, 0.0); contactParams->addGeometry("ball1"); contactParams->addGeometry("ball2"); force = new OpenSim::HuntCrossleyForce(contactParams); prefix = "Hertz"; } force->setName("contact"); osimModel->addForce(force); osimModel->setGravity(gravity_vec); osimModel->setName(prefix); osimModel->clone()->print(prefix+".osim"); Kinematics* kin = new Kinematics(osimModel); osimModel->addAnalysis(kin); ForceReporter* reporter = new ForceReporter(osimModel); osimModel->addAnalysis(reporter); SimTK::State& osim_state = osimModel->initSystem(); osim_state.updQ()[4] = height; osimModel->getMultibodySystem().realize(osim_state, Stage::Position ); //========================================================================================================== // Simulate it and see if it bounces correctly. cout << "stateY=" << osim_state.getY() << std::endl; RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() ); integrator.setAccuracy(integ_accuracy); integrator.setMaximumStepSize(100*integ_accuracy); Manager manager(*osimModel, integrator); manager.setInitialTime(0.0); manager.setFinalTime(duration); manager.integrate(osim_state); kin->printResults(prefix); reporter->printResults(prefix); osimModel->disownAllComponents(); // model takes ownership of components unless container set is told otherwise delete osimModel; return 0; }
/** * Run a simulation of block sliding with contact on by two muscles sliding with contact */ int main() { clock_t startTime = clock(); try { ////////////////////// // MODEL PARAMETERS // ////////////////////// // Specify body mass of a 20 kg, 0.1m sides of cubed block body double blockMass = 20.0, blockSideLength = 0.1; // Constant distance of constraint to limit the block's motion double constantDistance = 0.2; // Contact parameters double stiffness = 1.0e7, dissipation = 0.1, friction = 0.2, viscosity=0.01; /////////////////////////////////////////// // DEFINE BODIES AND JOINTS OF THE MODEL // /////////////////////////////////////////// // Create an OpenSim model and set its name Model osimModel; osimModel.setName("tugOfWar"); // GROUND BODY // Get a reference to the model's ground body OpenSim::Body& ground = osimModel.getGroundBody(); // Add display geometry to the ground to visualize in the Visualizer and GUI // add a checkered floor ground.addDisplayGeometry("checkered_floor.vtp"); // add anchors for the muscles to be fixed too ground.addDisplayGeometry("block.vtp"); ground.addDisplayGeometry("block.vtp"); // block is 0.1 by 0.1 by 0.1m cube and centered at origin. // transform anchors to be placed at the two extremes of the sliding block (to come) GeometrySet& geometry = ground.updDisplayer()->updGeometrySet(); DisplayGeometry& anchor1 = geometry[1]; DisplayGeometry& anchor2 = geometry[2]; // scale the anchors anchor1.setScaleFactors(Vec3(5, 1, 1)); anchor2.setScaleFactors(Vec3(5, 1, 1)); // reposition the anchors anchor1.setTransform(Transform(Vec3(0, 0.05, 0.35))); anchor2.setTransform(Transform(Vec3(0, 0.05, -0.35))); // BLOCK BODY Vec3 blockMassCenter(0); Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength); // Create a new block body with the specified properties OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia); // Add display geometry to the block to visualize in the GUI block->addDisplayGeometry("block.vtp"); // FREE JOINT // Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0); FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody); // Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet(); // Set the angle and position ranges for the coordinate set double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2}; double positionRange[2] = {-1, 1}; jointCoordinateSet[0].setRange(angleRange); jointCoordinateSet[1].setRange(angleRange); jointCoordinateSet[2].setRange(angleRange); jointCoordinateSet[3].setRange(positionRange); jointCoordinateSet[4].setRange(positionRange); jointCoordinateSet[5].setRange(positionRange); // GRAVITY // Obtaine the default acceleration due to gravity Vec3 gravity = osimModel.getGravity(); // Define non-zero default states for the free joint jointCoordinateSet[3].setDefaultValue(constantDistance); // set x-translation value double h_start = blockMass*gravity[1]/(stiffness*blockSideLength*blockSideLength); jointCoordinateSet[4].setDefaultValue(h_start); // set y-translation which is height // Add the block and joint to the model osimModel.addBody(block); osimModel.addJoint(blockToGround); /////////////////////////////////////////////// // DEFINE THE SIMULATION START AND END TIMES // /////////////////////////////////////////////// // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 3.00; ///////////////////////////////////////////// // DEFINE CONSTRAINTS IMPOSED ON THE MODEL // ///////////////////////////////////////////// Vec3 pointOnGround(0, blockSideLength/2 ,0); Vec3 pointOnBlock(0, 0, 0); // Create a new constant distance constraint ConstantDistanceConstraint *constDist = new ConstantDistanceConstraint(ground, pointOnGround, *block, pointOnBlock, constantDistance); // Add the new point on a line constraint to the model osimModel.addConstraint(constDist); /////////////////////////////////////// // DEFINE FORCES ACTING ON THE MODEL // /////////////////////////////////////// // MUSCLE FORCES // Create two new muscles with identical properties double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0; Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle); Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle); // Specify the paths for the two muscles // Path for muscle 1 muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35)); muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0,0.0,-0.05)); // Path for muscle 2 muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35)); muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0,0.0,0.05)); // Add the two muscles (as forces) to the model osimModel.addForce(muscle1); osimModel.addForce(muscle2); // CONTACT FORCE // Define contact geometry // Create new floor contact halfspace ContactHalfSpace *floor = new ContactHalfSpace(SimTK::Vec3(0), SimTK::Vec3(0, 0, -0.5*SimTK_PI), ground, "floor"); // Create new cube contact mesh OpenSim::ContactMesh *cube = new OpenSim::ContactMesh("blockMesh.obj", SimTK::Vec3(0), SimTK::Vec3(0), *block, "cube"); // Add contact geometry to the model osimModel.addContactGeometry(floor); osimModel.addContactGeometry(cube); // Define contact parameters for elastic foundation force OpenSim::ElasticFoundationForce::ContactParameters *contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(stiffness, dissipation, friction, friction, viscosity); contactParams->addGeometry("cube"); contactParams->addGeometry("floor"); // Create a new elastic foundation (contact) force between the floor and cube. OpenSim::ElasticFoundationForce *contactForce = new OpenSim::ElasticFoundationForce(contactParams); contactForce->setName("contactForce"); // Add the new elastic foundation force to the model osimModel.addForce(contactForce); // PRESCRIBED FORCE // Create a new prescribed force to be applied to the block PrescribedForce *prescribedForce = new PrescribedForce(block); prescribedForce->setName("prescribedForce"); // Specify properties of the force function to be applied to the block double time[2] = {0, finalTime}; // time nodes for linear function double fXofT[2] = {0, -blockMass*gravity[1]*3.0}; // force values at t1 and t2 // Create linear function for the force components PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT); // Set the force and point functions for the new prescribed force prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0)); prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0)); // Add the new prescribed force to the model osimModel.addForce(prescribedForce); /////////////////////////////////// // DEFINE CONTROLS FOR THE MODEL // /////////////////////////////////// // Create a prescribed controller that simply applies controls as function of time // For muscles, controls are normalized motor-neuron excitations PrescribedController *muscleController = new PrescribedController(); muscleController->setActuators(osimModel.updActuators()); // Define linear functions for the control values for the two muscles Array<double> slopeAndIntercept1(0.0, 2); // array of 2 doubles Array<double> slopeAndIntercept2(0.0, 2); // muscle1 control has slope of -1 starting 1 at t = 0 slopeAndIntercept1[0] = -1.0/(finalTime-initialTime); slopeAndIntercept1[1] = 1.0; // muscle2 control has slope of 0.95 starting 0.05 at t = 0 slopeAndIntercept2[0] = 0.95/(finalTime-initialTime); slopeAndIntercept2[1] = 0.05; // Set the indiviudal muscle control functions for the prescribed muscle controller muscleController->prescribeControlForActuator("muscle1", new LinearFunction(slopeAndIntercept1)); muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2)); // Add the muscle controller to the model osimModel.addController(muscleController); /////////////////////////////////// // SPECIFY MODEL DEFAULT STATES // /////////////////////////////////// // Define the default states for the two muscles // Activation muscle1->setDefaultActivation(slopeAndIntercept1[1]); muscle2->setDefaultActivation(slopeAndIntercept2[1]); // Fiber length muscle2->setDefaultFiberLength(optimalFiberLength); muscle1->setDefaultFiberLength(optimalFiberLength); // Save the model to a file osimModel.print("tugOfWar_model.osim"); ////////////////////////// // PERFORM A SIMULATION // ////////////////////////// // set use visualizer to true to visualize the simulation live osimModel.setUseVisualizer(false); // Initialize the system and get the default state SimTK::State& si = osimModel.initSystem(); // Enable constraint consistent with current configuration of the model constDist->setDisabled(si, false); cout << "Start height = "<< h_start << endl; osimModel.getMultibodySystem().realize(si, Stage::Velocity); // Compute initial conditions for muscles osimModel.equilibrateMuscles(si); double mfv1 = muscle1->getFiberVelocity(si); double mfv2 = muscle2->getFiberVelocity(si); // Create the force reporter for obtaining the forces applied to the model // during a forward simulation ForceReporter* reporter = new ForceReporter(&osimModel); osimModel.addAnalysis(reporter); // Create the integrator for integrating system dynamics SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem()); integrator.setAccuracy(1.0e-6); // Create the manager managing the forward integration and its outputs Manager manager(osimModel, integrator); // Print out details of the model osimModel.printDetailedInfo(si, cout); // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<endl; manager.integrate(si); ////////////////////////////// // SAVE THE RESULTS TO FILE // ////////////////////////////// // Save the model states from forward integration Storage statesDegrees(manager.getStateStorage()); statesDegrees.print("tugOfWar_states.sto"); // Save the forces reporter->getForceStorage().print("tugOfWar_forces.mot"); } catch (const std::exception& ex) { cerr << ex.what() << endl; return 1; } catch (...) { cerr << "UNRECOGNIZED EXCEPTION" << endl; return 1; } cout << "main() routine time = " << 1.e3*(clock()-startTime)/CLOCKS_PER_SEC << "ms\n"; cout << "OpenSim example completed successfully." << endl; return 0; }
void testCoordinateLimitForce() { using namespace SimTK; double mass = 1; double ball_radius = 0.25; // Setup OpenSim model Model *osimModel = new Model; osimModel->setName("CoordinateLimitForceTest"); //OpenSim bodies OpenSim::Body& ground = osimModel->getGroundBody(); OpenSim::Body ball("ball", mass ,Vec3(0), mass*SimTK::Inertia::sphere(0.1)); ball.addDisplayGeometry("sphere.vtp"); ball.scale(Vec3(ball_radius), false); // Add joints SliderJoint slider("", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2)); double positionRange[2] = {0.1, 2}; // Rename coordinates for a slider joint CoordinateSet &slider_coords = slider.upd_CoordinateSet(); slider_coords[0].setName("ball_h"); slider_coords[0].setRange(positionRange); slider_coords[0].setMotionType(Coordinate::Translational); osimModel->addBody(&ball); osimModel->addJoint(&slider); osimModel->setGravity(gravity_vec); // Define the parameters of the Coordinate Limit Force double K_upper = 200.0; double K_lower = 1000.0; double damping = 0.01; double trans = 0.05; CoordinateLimitForce limitForce("ball_h", positionRange[1], K_upper, positionRange[0], K_lower, damping, trans, true); osimModel->addForce(&limitForce); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); osimModel->print("CoordinateLimitForceTest.osim"); // Check serialization and deserilaization Model* loadedModel = new Model("CoordinateLimitForceTest.osim"); ASSERT(*loadedModel == *osimModel, "Deserialized CoordinateLimitForceTest failed to be equivalent to original."); // check copy Model *copyModel = osimModel->clone(); ASSERT(*copyModel == *loadedModel, "Clone of CoordinateLimitForceTest failed to be equivalent to original."); copyModel->print("cloneCoordinateLimitForceTest.osim"); delete osimModel; osimModel = copyModel; // Create the force reporter ForceReporter* reporter = new ForceReporter(osimModel); osimModel->addAnalysis(reporter); SimTK::State& osim_state = osimModel->initSystem(); double dh = 0.2; double start_h = positionRange[1]; double start_v = 2.0; const Coordinate &q_h = osimModel->getCoordinateSet()[0]; q_h.setValue(osim_state, start_h); q_h.setSpeedValue(osim_state, start_v); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration ); CoordinateLimitForce *clf = dynamic_cast<CoordinateLimitForce *>(&osimModel->getForceSet()[0]); // initial energy of the system; double clfPE = clf->computePotentialEnergy(osim_state); double constStiffnessPE = 0.5*K_upper*dh*dh; ASSERT(clfPE < constStiffnessPE); double energy0 = clfPE + mass*(-gravity_vec[1])*start_h + 0.5*mass*start_v*start_v; // system KE + PE including strain energy in CLF double eSys0 = osimModel->getMultibodySystem().calcEnergy(osim_state); // instantaneous power dissipation double clfPowerDissipation = clf->getPowerDissipation(osim_state); //========================================================================== // 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 = 1.0; double nsteps = 20; double dt = final_t/nsteps; for(int i = 1; i <=nsteps; i++){ manager.setFinalTime(dt*i); manager.integrate(osim_state); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); double h = q_h.getValue(osim_state); double v = q_h.getSpeedValue(osim_state); //Now check that the force reported by spring Array<double> model_force = clf->getRecordValues(osim_state); double ediss = clf->getDissipatedEnergy(osim_state); double clfE = clf->computePotentialEnergy(osim_state) + ediss; // EK + EM of mass alone double eMass = 0.5*mass*v*v - mass*gravity_vec[1]*h; double eSpringGuess = energy0-eMass; double e = eMass+clfE; // system KE + PE including strain energy in CLF double eSys = osimModel->getMultibodySystem().calcEnergy(osim_state)+ ediss; ASSERT_EQUAL(1.0, e/energy0, integ_accuracy, "CoordinateLimitForce Failed to conserve energy"); ASSERT_EQUAL(1.0, eSys/eSys0, integ_accuracy, "CoordinateLimitForce Failed to conserve system energy"); // get the forces applied to the ball by the limit force double analytical_force = 0; if(h > (positionRange[1]+trans)){ ASSERT_EQUAL(-K_upper*(h-positionRange[1])-damping*v, model_force[0], 1e-4); } else if( h < (positionRange[0]-trans)){ ASSERT_EQUAL(K_lower*(positionRange[0]-h)-damping*v, model_force[0], 1e-4); } else if( (h < positionRange[1]) && (h > positionRange[0])){ // Verify no force is being applied when within limits ASSERT_EQUAL(0.0, model_force[0], 1e-5); } manager.setInitialTime(dt*i); } manager.getStateStorage().print("coordinte_limit_force_model_states.sto"); // Save the forces reporter->getForceStorage().print("limit_forces.mot"); }
// Test our wraapping of Hunt-Crossley force in OpenSim // Simple simulation of bouncing ball with dissipation should generate contact // forces that settle to ball weight. void testHuntCrossleyForce() { using namespace SimTK; double start_h = 0.5; // Setup OpenSim model Model *osimModel = new Model("BouncingBall_HuntCrossley.osim"); // Create the force reporter ForceReporter* reporter = new ForceReporter(osimModel); osimModel->addAnalysis(reporter); SimTK::State& osim_state = osimModel->initSystem(); osimModel->getCoordinateSet()[4].setValue(osim_state, start_h); osimModel->getMultibodySystem().realize(osim_state, Stage::Position ); const OpenSim::Body &ball = osimModel->getBodySet().get("ball"); //========================================================================== // 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 = 2.0; manager.setFinalTime(final_t); // start timing clock_t startTime = clock(); manager.integrate(osim_state); // end timing cout << "Hunt Crossley simulation time = " << 1.e3*(clock()-startTime)/CLOCKS_PER_SEC << "ms" << endl; //make sure we can access dynamic variables osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); // Print out the motion for visualizing/debugging manager.getStateStorage().print("bouncing_ball_HC_states.sto"); // Save the forces reporter->getForceStorage().print("HuntCrossley_contact_forces.mot"); // Bouncing ball should have settled to rest on groun due to dissipation // In that case the force generated by contact should be identically body weight // in vertical and zero else where. OpenSim::HuntCrossleyForce &contact = (OpenSim::HuntCrossleyForce &)osimModel->getForceSet().get("contact"); Array<double> contact_force = contact.getRecordValues(osim_state); ASSERT_EQUAL(contact_force[0], 0.0, 1e-4); // no horizontal force on the ball ASSERT_EQUAL(contact_force[1], -ball.getMass()*gravity_vec[1], 1e-3); // vertical is weight ASSERT_EQUAL(contact_force[2], 0.0, 1e-4); // no horizontal force on the ball ASSERT_EQUAL(contact_force[3], 0.0, 1e-4); // no torque on the ball ASSERT_EQUAL(contact_force[4], 0.0, 1e-4); // no torque on the ball ASSERT_EQUAL(contact_force[5], 0.0, 1e-4); // no torque on the ball // Before exiting lets see if copying the force works OpenSim::HuntCrossleyForce *copyOfForce = contact.clone(); bool isEqual = (*copyOfForce == contact); if(!isEqual){ contact.print("originalForce.xml"); copyOfForce->print("copyOfForce.xml"); } ASSERT(isEqual); }
void testFunctionBasedBushingForce() { using namespace SimTK; double mass = 1; double stiffness = 10; double restlength = 0.0; 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("FunctionBasedBushingTest"); //OpenSim bodies OpenSim::Body& ground = osimModel->getGroundBody(); OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1)); ball.addDisplayGeometry("sphere.vtp"); ball.scale(Vec3(ball_radius), false); // Add joints SliderJoint slider("", ground, Vec3(0), Vec3(0,0,Pi/2), ball, 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("ball_h"); slider_coords[0].setRange(positionRange); slider_coords[0].setMotionType(Coordinate::Translational); osimModel->addBody(&ball); osimModel->addJoint(&slider); Vec3 rotStiffness(0); Vec3 transStiffness(stiffness); Vec3 rotDamping(0); Vec3 transDamping(0); osimModel->setGravity(gravity_vec); FunctionBasedBushingForce spring("ground", Vec3(0), Vec3(0), "ball", Vec3(0), Vec3(0), transStiffness, rotStiffness, transDamping, rotDamping); spring.setName("linear_bushing"); osimModel->addForce(&spring); osimModel->print("FunctionBasedBushingForceModel.osim"); // Create the force reporter ForceReporter* reporter = new ForceReporter(osimModel); osimModel->addAnalysis(reporter); 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 = 2.0; double nsteps = 10; double dt = final_t/nsteps; for(int i = 1; i <=nsteps; i++){ manager.setFinalTime(dt*i); manager.integrate(osim_state); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); Vec3 pos; osimModel->updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos); double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh; ASSERT_EQUAL(height, pos(1), 1e-4); //Now check that the force reported by spring Array<double> model_force = spring.getRecordValues(osim_state); // get the forces applied to the ground and ball double analytical_force = -stiffness*height; // analytical force corresponds in direction to the force on the ball Y index = 7 ASSERT_EQUAL(analytical_force, model_force[7], 2e-4); manager.setInitialTime(dt*i); } osimModel->disownAllComponents(); manager.getStateStorage().print("function_based_bushing_model_states.sto"); // Save the forces reporter->getForceStorage().print("function_based_bushing_forces.mot"); // Before exiting lets see if copying the spring works FunctionBasedBushingForce *copyOfSpring = spring.clone(); ASSERT(*copyOfSpring == spring); }
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(); }
void testCoordinateLimitForceRotational() { using namespace SimTK; double mass = 1; double edge = 0.2; // Setup OpenSim model Model *osimModel = new Model; osimModel->setName("RotationalCoordinateLimitForceTest"); //OpenSim bodies OpenSim::Body& ground = osimModel->getGroundBody(); OpenSim::Body block("block", mass ,Vec3(0), mass*SimTK::Inertia::brick(edge,edge,edge)); block.addDisplayGeometry("box.vtp"); block.scale(Vec3(edge), false); // Add joints PinJoint pin("", ground, Vec3(0), Vec3(0,0,0), block, Vec3(0,-edge,0), Vec3(0,0,0)); // NOTE: Angular limits are in degrees NOT radians double positionRange[2] = {-30, 90}; // Rename coordinates for a slider joint CoordinateSet &pin_coords = pin.upd_CoordinateSet(); pin_coords[0].setName("theta"); pin_coords[0].setRange(positionRange); pin_coords[0].setMotionType(Coordinate::Rotational); osimModel->addBody(&block); osimModel->addJoint(&pin); osimModel->setGravity(Vec3(0)); // Define the parameters of the Coordinate Limit Force // For rotational coordinates, these are in Nm/degree double K_upper = 10.0; double K_lower = 20.0; double damping = 0.1; double trans = 0.05; CoordinateLimitForce limitForce("theta", positionRange[1], K_upper, positionRange[0], K_lower, damping, trans, true); osimModel->addForce(&limitForce); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); // Create the force reporter ForceReporter* reporter = new ForceReporter(osimModel); osimModel->addAnalysis(reporter); SimTK::State& osim_state = osimModel->initSystem(); // Start 2 degrees beyond the upper limit double start_q = SimTK_DEGREE_TO_RADIAN*positionRange[1] + SimTK::Pi/90; double start_v = 0.0; const Coordinate &coord = osimModel->getCoordinateSet()[0]; coord.setValue(osim_state, start_q); coord.setSpeedValue(osim_state, start_v); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration ); CoordinateLimitForce *clf = dynamic_cast<CoordinateLimitForce *>(&osimModel->getForceSet()[0]); //Now check that the force reported by spring Array<double> model_force = clf->getRecordValues(osim_state); ASSERT_EQUAL(model_force[0]/(-2*K_upper), 1.0, integ_accuracy); double clfPE = clf->computePotentialEnergy(osim_state); double constSpringPE = 0.5*(K_upper*2.0)*2.0*SimTK_DEGREE_TO_RADIAN; ASSERT_EQUAL(clfPE/constSpringPE, 1.0, 0.001, "Specified upper rotational stiffness not met."); ASSERT(clfPE < constSpringPE); //Now test lower bound start_q = SimTK_DEGREE_TO_RADIAN*positionRange[0] - SimTK::Pi/90; coord.setValue(osim_state, start_q); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration ); model_force = clf->getRecordValues(osim_state); ASSERT_EQUAL(model_force[0]/(2*K_lower), 1.0, integ_accuracy); clfPE = clf->computePotentialEnergy(osim_state); constSpringPE = 0.5*(K_lower*2.0)*2.0*SimTK_DEGREE_TO_RADIAN; ASSERT_EQUAL(clfPE/constSpringPE, 1.0, 0.001); ASSERT(clfPE < constSpringPE); // total system energy prior to simulation double eSys0 = osimModel->getMultibodySystem().calcEnergy(osim_state); //========================================================================== // Perform a simulation an monitor energy conservation RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() ); integrator.setAccuracy(1e-8); Manager manager(*osimModel, integrator); manager.setInitialTime(0.0); double final_t = 1.0; double nsteps = 20; double dt = final_t/nsteps; for(int i = 1; i <=nsteps; i++){ manager.setFinalTime(dt*i); manager.integrate(osim_state); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); double q = coord.getValue(osim_state); double u = coord.getSpeedValue(osim_state); double ediss = clf->getDissipatedEnergy(osim_state); // system KE + PE including strain energy in CLF double eSys = osimModel->getMultibodySystem().calcEnergy(osim_state)+ ediss; double EKsys = osimModel->getMultibodySystem().calcKineticEnergy(osim_state); ASSERT_EQUAL(eSys/eSys0, 1.0, integ_accuracy); manager.setInitialTime(dt*i); } manager.getStateStorage().print("rotational_coordinte_limit_force_states.sto"); // Save the forces reporter->getForceStorage().print("rotational_limit_forces.mot"); }