void testForceAtPoint()
{
	using namespace SimTK;

    Mat33 invInertia = ballMass.getInertia().toMat33().invert();
    vector<Real> times;
    vector<Vec3> accel;
    vector<Vec3> angularAccel;
    times.push_back(0.0);
    accel.push_back(gravity_vec+Vec3(1.0/ballMass.getMass(), 0, 0));
    angularAccel.push_back(invInertia*(Vec3(1, -1, 0)%Vec3(1.0, 0, 0)));
    PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(), *forceY = new PiecewiseLinearFunction(), *forceZ = new PiecewiseLinearFunction();
    forceX->addPoint(0, 1);
    forceY->addPoint(0, 0);
    forceZ->addPoint(0, 0);
    PiecewiseLinearFunction *pointX = new PiecewiseLinearFunction(), *pointY = new PiecewiseLinearFunction(), *pointZ = new PiecewiseLinearFunction();
    pointX->addPoint(0, 1);
    pointY->addPoint(0, -1);
    pointZ->addPoint(0, 0);
    testPrescribedForce(forceX, forceY, forceZ, pointX, pointY, pointZ, NULL, NULL, NULL, times, accel, angularAccel);
}
void testForceAtOrigin()
{
	using namespace SimTK;

    vector<Real> times;
    vector<Vec3> accel;
    vector<Vec3> angularAccel;
    times.push_back(0.0);
    accel.push_back(gravity_vec+Vec3(1.0/ballMass.getMass(), 0, 0));
    angularAccel.push_back(Vec3(0));
    times.push_back(0.5);
    accel.push_back(gravity_vec+Vec3(0.5/ballMass.getMass(), 0.5/ballMass.getMass(), 0));
    angularAccel.push_back(Vec3(0));
    times.push_back(1.0);
    accel.push_back(gravity_vec+Vec3(0, 1/ballMass.getMass(), 0));
    angularAccel.push_back(Vec3(0));
    PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(), *forceY = new PiecewiseLinearFunction(), *forceZ = new PiecewiseLinearFunction();
    forceX->addPoint(0, 1);
    forceX->addPoint(1, 0);
    forceY->addPoint(0, 0);
    forceY->addPoint(1, 1);
    forceZ->addPoint(0, 0);
    testPrescribedForce(forceX, forceY, forceZ, NULL, NULL, NULL, NULL, NULL, NULL, times, accel, angularAccel);
}
void testTorque()
{
	using namespace SimTK;

    Mat33 invInertia = ballMass.getInertia().toMat33().invert();
    vector<Real> times;
    vector<Vec3> accel;
    vector<Vec3> angularAccel;
    times.push_back(0.0);
    accel.push_back(gravity_vec);
    angularAccel.push_back(invInertia*Vec3(1, 0.5, 0));
    PiecewiseLinearFunction *torqueX = new PiecewiseLinearFunction(), *torqueY = new PiecewiseLinearFunction(), *torqueZ = new PiecewiseLinearFunction();
    torqueX->addPoint(0, 1);
    torqueY->addPoint(0, 0.5);
    torqueZ->addPoint(0, 0);
    testPrescribedForce(NULL, NULL, NULL, NULL, NULL, NULL, torqueX, torqueY, torqueZ, times, accel, angularAccel);
}
//==========================================================================================================
// 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);
    }
}
Esempio n. 5
0
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");
}
Esempio n. 6
0
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
Esempio n. 7
0
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");
}
Esempio n. 8
0
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());
}
Esempio n. 9
0
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");
}