void createState(MultibodySystem& system, State& state, const Vector& qOverride=Vector()) {
system.realizeTopology();
state = system.getDefaultState();
Random::Uniform random;
for (int i = 0; i < state.getNY(); ++i)
state.updY()[i] = random.getValue();
if (qOverride.size())
state.updQ() = qOverride;
system.realize(state, Stage::Velocity);
Vector dummy;
system.project(state, ConstraintTol);
system.realize(state, Stage::Acceleration);
}
void createState(MultibodySystem& system, State& state, const Vector& y=Vector()) {
system.realizeTopology();
state = system.getDefaultState();
if (y.size() > 0)
state.updY() = y;
else {
Random::Uniform random;
for (int i = 0; i < state.getNY(); ++i)
state.updY()[i] = random.getValue();
}
system.realize(state, Stage::Velocity);
// Solve to tight tolerance here
system.project(state, 1e-12);
system.realize(state, Stage::Acceleration);
}
void testExpressionBasedPointToPointForce()
{
using namespace SimTK;
double mass = 100;
double h0 = 0;
double start_h = 0.5;
double ball_radius = 0.25;
Random::Uniform rand;
Vec3 p1(rand.getValue(), rand.getValue(), rand.getValue());
Vec3 p2(rand.getValue(), rand.getValue(), rand.getValue());
// Setup OpenSim model
Model *model = new Model;
model->setName("ExpressionBasedPointToPointForce");
//OpenSim bodies
OpenSim::Body& ground = model->getGroundBody();
OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(ball_radius));
ball.addDisplayGeometry("sphere.vtp");
ball.scale(Vec3(ball_radius), false);
// define body's joint
FreeJoint free("free", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2));
model->addBody(&ball);
model->addJoint(&free);
string expression("2/(d^2)-3.0*(d-0.2)*(1+0.0123456789*ddot)");
ExpressionBasedPointToPointForce* p2pForce =
new ExpressionBasedPointToPointForce("ground", p1, "ball", p2, expression);
p2pForce->setName("P2PTestForce");
model->addForce(p2pForce);
// Create the force reporter
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
//model->setUseVisualizer(true);
SimTK::State& state = model->initSystem();
model->print("ExpressionBasedPointToPointForceModel.osim");
Vector& q = state.updQ();
Vector& u = state.updU();
for(int i=0; i < state.getNU(); ++i){
q[i] = rand.getValue();
u[i] = rand.getValue();
}
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem() );
integrator.setAccuracy(1e-6);
Manager manager(*model, integrator);
manager.setInitialTime(0.0);
double final_t = 1.0;
manager.setFinalTime(final_t);
manager.integrate(state);
//manager.getStateStorage().print("testExpressionBasedPointToPointForce.sto");
// force is only velocity dependent but is only compute in Dynamics
model->getMultibodySystem().realize(state, Stage::Dynamics);
// Now check that the force reported by spring
double model_force = p2pForce->getForceMagnitude(state);
// Save the forces
//reporter->getForceStorage().print("path_spring_forces.mot");
double d = model->getSimbodyEngine().calcDistance(state, ground, p1, ball, p2);
const MobilizedBody& b1 = model->getMatterSubsystem().getMobilizedBody(ground.getIndex());
const MobilizedBody& b2 = model->getMatterSubsystem().getMobilizedBody(ball.getIndex());
double ddot = b1.calcStationToStationDistanceTimeDerivative(state, p1, b2, p2);
//string expression("2/(d^2)-3.0*(d-0.2)*(1+0.0123456789*ddot)");
double analytical_force = 2/(d*d)-3.0*(d-0.2)*(1+0.0123456789*ddot);
// something is wrong if the block does not reach equilibrium
ASSERT_EQUAL(analytical_force, model_force, 1e-5);
// Before exiting lets see if copying the P2P force works
ExpressionBasedPointToPointForce *copyOfP2pForce = p2pForce->clone();
ASSERT(*copyOfP2pForce == *p2pForce);
model->disownAllComponents();
}