//==========================================================================================================
// Test Cases
//==========================================================================================================
int testBouncingBall(bool useMesh)
{
// 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.set_mass(mass);
ball.set_mass_center(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.
ContactHalfSpace *floor = new ContactHalfSpace(Vec3(0), Vec3(0, 0, -0.5*SimTK_PI), ground, "ground");
osimModel->addContactGeometry(floor);
OpenSim::ContactGeometry* geometry;
if (useMesh)
geometry = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball");
else
geometry = new ContactSphere(radius, Vec3(0), ball, "ball");
osimModel->addContactGeometry(geometry);
OpenSim::Force* force;
if (useMesh)
{
// Add an ElasticFoundationForce.
OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/radius, 1e-5, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball");
contactParams->addGeometry("ground");
force = new OpenSim::ElasticFoundationForce(contactParams);
osimModel->addForce(force);
}
else
{
// Add a HuntCrossleyForce.
OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 1e-5, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball");
contactParams->addGeometry("ground");
force = new OpenSim::HuntCrossleyForce(contactParams);
osimModel->addForce(force);
}
osimModel->setGravity(gravity_vec);
osimModel->setName("TestContactGeomtery_Ball");
osimModel->clone()->print("TestContactGeomtery_Ball.osim");
Kinematics* kin = new Kinematics(osimModel);
osimModel->addAnalysis(kin);
SimTK::State& osim_state = osimModel->initSystem();
osim_state.updQ()[4] = height;
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
//Initial system energy is all potential
double Etot_orig = mass*(-gravity_vec[1])*height;
//==========================================================================================================
// Simulate it and see if it bounces correctly.
cout << "stateY=" << osim_state.getY() << std::endl;
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
Manager manager(*osimModel, integrator);
for (unsigned int i = 0; i < duration/interval; ++i)
{
manager.setInitialTime(i*interval);
manager.setFinalTime((i+1)*interval);
manager.integrate(osim_state);
double time = osim_state.getTime();
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
Vec3 pos, vel;
osimModel->updSimbodyEngine().getPosition(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), pos);
osimModel->updSimbodyEngine().getVelocity(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), vel);
double Etot = mass*((-gravity_vec[1])*pos[1] + 0.5*vel[1]*vel[1]);
//cout << "starting system energy = " << Etot_orig << " versus current energy = " << Etot << endl;
// contact absorbs and returns energy so make sure not in contact
if (pos[1] > 2*radius)
{
ASSERT_EQUAL(Etot_orig, Etot, 1e-2, __FILE__, __LINE__, "Bouncing ball on plane Failed: energy was not conserved.");
}
else
{
cout << "In contact at time = " << time << endl;
ASSERT(pos[1] < 5.0 && pos[1] > 0);
}
ASSERT_EQUAL(0.0, pos[0], 1e-4);
ASSERT_EQUAL(0.0, pos[2], 1e-4);
ASSERT_EQUAL(0.0, vel[0], 1e-3);
ASSERT_EQUAL(0.0, vel[2], 1e-3);
}
std::string prefix = useMesh?"Kinematics_Mesh":"Kinematics_NoMesh";
kin->printResults(prefix);
osimModel->disownAllComponents();
// model takes ownership of components unless container set is told otherwise
delete osimModel;
return 0;
}
//==========================================================================================================
// 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);
}
}
// 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;
}