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 testSolveArbitrary() { static Random::Uniform uniform(2.0, 7.0); static Random::Gaussian random(0.0, 100.0); // Test random polynomials of various degrees with real coefficients. Vector realCoeff; Vector_<Complex> roots; for (int i = 0; i < 1000; ++i) { int length = uniform.getIntValue(); realCoeff.resize(length); roots.resize(length-1); for (int j = 0; j < length; ++j) realCoeff[j] = random.getValue(); PolynomialRootFinder::findRoots(realCoeff, roots); verifyRoots(realCoeff, roots); } // Test random polynomials of various degrees with complex coefficients. Vector_<Complex> complexCoeff; for (int i = 0; i < 1000; ++i) { int length = uniform.getIntValue(); complexCoeff.resize(length); roots.resize(length-1); for (int j = 0; j < length; ++j) complexCoeff[j] = Complex(random.getValue(), random.getValue()); PolynomialRootFinder::findRoots(complexCoeff, roots); verifyRoots(complexCoeff, roots); } // Verify that if the leading coefficient is zero, it throws an exception. try { realCoeff[0] = 0.0; PolynomialRootFinder::findRoots(realCoeff, roots); ASSERT(false) } catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) { } try { complexCoeff[0] = 0.0; PolynomialRootFinder::findRoots(complexCoeff, roots); ASSERT(false) } catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) { } }
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(); }