void testCoordinateCouplerConstraint() { using namespace SimTK; cout << endl; cout << "=================================================================" << endl; cout << " OpenSim CoordinateCouplerConstraint vs. FunctionBasedMobilizer " << endl; cout << "=================================================================" << endl; // Define spline data for the custom knee joint int npx = 12; double angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197, -0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443, 0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393}; double kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000, 0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000, -0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000}; int npy = 7; double angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399, -0.174532925199, 0.159148563428, 2.094395102393}; double kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000, -0.396600000000, -0.395264000000, -0.396000000000 }; for(int i = 0; i<npy; i++) { // Spline data points from experiment w.r.t. hip location. Change to make it w.r.t knee location kneeY[i] += (-kneeInFemur[1]+hipInFemur[1]); } SimmSpline tx(npx, angX, kneeX); SimmSpline ty(npy, angY, kneeY);; // Define the functions that specify the FunctionBased Mobilized Body. std::vector<std::vector<int> > coordIndices; std::vector<const SimTK::Function*> functions; std::vector<bool> isdof(6,false); // Set the 1 spatial rotation about Z-axis isdof[2] = true; //rot Z int nm = 0; for(int i=0; i<6; i++){ if(isdof[i]) { Vector coeff(2); coeff[0] = 1; coeff[1] = 0; std::vector<int> findex(1); findex[0] = nm++; functions.push_back(new SimTK::Function::Linear(coeff)); coordIndices.push_back(findex); } else if(i==3 || i ==4){ std::vector<int> findex(1,0); if(i==3) functions.push_back(tx.createSimTKFunction()); else functions.push_back(ty.createSimTKFunction()); coordIndices.push_back(findex); } else{ std::vector<int> findex(0); functions.push_back(new SimTK::Function::Constant(0, 0)); coordIndices.push_back(findex); } } // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); //system.updDefaultSubsystem().addEventReporter(new VTKEventReporter(system, 0.01)); // Thigh connected by hip MobilizedBody::Pin thigh(matter.Ground(), Transform(hipInGround), SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur)); //Function-based knee connects shank MobilizedBody::FunctionBased shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia), nm, functions, coordIndices); //MobilizedBody::Pin shank(thigh, SimTK::Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), SimTK::Transform(kneeInTibia)); // Simbody model state setup system.realizeTopology(); State state = system.getDefaultState(); matter.setUseEulerAngles(state, true); system.realizeModel(state); //========================================================================================================== // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies const Ground& ground = osimModel->getGround();; OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM); // create hip as a pin joint PinJoint hip("hip",ground, hipInGround, Vec3(0), osim_thigh, hipInFemur, Vec3(0)); // Rename hip coordinates for a pin joint hip.getCoordinateSet()[0].setName("hip_flex"); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_thigh); osimModel->addJoint(&hip); // Add another body via a knee joint OpenSim::Body osim_shank("shank", tibiaMass.getMass(), tibiaMass.getMassCenter(), tibiaMass.getInertia()); // Define knee coordinates and axes for custom joint spatial transform SpatialTransform kneeTransform; string knee_q_name = "knee_q"; string tx_name = "knee_tx"; string ty_name = "knee_ty"; Array<string> indepCoords(knee_q_name, 1, 1); // knee flexion/extension kneeTransform[2].setCoordinateNames(indepCoords); kneeTransform[2].setFunction(new LinearFunction()); // translation X kneeTransform[3].setCoordinateNames(OpenSim::Array<std::string>(tx_name, 1, 1)); kneeTransform[3].setFunction(new LinearFunction()); // translation Y kneeTransform[4].setCoordinateNames(OpenSim::Array<std::string>(ty_name, 1, 1)); kneeTransform[4].setFunction(new LinearFunction()); // create custom knee joint CustomJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), osim_shank, kneeInTibia, Vec3(0), kneeTransform); // Add the shank body which now also contains the knee joint to the model osimModel->addBody(&osim_shank); osimModel->addJoint(&knee); // Constrain the knee translations to follow the desired manifold CoordinateCouplerConstraint knee_tx_constraint; CoordinateCouplerConstraint knee_ty_constraint; knee_tx_constraint.setName("knee_tx_coupler"); knee_ty_constraint.setName("knee_ty_coupler"); knee_tx_constraint.setIndependentCoordinateNames(indepCoords); knee_ty_constraint.setIndependentCoordinateNames(indepCoords); knee_tx_constraint.setDependentCoordinateName(tx_name); knee_ty_constraint.setDependentCoordinateName(ty_name); knee_tx_constraint.setFunction(tx); knee_ty_constraint.setFunction(ty); // Add the constraints osimModel->addConstraint(&knee_tx_constraint); osimModel->addConstraint(&knee_ty_constraint); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); // OpenSim model must realize the topology to get valid osim_state osimModel->setGravity(gravity_vec); PointKinematics *pointKin = new PointKinematics(osimModel); // Get the point location of the shank origin in space pointKin->setBodyPoint("shank", Vec3(0)); osimModel->addAnalysis(pointKin); // Model cannot own model components created on the stack in this test program osimModel->disownAllComponents(); // write out the model to file osimModel->print("testCouplerConstraint.osim"); //wipe-out the model just constructed delete osimModel; // reconstruct from the model file osimModel = new Model("testCouplerConstraint.osim"); ForceReporter *forceReport = new ForceReporter(osimModel); forceReport->includeConstraintForces(true); osimModel->addAnalysis(forceReport); // Need to setup model before adding an analysis since it creates the AnalysisSet // for the model if it does not exist. State& osim_state = osimModel->initSystem(); //========================================================================================================== // Compare Simbody system and OpenSim model simulations compareSimulations(system, state, osimModel, osim_state, "testCoordinateCouplerConstraint FAILED\n"); // Forces were held in storage during simulation, now write to file forceReport->printResults("CouplerModelForces"); }
// 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; }