/** * 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; }
/** * Method for building the Luxo Jr articulating model. It sets up the system of * rigid bodies and joint articulations to define Luxo Jr lamp geometry. */ void createLuxoJr(OpenSim::Model &model){ // Create base //-------------- OpenSim::Body* base = new OpenSim::Body("base", baseMass, Vec3(0.0), Inertia::cylinderAlongY(0.1, baseHeight)); // Add visible geometry base->attachMeshGeometry("Base_meters.obj"); // Define base to float relative to ground via free joint FreeJoint* base_ground = new FreeJoint("base_ground", // parent body, location in parent body, orientation in parent model.getGround(), Vec3(0.0), Vec3(0.0), // child body, location in child body, orientation in child *base, Vec3(0.0,-baseHeight/2.0,0.0),Vec3(0.0)); // add base to model model.addBody(base); model.addJoint(base_ground); /*for (int i = 0; i<base_ground->get_CoordinateSet().getSize(); ++i) { base_ground->upd_CoordinateSet()[i].set_locked(true); }*/ // Fix a frame to the base axis for attaching the bottom bracket SimTK::Transform* shift_and_rotate = new SimTK::Transform(); //shift_and_rotate->setToZero(); shift_and_rotate->set(Rotation(-1*SimTK::Pi/2, SimTK::CoordinateAxis::XCoordinateAxis()), Vec3(0.0, bracket_location, 0.0)); PhysicalOffsetFrame pivot_frame_on_base("pivot_frame_on_base", *base, *shift_and_rotate); // Create bottom bracket //----------------------- OpenSim::Body* bottom_bracket = new OpenSim::Body("bottom_bracket", bracket_mass, Vec3(0.0), Inertia::brick(0.03, 0.03, 0.015)); // add bottom bracket to model model.addBody(bottom_bracket); // Fix a frame to the bracket for attaching joint shift_and_rotate->setP(Vec3(0.0)); PhysicalOffsetFrame pivot_frame_on_bottom_bracket( "pivot_frame_on_bottom_bracket", *bottom_bracket, *shift_and_rotate); // Add visible geometry bottom_bracket->attachMeshGeometry("bottom_bracket_meters.obj"); // Make bottom bracket to twist on base with vertical pin joint. // You can create a joint from any existing physical frames attached to // rigid bodies. One way to reference them is by name, like this... PinJoint* base_pivot = new PinJoint("base_pivot", pivot_frame_on_base, pivot_frame_on_bottom_bracket); base_pivot->append_frames(pivot_frame_on_base); base_pivot->append_frames(pivot_frame_on_bottom_bracket); // add base pivot joint to the model model.addJoint(base_pivot); // add some damping to the pivot // initialized to zero stiffness and damping BushingForce* pivotDamper = new BushingForce("pivot_bushing", "pivot_frame_on_base", "pivot_frame_on_bottom_bracket"); pivotDamper->set_rotational_damping(pivot_damping); model.addForce(pivotDamper); // Create posterior leg //----------------------- OpenSim::Body* posteriorLegBar = new OpenSim::Body("posterior_leg_bar", bar_mass, Vec3(0.0), Inertia::brick(leg_bar_dimensions/2.0)); posteriorLegBar->attachMeshGeometry("Leg_meters.obj"); PhysicalOffsetFrame posterior_knee_on_bottom_bracket( "posterior_knee_on_bottom_bracket", *bottom_bracket, Transform(posterior_bracket_hinge_location) ); PhysicalOffsetFrame posterior_knee_on_posterior_bar( "posterior_knee_on_posterior_bar", *posteriorLegBar, Transform(inferior_bar_hinge_location) ); // Attach posterior leg to bottom bracket using another pin joint. // Another way to reference physical frames in a joint is by creating them // in place, like this... OpenSim::PinJoint* posteriorKnee = new OpenSim::PinJoint("posterior_knee", posterior_knee_on_bottom_bracket, posterior_knee_on_posterior_bar); // posteriorKnee will own and serialize the attachment offset frames posteriorKnee->append_frames(posterior_knee_on_bottom_bracket); posteriorKnee->append_frames(posterior_knee_on_posterior_bar); // add posterior leg to model model.addBody(posteriorLegBar); model.addJoint(posteriorKnee); // allow this joint's coordinate to float freely when assembling constraints // the joint we create next will drive the pose of the 4-bar linkage posteriorKnee->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Create anterior leg Hlink //---------------------------- OpenSim::Body* leg_Hlink = new OpenSim::Body("leg_Hlink", bar_mass, Vec3(0.0), Inertia::brick(leg_Hlink_dimensions/2.0)); leg_Hlink->attachMeshGeometry("H_Piece_meters.obj"); PhysicalOffsetFrame anterior_knee_on_bottom_bracket( "anterior_knee_on_bottom_bracket", *bottom_bracket, Transform(anterior_bracket_hinge_location)); PhysicalOffsetFrame anterior_knee_on_anterior_bar( "anterior_knee_on_anterior_bar", *leg_Hlink, Transform(inferior_Hlink_hinge_location)); // Connect anterior leg to bottom bracket via pin joint OpenSim::PinJoint* anterior_knee = new OpenSim::PinJoint("anterior_knee", anterior_knee_on_bottom_bracket, anterior_knee_on_anterior_bar); anterior_knee->append_frames(anterior_knee_on_bottom_bracket); anterior_knee->append_frames(anterior_knee_on_anterior_bar); // add anterior leg to model model.addBody(leg_Hlink); model.addJoint(anterior_knee); // this anterior knee joint defines the motion of the lower 4-bar linkage // set it's default coordinate value to a slightly flexed position. anterior_knee->upd_CoordinateSet()[0].set_default_value(SimTK::Pi/6); // Create pelvis bracket //----------------------- OpenSim::Body* pelvisBracket = new OpenSim::Body("pelvis_bracket", bracket_mass, Vec3(0.0), Inertia::brick(pelvis_dimensions/2.0)); pelvisBracket->attachMeshGeometry("Pelvis_bracket_meters.obj"); // Connect pelvis to Hlink via pin joint SimTK::Transform pelvis_anterior_shift( anterior_superior_pelvis_pin_location); PhysicalOffsetFrame anterior_hip_on_Hlink( "anterior_hip_on_Hlink", *leg_Hlink, Transform(superior_Hlink_hinge_location)); PhysicalOffsetFrame anterior_hip_on_pelvis( "anterior_hip_on_pelvis", *pelvisBracket, pelvis_anterior_shift); OpenSim::PinJoint* anteriorHip = new OpenSim::PinJoint("anterior_hip", anterior_hip_on_Hlink, anterior_hip_on_pelvis); anteriorHip->append_frames(anterior_hip_on_Hlink); anteriorHip->append_frames(anterior_hip_on_pelvis); // add anterior leg to model model.addBody(pelvisBracket); model.addJoint(anteriorHip); // since the previous, anterior knee joint drives the pose of the lower // 4-bar linkage, set the anterior hip angle such that it's free to satisfy // constraints that couple it to the 4-bar linkage. anteriorHip->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Close the loop for the lower, four-bar linkage with a constraint //------------------------------------------------------------------ // Create and configure point on line constraint OpenSim::PointOnLineConstraint* posteriorHip = new OpenSim::PointOnLineConstraint(); posteriorHip->setLineBodyByName(pelvisBracket->getName()); posteriorHip->setLineDirection(Vec3(0.0,0.0,1.0)); posteriorHip->setPointOnLine(inferior_pelvis_pin_location); posteriorHip->setFollowerBodyByName(posteriorLegBar->getName()); posteriorHip->setPointOnFollower(superior_bar_hinge_location); // add constraint to model model.addConstraint(posteriorHip); // Create chest piece //----------------------- OpenSim::Body* chest = new OpenSim::Body("chest_bar", bar_mass, Vec3(0.0), Inertia::brick(torso_bar_dimensions/2.0)); chest->attachMeshGeometry("Anterior_torso_bar.obj"); PhysicalOffsetFrame anterior_torso_hinge_on_pelvis( "anterior_torso_hinge_on_pelvis", *pelvisBracket, Transform(anterior_superior_pelvis_pin_location) ); PhysicalOffsetFrame anterior_torso_hinge_on_chest( "anterior_torso_hinge_on_chest", *chest, Transform(inferior_torso_hinge_location) ); // Attach chest piece to pelvice with pin joint OpenSim::PinJoint* anteriorTorsoHinge = new OpenSim::PinJoint( "anterior_torso_hinge", anterior_torso_hinge_on_pelvis, anterior_torso_hinge_on_chest); anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_pelvis); anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_chest); // add posterior leg to model model.addBody(chest); model.addJoint(anteriorTorsoHinge); // set torso rotation slightly anterior anteriorTorsoHinge->upd_CoordinateSet()[0].setDefaultValue(-1*SimTK::Pi/4); // Create chest piece //----------------------- OpenSim::Body* back = new OpenSim::Body("back_bar", bar_mass, Vec3(0.0), Inertia::brick(torso_bar_dimensions/2.0)); back->attachMeshGeometry("Posterior_torso_bar.obj"); PhysicalOffsetFrame posterior_torso_hinge_on_pelvis( "posterior_torso_hinge_on_pelvis", *pelvisBracket, Transform(posterior_superior_pelvis_pin_location) ); PhysicalOffsetFrame posterior_torso_hinge_on_back( "posterior_torso_hinge_on_back", *back, Transform(back_peg_center) ); // Attach chest piece to pelvis with pin joint OpenSim::PinJoint* posteriorTorsoHinge = new OpenSim::PinJoint( "posterior_torso_hinge", posterior_torso_hinge_on_pelvis, posterior_torso_hinge_on_back); posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_pelvis); posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_back); // add posterior leg to model model.addBody(back); model.addJoint(posteriorTorsoHinge); // set posterior back joint to freely follow anterior joint through 4-bar // linkage coupling. posteriorTorsoHinge->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Create shoulder bracket //----------------------- OpenSim::Body* shoulderBracket = new OpenSim::Body("shoulder_bracket", bracket_mass, Vec3(0.0), Inertia::brick(shoulder_dimensions/2.0)); shoulderBracket->attachMeshGeometry("Shoulder_meters.obj"); // add anterior leg to model model.addBody(shoulderBracket); PhysicalOffsetFrame anterior_thoracic_joint_on_chest( "anterior_thoracic_joint_on_chest", *chest, Transform(superior_torso_hinge_location) ); PhysicalOffsetFrame anterior_thoracic_joint_on_shoulder( "anterior_thoracic_joint_on_shoulder", *shoulderBracket, Transform(anterior_thoracic_joint_center)); // Connect pelvis to Hlink via pin joint OpenSim::PinJoint* anteriorThoracicJoint = new OpenSim::PinJoint("anterior_thoracic_joint", anterior_thoracic_joint_on_chest, anterior_thoracic_joint_on_shoulder); anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_chest); anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_shoulder); // add back joint model.addJoint(anteriorThoracicJoint); // since the previous, anterior thoracic joint drives the pose of the lower // 4-bar linkage, set the anterior shoulder angle such that it's free to // satisfy constraints that couple it to the 4-bar linkage. anteriorThoracicJoint->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Close the loop for the lower, four-bar linkage with a constraint //------------------------------------------------------------------ // Create and configure point on line constraint OpenSim::PointOnLineConstraint* posteriorShoulder = new OpenSim::PointOnLineConstraint(); posteriorShoulder->setLineBodyByName(shoulderBracket->getName()); posteriorShoulder->setLineDirection(Vec3(0.0,0.0,1.0)); posteriorShoulder->setPointOnLine(posterior_thoracic_joint_center); posteriorShoulder->setFollowerBodyByName(back->getName()); posteriorShoulder->setPointOnFollower(superior_torso_hinge_location); // add constraint to model model.addConstraint(posteriorShoulder); // Create and add luxo head OpenSim::Body* head = new OpenSim::Body("head", head_mass, Vec3(0), Inertia::cylinderAlongX(0.5*head_dimension[1], head_dimension[1])); head->attachMeshGeometry("luxo_head_meters.obj"); head->attachMeshGeometry("Bulb_meters.obj"); model.addBody(head); PhysicalOffsetFrame cervical_joint_on_shoulder("cervical_joint_on_shoulder", *shoulderBracket, Transform(superior_shoulder_hinge_location) ); PhysicalOffsetFrame cervical_joint_on_head("cervical_joint_on_head", *head, Transform(cervicle_joint_center)); // attach to shoulder via pin joint OpenSim::PinJoint* cervicalJoint = new OpenSim::PinJoint("cervical_joint", cervical_joint_on_shoulder, cervical_joint_on_head); cervicalJoint->append_frames(cervical_joint_on_shoulder); cervicalJoint->append_frames(cervical_joint_on_head); // add a neck joint model.addJoint(cervicalJoint); // lock the kneck coordinate so the head doens't spin without actuators or // passive forces cervicalJoint->upd_CoordinateSet()[0].set_locked(true); // Coordinate Limit forces for restricting leg range of motion. //----------------------------------------------------------------------- CoordinateLimitForce* kneeLimitForce = new CoordinateLimitForce( anterior_knee->get_CoordinateSet()[0].getName(), knee_flexion_max, joint_softstop_stiffness, knee_flexion_min, joint_softstop_stiffness, joint_softstop_damping, transition_region); model.addForce(kneeLimitForce); // Coordinate Limit forces for restricting back range motion. //----------------------------------------------------------------------- CoordinateLimitForce* backLimitForce = new CoordinateLimitForce( anteriorTorsoHinge->get_CoordinateSet()[0].getName(), back_extension_max, joint_softstop_stiffness, back_extension_min, joint_softstop_stiffness, joint_softstop_damping, transition_region); model.addForce(backLimitForce); // Contact //----------------------------------------------------------------------- ContactHalfSpace* floor_surface = new ContactHalfSpace(SimTK::Vec3(0), SimTK::Vec3(0, 0, -0.5*SimTK::Pi), model.updGround(), "floor_surface"); OpenSim::ContactMesh* foot_surface = new ContactMesh( "thin_disc_0.11_by_0.01_meters.obj", SimTK::Vec3(0), SimTK::Vec3(0), *base, "foot_surface"); // add contact geometry to model model.addContactGeometry(floor_surface); model.addContactGeometry(foot_surface); // define contact as an elastic foundation force OpenSim::ElasticFoundationForce::ContactParameters* contactParameters = new OpenSim::ElasticFoundationForce::ContactParameters( stiffness, dissipation, friction, friction, viscosity); contactParameters->addGeometry("foot_surface"); contactParameters->addGeometry("floor_surface"); OpenSim::ElasticFoundationForce* contactForce = new OpenSim::ElasticFoundationForce(contactParameters); contactForce->setName("contact_force"); model.addForce(contactForce); // MUSCLES //----------------------------------------------------------------------- // add a knee extensor to control the lower 4-bar linkage Millard2012EquilibriumMuscle* kneeExtensorRight = new Millard2012EquilibriumMuscle( "knee_extensor_right", knee_extensor_F0, knee_extensor_lm0, knee_extensor_lts, pennationAngle); kneeExtensorRight->addNewPathPoint("knee_extensor_right_origin", *leg_Hlink, knee_extensor_origin); kneeExtensorRight->addNewPathPoint("knee_extensor_right_insertion", *bottom_bracket, knee_extensor_insertion); kneeExtensorRight->set_ignore_tendon_compliance(true); model.addForce(kneeExtensorRight); // add a second copy of this knee extensor for the left side Millard2012EquilibriumMuscle* kneeExtensorLeft = new Millard2012EquilibriumMuscle(*kneeExtensorRight); kneeExtensorLeft->setName("kneeExtensorLeft"); // flip the z coordinates of all path points PathPointSet& points = kneeExtensorLeft->updGeometryPath().updPathPointSet(); for (int i=0; i<points.getSize(); ++i) { points[i].setLocationCoord(2, -1*points[i].getLocationCoord(2)); } kneeExtensorLeft->set_ignore_tendon_compliance(true); model.addForce(kneeExtensorLeft); // add a back extensor to controll the upper 4-bar linkage Millard2012EquilibriumMuscle* backExtensorRight = new Millard2012EquilibriumMuscle( "back_extensor_right", back_extensor_F0, back_extensor_lm0, back_extensor_lts, pennationAngle); backExtensorRight->addNewPathPoint("back_extensor_right_origin", *chest, back_extensor_origin); backExtensorRight->addNewPathPoint("back_extensor_right_insertion", *back, back_extensor_insertion); backExtensorRight->set_ignore_tendon_compliance(true); model.addForce(backExtensorRight); // copy right back extensor and use to make left extensor Millard2012EquilibriumMuscle* backExtensorLeft = new Millard2012EquilibriumMuscle(*backExtensorRight); backExtensorLeft->setName("back_extensor_left"); PathPointSet& pointsLeft = backExtensorLeft->updGeometryPath() .updPathPointSet(); for (int i=0; i<points.getSize(); ++i) { pointsLeft[i].setLocationCoord(2, -1*pointsLeft[i].getLocationCoord(2)); } backExtensorLeft->set_ignore_tendon_compliance(true); model.addForce(backExtensorLeft); // MUSCLE CONTROLLERS //________________________________________________________________________ // specify a piecwise linear function for the muscle excitations PiecewiseConstantFunction* x_of_t = new PiecewiseConstantFunction(3, times, excitations); PrescribedController* kneeController = new PrescribedController(); kneeController->addActuator(*kneeExtensorLeft); kneeController->addActuator(*kneeExtensorRight); kneeController->prescribeControlForActuator(0, x_of_t); kneeController->prescribeControlForActuator(1, x_of_t->clone()); model.addController(kneeController); PrescribedController* backController = new PrescribedController(); backController->addActuator(*backExtensorLeft); backController->addActuator(*backExtensorRight); backController->prescribeControlForActuator(0, x_of_t->clone()); backController->prescribeControlForActuator(1, x_of_t->clone()); model.addController(backController); /* You'll find that these muscles can make Luxo Myo stand, but not jump. * Jumping will require an assistive device. We'll add two frames for * attaching a point to point assistive actuator. */ // add frames for connecting a back assitance device between the chest // and pelvis PhysicalOffsetFrame* back_assist_origin_frame = new PhysicalOffsetFrame("back_assist_origin", *chest, back_assist_origin_transform); PhysicalOffsetFrame* back_assist_insertion_frame = new PhysicalOffsetFrame("back_assist_insertion", *pelvisBracket, back_assist_insertion_transform); model.addFrame(back_assist_origin_frame); model.addFrame(back_assist_insertion_frame); // add frames for connecting a knee assistance device between the posterior // leg and bottom bracket. PhysicalOffsetFrame* knee_assist_origin_frame = new PhysicalOffsetFrame("knee_assist_origin", *posteriorLegBar, knee_assist_origin_transform); PhysicalOffsetFrame* knee_assist_insertion_frame = new PhysicalOffsetFrame("knee_assist_insertion", *bottom_bracket, knee_assist_insertion_transform); model.addFrame(knee_assist_origin_frame); model.addFrame(knee_assist_insertion_frame); // Temporary: make the frame geometry disappear. for (auto& c : model.getComponentList<OpenSim::FrameGeometry>()) { const_cast<OpenSim::FrameGeometry*>(&c)->set_scale_factors( SimTK::Vec3(0.001, 0.001, 0.001)); } }
// Test our wrapping of elastic foundation in OpenSim // Simple simulation of bouncing ball with dissipation should generate contact // forces that settle to ball weight. void testElasticFoundation() { using namespace SimTK; double start_h = 0.5; // Setup OpenSim model Model *osimModel = new Model("BouncingBallModelEF.osim"); // Create the force reporter ForceReporter* reporter = new ForceReporter(osimModel); osimModel->addAnalysis(reporter); SimTK::State& osim_state = osimModel->initSystem(); osimModel->getCoordinateSet().get("ball_ty").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 << "Elastic Foundation 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_states.sto"); // Save the forces reporter->getForceStorage().print("elastic_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::ElasticFoundationForce &contact = (OpenSim::ElasticFoundationForce &)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], 2e-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 spring works OpenSim::ElasticFoundationForce *copyOfForce = contact.clone(); bool isEqual = (*copyOfForce == contact); if(!isEqual){ contact.print("originalForce.xml"); copyOfForce->print("copyOfForce.xml"); } ASSERT(isEqual); }