Esempio n. 1
0
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);
}
Esempio n. 2
0
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);
}
Esempio n. 4
0
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();
}