void testForceAtPoint() { using namespace SimTK; Mat33 invInertia = ballMass.getInertia().toMat33().invert(); vector<Real> times; vector<Vec3> accel; vector<Vec3> angularAccel; times.push_back(0.0); accel.push_back(gravity_vec+Vec3(1.0/ballMass.getMass(), 0, 0)); angularAccel.push_back(invInertia*(Vec3(1, -1, 0)%Vec3(1.0, 0, 0))); PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(), *forceY = new PiecewiseLinearFunction(), *forceZ = new PiecewiseLinearFunction(); forceX->addPoint(0, 1); forceY->addPoint(0, 0); forceZ->addPoint(0, 0); PiecewiseLinearFunction *pointX = new PiecewiseLinearFunction(), *pointY = new PiecewiseLinearFunction(), *pointZ = new PiecewiseLinearFunction(); pointX->addPoint(0, 1); pointY->addPoint(0, -1); pointZ->addPoint(0, 0); testPrescribedForce(forceX, forceY, forceZ, pointX, pointY, pointZ, NULL, NULL, NULL, times, accel, angularAccel); }
void testForceAtOrigin() { using namespace SimTK; vector<Real> times; vector<Vec3> accel; vector<Vec3> angularAccel; times.push_back(0.0); accel.push_back(gravity_vec+Vec3(1.0/ballMass.getMass(), 0, 0)); angularAccel.push_back(Vec3(0)); times.push_back(0.5); accel.push_back(gravity_vec+Vec3(0.5/ballMass.getMass(), 0.5/ballMass.getMass(), 0)); angularAccel.push_back(Vec3(0)); times.push_back(1.0); accel.push_back(gravity_vec+Vec3(0, 1/ballMass.getMass(), 0)); angularAccel.push_back(Vec3(0)); PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(), *forceY = new PiecewiseLinearFunction(), *forceZ = new PiecewiseLinearFunction(); forceX->addPoint(0, 1); forceX->addPoint(1, 0); forceY->addPoint(0, 0); forceY->addPoint(1, 1); forceZ->addPoint(0, 0); testPrescribedForce(forceX, forceY, forceZ, NULL, NULL, NULL, NULL, NULL, NULL, times, accel, angularAccel); }
void testTorque() { using namespace SimTK; Mat33 invInertia = ballMass.getInertia().toMat33().invert(); vector<Real> times; vector<Vec3> accel; vector<Vec3> angularAccel; times.push_back(0.0); accel.push_back(gravity_vec); angularAccel.push_back(invInertia*Vec3(1, 0.5, 0)); PiecewiseLinearFunction *torqueX = new PiecewiseLinearFunction(), *torqueY = new PiecewiseLinearFunction(), *torqueZ = new PiecewiseLinearFunction(); torqueX->addPoint(0, 1); torqueY->addPoint(0, 0.5); torqueZ->addPoint(0, 0); testPrescribedForce(NULL, NULL, NULL, NULL, NULL, NULL, torqueX, torqueY, torqueZ, times, accel, angularAccel); }
//========================================================================================================== // Test Cases //========================================================================================================== void testPrescribedForce(OpenSim::Function* forceX, OpenSim::Function* forceY, OpenSim::Function* forceZ, OpenSim::Function* pointX, OpenSim::Function* pointY, OpenSim::Function* pointZ, OpenSim::Function* torqueX, OpenSim::Function* torqueY, OpenSim::Function* torqueZ, vector<SimTK::Real>& times, vector<SimTK::Vec3>& accelerations, vector<SimTK::Vec3>& angularAccelerations) { using namespace SimTK; //========================================================================================================== // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies OpenSim::Body& ground = osimModel->getGroundBody(); OpenSim::Body ball; ball.setName("ball"); // Add joints FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0), false); // Rename coordinates for a free joint CoordinateSet free_coords = free.getCoordinateSet(); for(int i=0; i<free_coords.getSize(); i++){ std::stringstream coord_name; coord_name << "free_q" << i; free_coords.get(i).setName(coord_name.str()); free_coords.get(i).setMotionType(i > 2 ? Coordinate::Translational : Coordinate::Rotational); } osimModel->addBody(&ball); osimModel->addJoint(&free); // Add a PrescribedForce. PrescribedForce force(&ball); if (forceX != NULL) force.setForceFunctions(forceX, forceY, forceZ); if (pointX != NULL) force.setPointFunctions(pointX, pointY, pointZ); if (torqueX != NULL) force.setTorqueFunctions(torqueX, torqueY, torqueZ); counter++; osimModel->updForceSet().append(&force); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); //Set mass ball.setMass(ballMass.getMass()); ball.setMassCenter(ballMass.getMassCenter()); ball.setInertia(ballMass.getInertia()); osimModel->setGravity(gravity_vec); osimModel->print("TestPrescribedForceModel.osim"); delete osimModel; // Check that serialization/deserialization is working correctly as well osimModel = new Model("TestPrescribedForceModel.osim"); SimTK::State& osim_state = osimModel->initSystem(); osimModel->getMultibodySystem().realize(osim_state, Stage::Position ); //========================================================================================================== // Compute the force and torque at the specified times. const OpenSim::Body& body = osimModel->getBodySet().get("ball"); RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() ); Manager manager(*osimModel, integrator); manager.setInitialTime(0.0); for (unsigned int i = 0; i < times.size(); ++i) { manager.setFinalTime(times[i]); manager.integrate(osim_state); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); Vec3 accel, angularAccel; osimModel->updSimbodyEngine().getAcceleration(osim_state, body, Vec3(0), accel); osimModel->updSimbodyEngine().getAngularAcceleration(osim_state, body, angularAccel); ASSERT_EQUAL(accelerations[i][0], accel[0], 1e-10); ASSERT_EQUAL(accelerations[i][1], accel[1], 1e-10); ASSERT_EQUAL(accelerations[i][2], accel[2], 1e-10); ASSERT_EQUAL(angularAccelerations[i][0], angularAccel[0], 1e-10); ASSERT_EQUAL(angularAccelerations[i][1], angularAccel[1], 1e-10); ASSERT_EQUAL(angularAccelerations[i][2], angularAccel[2], 1e-10); } }
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"); }
void testPointOnLineConstraint() { using namespace SimTK; cout << endl; cout << "==================================================================" << endl; cout << "OpenSim PointOnLineConstraint vs. Simbody Constraint::PointOnLine " << endl; cout << "==================================================================" << endl; Random::Uniform randomDirection(-1, 1); Vec3 lineDirection(randomDirection.getValue(), randomDirection.getValue(), randomDirection.getValue()); UnitVec3 normLineDirection(lineDirection.normalize()); Vec3 pointOnLine(0,0,0); Vec3 pointOnFollower(0,0,0); // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); // Create a free joint between the foot and ground MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)), SimTK::Body::Rigid(footMass), Transform(Vec3(0))); // Constrain foot to line on ground SimTK::Constraint::PointOnLine simtkPointOnLine(matter.Ground(), normLineDirection, pointOnLine, foot, pointOnFollower); // 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 foot OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia()); // create foot as a free joint FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0), osim_foot, Vec3(0), Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_foot); osimModel->addJoint(&footJoint); // add a point on line constraint PointOnLineConstraint lineConstraint(ground, normLineDirection, pointOnLine, osim_foot, pointOnFollower); osimModel->addConstraint(&lineConstraint); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); osimModel->setGravity(gravity_vec); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); // 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, "testPointOnLineConstraint FAILED\n"); } // end testPointOnLineConstraint
void testWeldConstraint() { using namespace SimTK; cout << endl; cout << "==================================================================" << endl; cout << " OpenSim WeldConstraint vs. Simbody Constraint::Weld " << endl; cout << "==================================================================" << endl; Random::Uniform randomValue(-0.05, 0.1); Vec3 weldInGround(randomValue.getValue(), randomValue.getValue(), 0); Vec3 weldInFoot(0.1*randomValue.getValue(), 0.1*randomValue.getValue(), 0); // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); // Thigh connected by hip MobilizedBody::Pin thigh(matter.Ground(), SimTK::Transform(hipInGround), SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur)); // Pin knee connects shank MobilizedBody::Pin shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia)); // Pin ankle connects foot MobilizedBody::Pin foot(shank, Transform(ankleInTibia), SimTK::Body::Rigid(footMass), Transform(ankleInFoot)); SimTK::Constraint::Weld weld(matter.Ground(), Transform(weldInGround), foot, Transform(weldInFoot)); // 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 thigh OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM); // create Pin hip joint PinJoint hip("hip", ground, hipInGround, Vec3(0), osim_thigh, hipInFemur, Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_thigh); osimModel->addJoint(&hip); //OpenSim shank OpenSim::Body osim_shank("shank", tibiaMass.getMass(), tibiaMass.getMassCenter(), tibiaMass.getInertia()); // create Pin knee joint PinJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), osim_shank, kneeInTibia, Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_shank); osimModel->addJoint(&knee); //OpenSim foot OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia()); // create Pin ankle joint PinJoint ankle("ankle", osim_shank, ankleInTibia, Vec3(0), osim_foot, ankleInFoot, Vec3(0)); // Add the foot body which now also contains the hip joint to the model osimModel->addBody(&osim_foot); osimModel->addJoint(&ankle); // add a point on line constraint WeldConstraint footConstraint( "footConstraint", ground, SimTK::Transform(weldInGround), osim_foot, SimTK::Transform(weldInFoot) ); osimModel->addConstraint(&footConstraint); // BAD: but if model maintains ownership, it will attempt to delete stack allocated objects osimModel->disownAllComponents(); osimModel->setGravity(gravity_vec); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); osimModel->setup(); osimModel->print("testWeldConstraint.osim"); // 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, "testWeldConstraint FAILED\n"); }
void testCoordinateLocking() { using namespace SimTK; double fixedKneeAngle = Pi/2; // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies const Ground& ground = osimModel->getGround();; //OpenSim thigh 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 osimModel->addBody(&osim_thigh); osimModel->addJoint(&hip); // Add OpenSim shank via a knee joint OpenSim::Body osim_shank("shank", tibiaMass.getMass(), tibiaMass.getMassCenter(), tibiaMass.getInertia()); // create pin knee joint PinJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), osim_shank, Vec3(0), Vec3(0)); // Add the shank body and knee joint osimModel->addBody(&osim_shank); osimModel->addJoint(&knee); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); osimModel->setGravity(gravity_vec); // Initialize the state of the model based on the defaults of ModelComponents. State& si = osimModel->initSystem(); // Model joint states CoordinateSet &coordinates = osimModel->updCoordinateSet(); coordinates[0].setValue(si, 0); coordinates[0].setSpeedValue(si, 1.0); coordinates[1].setValue(si, fixedKneeAngle); coordinates[1].setLocked(si, true); // lock the knee osimModel->setPropertiesFromState(si); // Test that serialization and deserialization of locking works osimModel->print("testLockingModel.osim"); // Model and all its contents are wiped out delete osimModel; // Reload the model osimModel = new Model("testLockingModel.osim"); // Re-initialize the state of the model based on now saved defaults of the model. State& si2 = osimModel->initSystem(); osimModel->getMultibodySystem().realize(si2, Stage::Velocity ); // Create the integrator and manager for the simulation. RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem()); integrator.setMaximumStepSize(1.0e-3); integrator.setMinimumStepSize(1.0e-6); integrator.setAccuracy(integ_accuracy); Manager manager(*osimModel, integrator); // Print out the initial position and velocity states si2.getQ().dump("Initial q's"); // pendulum positions si2.getU().dump("Initial u's"); // pendulum velocities Vector qi = si2.getQ(); // Integrate from initial time to final time manager.setInitialTime(0.0); manager.setFinalTime(duration); cout<<"\n\nIntegrating from "<<manager.getInitialTime()<<" to "<<manager.getFinalTime()<<std::endl; manager.integrate(si2); // Print out the final position and velocity states Vector qf = si2.getQ(); qf.dump("Final q's"); // pendulum positions si2.getU().dump("Final u's"); // pendulum velocities stringstream errorMessage; errorMessage << "testCoordinateLocking FAILED\ntestCoordinateLocking: q_err = " << qf[1]-qi[1]; ASSERT(fabs(qf[1]-fixedKneeAngle) <= integ_accuracy, __FILE__, __LINE__, errorMessage.str()); }
void testConstantDistanceConstraint() { using namespace SimTK; cout << endl; cout << "==================================================================" << endl; cout << " OpenSim ConstantDistanceConstraint vs. Simbody Constraint::Rod " << endl; cout << "==================================================================" << endl; Random::Uniform randomLocation(-1, 1); Vec3 pointOnFoot(randomLocation.getValue(), randomLocation.getValue(), randomLocation.getValue()); Vec3 pointOnGround(0,0,0); /** for some reason, adding another Random::Uniform causes testWeldConstraint to fail. Why doesn't it cause this test to fail???? */ //Random::Uniform randomLength(0.01, 0.2); //randomLength.setSeed(1024); //double rodLength = randomLength.getValue(); double rodLength = 0.05; //std::cout << "Random Length = " << rodLength2 << ", used length = " << rodLength << std::endl; // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); // Create a free joint between the foot and ground MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)), SimTK::Body::Rigid(footMass), Transform(Vec3(0))); // Constrain foot to point on ground SimTK::Constraint::Rod simtkRod(matter.Ground(), pointOnGround, foot, pointOnFoot, rodLength); // 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 foot OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia()); // create foot as a free joint FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0), osim_foot, Vec3(0), Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_foot); osimModel->addJoint(&footJoint); // add a constant distance constraint ConstantDistanceConstraint rodConstraint(ground, pointOnGround, osim_foot, pointOnFoot,rodLength); osimModel->addConstraint(&rodConstraint); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); osimModel->setGravity(gravity_vec); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); // 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, "testConstantDistanceConstraint FAILED\n"); }