//==========================================================================================================
// 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");
}