Example #1
0
//------------------------------------------------------------------------------
//                                 ASSEMBLER
//------------------------------------------------------------------------------
Assembler::Assembler(const MultibodySystem& system)
:   system(system), accuracy(0), tolerance(0), // i.e., 1e-3, 1e-4
    forceNumericalGradient(false), forceNumericalJacobian(false),
    useRMSErrorNorm(false), alreadyInitialized(false),
    asmSys(0), optimizer(0), nAssemblySteps(0), nInitializations(0)
{
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
    matter.convertToEulerAngles(system.getDefaultState(),
                                internalState);
    system.realizeModel(internalState);

    // Make sure the System's Constraints are always present; this sets the
    // weight to Infinity which makes us treat this as an assembly error
    // rather than merely a goal; that can be changed by the user.
    systemConstraints = adoptAssemblyError(new BuiltInConstraints());
}
void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy, 
                   const Array_<MobilizedBodyIndex>& originalBodyIxs, 
                   Array_<MobilizedBodyIndex>& copyBodyIxs,
                   bool& hasArtificialBaseBody) 
{
    const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
    SimbodyMatterSubsystem copyMatter(copy);
    Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
    body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
    std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
    hasArtificialBaseBody = false;
    for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
        const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
        MobilizedBody* copyBody;
        if (i == 0) {
            if (originalBody.isGround())
                copyBody = &copyMatter.Ground();
            else {
                hasArtificialBaseBody = true; // not using the original joint here
                MobilizedBody::Free free(copyMatter.Ground(), body);
                copyBody = &copyMatter.updMobilizedBody(free.getMobilizedBodyIndex());
            }
        }
        else {
            MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
            copyBody = &originalBody.cloneForNewParent(parent);
        }
        copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
        idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
    }
    copy.realizeTopology();
    State& s = copy.updDefaultState();
    copyMatter.setUseEulerAngles(s, true);
    copy.realizeModel(s);
}
Example #3
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");
}
Example #4
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
Example #5
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");
}
Example #6
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");
}
Example #7
0
int main(int argc, char** argv) {
  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem         mbs; mbs.setUseUniformBackground(true);

    SimbodyMatterSubsystem  crankRocker(mbs);
    GeneralForceSubsystem   forces(mbs);
    DecorationSubsystem     viz(mbs);
    Force::UniformGravity   gravity(forces, crankRocker, Vec3(0, -g, 0));

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid crankBody  = Body::Rigid(MassProperties(.1, Vec3(0), 0.1*UnitInertia::brick(1,3,.5)));
    crankBody.addDecoration(DecorativeEllipsoid(0.1*Vec3(1,3,.4))
                                .setResolution(10)
                                .setOpacity(.2));
    Body::Rigid sliderBody = Body::Rigid(MassProperties(.2, Vec3(0), 0.2*UnitInertia::brick(1,5,.5)));
    sliderBody.addDecoration(DecorativeEllipsoid(0.2*Vec3(1,5,.4))
                                .setColor(Blue)
                                .setResolution(10)
                                .setOpacity(.2));
    Body::Rigid longBar = Body::Rigid(MassProperties(0.01, Vec3(0), 0.01*UnitInertia::cylinderAlongX(.1, 5)));
    longBar.addDecoration(Rotation(Pi/2,ZAxis), DecorativeCylinder(.01, 1));

    MobilizedBody::Pin
        crank(crankRocker.Ground(), Transform(), crankBody, Transform(Vec3(0, .15, 0)));
    MobilizedBody::Pin
        slider(crankRocker.Ground(), Transform(Vec3(2.5,0,0)), sliderBody, Transform(Vec3(0, 1, 0)));

    MobilizedBody::Universal
        rightConn(crank, Transform(Rotation(-Pi/2,YAxis),Vec3(0,-.3,0)),
                  longBar, Transform(Rotation(-Pi/2,YAxis),Vec3(-1,0,0)));

    Constraint::Ball ball(slider, Vec3(0,-1, 0), rightConn, Vec3(1,0,0));
    ball.setDefaultRadius(0.01);

    //Constraint::Rod rodl(leftConn, Vec3(0), rightConn, Vec3(-2.5,0,0), 2.5);
    //Constraint::Rod rodr(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-1.5,0,0), std::sqrt(2.));
    //Constraint::Rod rodo(leftConn, Vec3(1.5,0,0), rightConn, Vec3(-2.5,1,0), std::sqrt(2.));
    //Constraint::Rod rodl(leftConn, Vec3(-2.5,0,0), rightConn, Vec3(-2.5,0,0), 5);
    //Constraint::Rod rodr(leftConn, Vec3(2.5,0,0), rightConn, Vec3(2.5,0,0), 5);
    //Constraint::Rod rodo(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-2.5,1,0), 2);

    // Add visualization line (orange=spring, black=constraint)
    //viz.addRubberBandLine(crank, Vec3(0,-3,0),
     //                     slider, Vec3(0,-5,0),
       //                   DecorativeLine().setColor(Black).setLineThickness(4));

    //forces.addMobilityConstantForce(leftPendulum, 0, 20);
    //forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
    //forces.addGlobalEnergyDrain(1);

    Force::MobilityConstantForce(forces, crank, 0, 1);
    Force::MobilityLinearDamper(forces, crank, 0, 1.0);


    //Motion::Linear(crank, Vec3(a,b,c)); // crank(t)=at^2+bt+c
    //Motion::Linear lmot(rightConn, Vec3(a,b,c)); // both axes follow
    //lmot.setAxis(1, Vec3(d,e,f));
    //Motion::Orientation(someBall, orientFuncOfT);
    //someBall.prescribeOrientation(orientFunc);
    //Motion::Relax(crank); // acc=vel=0, pos determined by some default relaxation solver

    // Like this, or should this be an Instance-stage mode of every mobilizer?
    //Motion::Lock(crank); // acc=vel=0; pos is discrete or fast
    //Motion::Steady(crank, Vec3(1,2,3)); // acc=0; vel=constant; pos integrated
    //Motion::Steady crankRate(crank, 1); // acc=0; vel=constant, same for each axis; pos integrated
    // or ...
    //crank.lock(state);
    //crank.setMotionType(state, Regular|Discrete|Fast|Prescribed, stage);

    // need a way to register a mobilizer with a particular relaxation solver,
    // switch between dynamic, continuous relaxed, end-of-step relaxed, discrete.
    // what about a "local" (explicit) relaxation, like q=(q1+q2)/2 ?


    State s = mbs.realizeTopology(); // returns a reference to the the default state
    mbs.realizeModel(s); // define appropriate states for this System

    //crankRate.setRate(s, 3);
    crank.setAngle(s, 5); //q
    crank.setRate(s, 3);  //u

    Visualizer display(mbs);

    mbs.realize(s, Stage::Position);
    display.report(s);
    cout << "q=" << s.getQ() << endl;
    cout << "qErr=" << s.getQErr() << endl;

    crank.setAngle(s, -30*Deg2Rad);
    //crank.setQToFitRotation (s, Rotation::aboutZ(-.9*Pi/2));


    //TODO
    //rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));

    //crank.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));

    mbs.realize(s, Stage::Velocity);
    display.report(s);

    cout << "q=" << s.getQ() << endl;
    cout << "qErr=" << s.getQErr() << endl;


    // These are the SimTK Simmath integrators:
    RungeKuttaMersonIntegrator myStudy(mbs);
    //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::Newton);


    //myStudy.setMaximumStepSize(0.001);
    myStudy.setAccuracy(1e-3);
    //myStudy.setProjectEveryStep(true);
    //myStudy.setAllowInterpolation(false);
    //myStudy.setMaximumStepSize(.1);

    const Real dt = 1./30; // output intervals
    const Real finalTime = 10;

    myStudy.setFinalTime(finalTime);

    cout << "Hit ENTER in console to continue ...\n";
    getchar();
    display.report(s);

    cout << "Hit ENTER in console to continue ...\n";
    getchar();

    // Peforms assembly if constraints are violated.
    myStudy.initialize(s);
    myStudy.setProjectEveryStep(false);
    myStudy.setConstraintTolerance(.001);
    myStudy.initialize(myStudy.getState());

    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    {
        const State& s = myStudy.getState();
        display.report(s);
        cout << "q=" << s.getQ() << endl;
        cout << "qErr=" << s.getQErr() << endl;
    }


    Integrator::SuccessfulStepStatus status;
    int nextReport = 0;
    while ((status=myStudy.stepTo(nextReport*dt, Infinity))
           != Integrator::EndOfSimulation)
    {
        const State& s = myStudy.getState();
        mbs.realize(s);
        const Real crankAngle = crank.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg;
        printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
            crankAngle,
            mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
            myStudy.getPreviousStepSizeTaken(),
            Integrator::getSuccessfulStepStatusString(status).c_str(),
            myStudy.isStateInterpolated()?" (INTERP)":"");
        printf("     qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
            crankRocker.getQErr(s).normRMS(),
            crankRocker.getUErr(s).normRMS(),
            crankRocker.getUDotErr(s).normRMS());

        display.report(s);


        if (status == Integrator::ReachedReportTime)
            ++nextReport;
    }

    for (int i=0; i<100; ++i)
        display.report(myStudy.getState());

    printf("Using Integrator %s:\n", myStudy.getMethodName());
    printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());

  }
  catch (const exception& e) {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
  }
  catch (...) {
    printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

}
Example #8
0
void main_simulation()
#endif
{
    // inputs
    double fitness;

    #ifdef OPTI
    double *optiParams;
    #endif

    Loop_inputs *loop_inputs;

    // initialization
	loop_inputs = init_simulation();

    // optimization init
    #ifdef OPTI

    optiParams = (double*) malloc(NB_PARAMS_TO_OPTIMIZE*sizeof(double));

    get_real_params_to_opt(optiNorms, optiParams);

    erase_for_opti(optiParams, loop_inputs->MBSdata);

    free(optiParams);

    #endif
    
    // -- Simbody -- //

    #ifdef SIMBODY

    // / Create the system. Define all "system" objects - system, matterm forces, tracker, contactForces.
    MultibodySystem           system;
    SimbodyMatterSubsystem    matter(system);
    ContactTrackerSubsystem   tracker(system); 
    CompliantContactSubsystem contactForces(system, tracker);
    system.setUpDirection(+ZAxis); // that is for visualization only. The default direction is +X


    SimbodyVariables simbodyVariables;

    // set all the mechanical parameters of the contact
    
    simbodyVariables.p_system        = &system;
    simbodyVariables.p_matter        = &matter;
    simbodyVariables.p_tracker       = &tracker;
    simbodyVariables.p_contactForces = &contactForces;

    //  cout<<"BoxInd in Main = "<<BoxInd<<" -- should be default \n";
    
    //init_Simbody(&simbodyVariables);

    init_Simbody(&simbodyVariables, loop_inputs->MBSdata->user_IO->simbodyBodies);
    
    //it is "system" commands. We cannot avoid them.    
    system.realizeTopology();
    State state = system.getDefaultState();

    simbodyVariables.p_state = &state;

    //it is "system" command. We cannot avoid them. 
    system.realizeModel(state);

    p_simbodyVariables = &simbodyVariables;

    #endif

	// loop
    fitness = loop_simulation(loop_inputs);

    // end of the simulation
    finish_simulation(loop_inputs);

    #ifdef OPTI

    return fitness;

    #else

    #if defined(PRINT_REPORT)
    printf("fitness: %f\n", fitness);
    #endif

    #endif
}
Example #9
0
int main() {
  try
  { // Create the system.

    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity   gravity(forces, matter, UnitVec3(-1,0,0), 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-2); // m/s

    // Ground's normal is +x for this model
    system.setUpDirection(+XAxis);

    // Uncomment this if you want a more elegant movie.
    //matter.setShowDefaultGeometry(false);

    const Real ud = .3; // dynamic
    const Real us = .6; // static
    const Real uv = 0;  // viscous (force/velocity)
    const Real k = 1e8; // pascals
    const Real c = 0.01; // dissipation (1/v)


    // Halfspace default is +x, this one occupies -x instead, so flip.
    const Rotation R_xdown(Pi,ZAxis);

    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(),
                       ContactMaterial(k,c,us,ud,uv)));


    const Real ellipsoidMass = 1; // kg
    const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
    const Vec3 comLoc(-1*Cm2m, 0, 0);
    const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
    const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
    Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));

    ellipsoidBody.addDecoration(Transform(),
        DecorativeEllipsoid(halfDims).setColor(Cyan)
         //.setOpacity(.5)
         .setResolution(3));
    ellipsoidBody.addContactSurface(Transform(),
        ContactSurface(ContactGeometry::Ellipsoid(halfDims),
                       ContactMaterial(k,c,us,ud,uv))
                       );
    MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
        ellipsoidBody, Transform(Vec3(0)));


    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    viz.setMode(Visualizer::RealTime);
    viz.setDesiredFrameRate(FrameRate);
    viz.setCameraClippingPlanes(0.1, 10);

    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Go", GoItem));
    runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    viz.addMenu("Run", RunMenuId, runMenuItems);

    Array_<std::pair<String,int> > helpMenuItems;
    helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
    viz.addMenu("Help", HelpMenuId, helpMenuItems);

    system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
    system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));

    // Check for a Run->Quit menu pick every 1/4 second.
    system.addEventHandler(new UserInputHandler(*silo, .25));

    // Initialize the system and state.

    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    ellipsoid.setQToFitTransform(state, Transform(
        Rotation(BodyRotationSequence,  0  *Deg2Rad, XAxis,
                                        0.5*Deg2Rad, YAxis,
                                       -0.5*Deg2Rad, ZAxis),
        Vec3(2.1*Cm2m, 0, 0)));

    ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s

    viz.report(state);
    printf("Default state\n");

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
         if (menuId != RunMenuId || item != GoItem)
             cout << "\aDude ... follow instructions!\n";
    } while (menuId != RunMenuId || item != GoItem);



    // Simulate it.

    //ExplicitEulerIntegrator integ(system);
    //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
    //RungeKuttaFeldbergIntegrator integ(system);
    RungeKuttaMersonIntegrator integ(system);
    //RungeKutta3Integrator integ(system);
    //VerletIntegrator integ(system);
    //integ.setMaximumStepSize(1e-0001);
    integ.setAccuracy(1e-4); // minimum for CPodes
    //integ.setAccuracy(.01);
    TimeStepper ts(system, integ);


    ts.initialize(state);
    double cpuStart = cpuTime();
    double realStart = realTime();

    ts.stepTo(10.0);

    const double timeInSec = realTime() - realStart;
    const int evals = integ.getNumRealizations();
    cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
        timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
        << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
        << (1000*ts.getTime())/evals << "ms/eval\n";
    cout << "  CPU time was " << cpuTime() - cpuStart << "s\n";

    printf("Using Integrator %s at accuracy %g:\n",
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());

    viz.dumpStats(std::cout);

    // Add as slider to control playback speed.
    viz.addSlider("Speed", 1, 0, 4, 1);
    viz.setMode(Visualizer::PassThrough);

    silo->clear(); // forget earlier input
    double speed = 1; // will change if slider moves
    while(true) {
        cout << "Choose Run/Replay to see that again ...\n";

        int menuId, item;
        silo->waitForMenuPick(menuId, item);


        if (menuId != RunMenuId) {
            cout << "\aUse the Run menu!\n";
            continue;
        }

        if (item == QuitItem)
            break;
        if (item != ReplayItem) {
            cout << "\aHuh? Try again.\n";
            continue;
        }

        for (double i=0; i < (int)saveEm.size(); i += speed ) {
            int slider; Real newValue;
            if (silo->takeSliderMove(slider,newValue)) {
                speed = newValue;
            }
            viz.report(saveEm[(int)i]);
        }
    }

  } catch (const std::exception& e) {
    std::printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);

  } catch (...) {
    std::printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

    return 0;
}
Example #10
0
void testConstrainedSystem() {
    MultibodySystem mbs;
    MyForceImpl* frcp;
    makeSystem(true, mbs, frcp);
    const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();

    State state = mbs.realizeTopology();
    mbs.realize(state, Stage::Instance); // allocate multipliers, etc.

    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int m  = state.getNMultipliers();
    const int nb = matter.getNumBodies();

    // Attainable accuracy drops with problem size.
    const Real Slop = nu*SignificantReal;

    mbs.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);

    Vector randMobFrc = 100*Test::randVector(nu);
    Vector_<SpatialVec> randBodyFrc(nb);
    for (int i=0; i < nb; ++i)
        randBodyFrc[i] = Test::randSpatialVec();

    // Apply random mobility forces
    frcp->setMobilityForces(randMobFrc);

    mbs.realize(state); // calculate accelerations and multipliers
    Vector udot = state.getUDot();
    Vector lambda = state.getMultipliers();
    Vector residual;
    matter.calcResidualForce(state,randMobFrc,Vector_<SpatialVec>(),
                             udot, lambda, residual);

    // Residual should be zero since we accounted for everything.
    SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);

    Vector abias, mgbias;
    // These are the acceleration error bias terms.
    matter.calcBiasForAccelerationConstraints(state, abias);
    // These use pverr (velocity-level errors) for holonomic constraints.
    matter.calcBiasForMultiplyByG(state, mgbias);

    Vector mgGudot; matter.multiplyByG(state, udot, mgbias, mgGudot);
    Matrix G; matter.calcG(state, G);
    Vector Gudot = G*udot;
    SimTK_TEST_EQ_TOL(mgGudot, Gudot, Slop);
    Vector aerr = state.getUDotErr(); // won't be zero because bad constraints
    Vector GudotPlusBias = Gudot + abias;
    SimTK_TEST_EQ_TOL(GudotPlusBias, aerr, Slop);

    // Add in some body forces
    state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
    frcp->setBodyForces(randBodyFrc);
    mbs.realize(state);
    udot = state.getUDot();
    lambda = state.getMultipliers();
    matter.calcResidualForce(state,randMobFrc,randBodyFrc,
                             udot, lambda, residual);
    SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);

    // Try body forces only.
    state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
    frcp->setMobilityForces(0*randMobFrc);
    mbs.realize(state);
    udot = state.getUDot();
    lambda = state.getMultipliers();
    matter.calcResidualForce(state,Vector(),randBodyFrc,
                             udot, lambda, residual);
    SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);

    // Put vectors in noncontiguous storage.
    Matrix udotmat(3,nu); // rows are noncontig
    Matrix mobFrcMat(11,nu);
    Matrix lambdamat(5,m);
    Matrix_<SpatialRow> bodyFrcMat(3,nb);
    udotmat[2]    = ~udot;
    lambdamat[3]  = ~lambda;
    mobFrcMat[8] = ~randMobFrc;
    bodyFrcMat[2] = ~randBodyFrc;
    Matrix residmat(4,nu);

    // We last computed udot,lambda with no mobility forces. This time
    // will throw some in and then make sure the residual tries to cancel them.
    matter.calcResidualForce(state,~mobFrcMat[8],~bodyFrcMat[2],
        ~udotmat[2],~lambdamat[3],~residmat[2]);
    SimTK_TEST_EQ_TOL(residmat[2], -1*mobFrcMat[8], Slop);
}
Example #11
0
void testUnconstrainedSystem() {
    MultibodySystem system;
    MyForceImpl* frcp;
    makeSystem(false, system, frcp);
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

    State state = system.realizeTopology();
    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int nb = matter.getNumBodies();

    // Attainable accuracy drops with problem size.
    const Real Slop = nu*SignificantReal;

    system.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);

    Vector randVec = 100*Test::randVector(nu);
    Vector result1, result2;

    // result1 = M*v
    system.realize(state, Stage::Position);
    matter.multiplyByM(state, randVec, result1);
    SimTK_TEST_EQ(result1.size(), nu);

    // result2 = M^-1 * result1 == M^-1 * M * v == v
    system.realize(state, Stage::Dynamics);
    matter.multiplyByMInv(state, result1, result2);
    SimTK_TEST_EQ(result2.size(), nu);

    SimTK_TEST_EQ_TOL(result2, randVec, Slop);

    Matrix M(nu,nu), MInv(nu,nu);

    Vector v(nu, Real(0));
    for (int j=0; j < nu; ++j) {
        v[j] = 1;
        matter.multiplyByM(state, v, M(j));
        matter.multiplyByMInv(state, v, MInv(j));
        v[j] = 0;
    }

    Matrix MInvCalc(M);
    MInvCalc.invertInPlace();
    SimTK_TEST_EQ_SIZE(MInv, MInvCalc, nu);

    Matrix identity(nu,nu); identity=1;
    SimTK_TEST_EQ_SIZE(M*MInv, identity, nu);
    SimTK_TEST_EQ_SIZE(MInv*M, identity, nu);

    // Compare above-calculated values with values returned by the
    // calcM() and calcMInv() methods.
    Matrix MM, MMInv;
    matter.calcM(state,MM); matter.calcMInv(state,MMInv);
    SimTK_TEST_EQ_SIZE(MM, M, nu);
    SimTK_TEST_EQ_SIZE(MMInv, MInv, nu);

    //assertIsIdentity(eye);
    //assertIsIdentity(MInv*M);

    frcp->setMobilityForces(randVec);
    //cout << "f=" << randVec << endl;
    system.realize(state, Stage::Acceleration);
    Vector accel = state.getUDot();
    //cout << "v!=0, accel=" << accel << endl;

    matter.multiplyByMInv(state, randVec, result1);
    //cout << "With velocities, |a - M^-1*f|=" << (accel-result1).norm() << endl;

    SimTK_TEST_NOTEQ(accel, result1); // because of the velocities
    //SimTK_TEST((accel-result1).norm() > SignificantReal); // because of velocities

    // With no velocities M^-1*f should match calculated acceleration.
    state.updU() = 0;
    system.realize(state, Stage::Acceleration);
    accel = state.getUDot();
    //cout << "v=0, accel=" << accel << endl;

    //cout << "With v=0, |a - M^-1*f|=" << (accel-result1).norm() << endl;

    SimTK_TEST_EQ(accel, result1); // because no velocities

    // And then M*a should = f.
    matter.multiplyByM(state, accel, result2);
    //cout << "v=0, M*accel=" << result2 << endl;
    //cout << "v=0, |M*accel-f|=" << (result2-randVec).norm() << endl;


    // Test forward and inverse dynamics operators.
    // Apply random forces and a random prescribed acceleration to
    // get back the residual generalized forces. Then applying those
    // should result in zero residual, and applying them. 

    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);


    // Inverse dynamics should require realization only to Velocity stage.
    system.realize(state, Stage::Velocity);

    // Randomize body forces.
    Vector_<SpatialVec> bodyForces(nb);
    for (int i=0; i < nb; ++i)
        bodyForces[i] = Test::randSpatialVec();

    // Random mobility forces and known udots.
    Vector mobilityForces = Test::randVector(nu);
    Vector knownUdots = Test::randVector(nu);

    // Check self consistency: compute residual, apply it, should be no remaining residual.
    Vector residualForces, shouldBeZeroResidualForces;
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces, bodyForces, knownUdots, residualForces);
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces+residualForces, bodyForces, knownUdots, shouldBeZeroResidualForces);

    SimTK_TEST(shouldBeZeroResidualForces.norm() <= Slop);

    // Now apply these forces in forward dynamics and see if we get the desired
    // acceleration. State must be realized to Dynamics stage.
    system.realize(state, Stage::Dynamics);
    Vector udots;
    Vector_<SpatialVec> bodyAccels;
    matter.calcAccelerationIgnoringConstraints(state, 
        mobilityForces+residualForces, bodyForces, udots, bodyAccels);

    SimTK_TEST_EQ_TOL(udots, knownUdots, Slop);

    // See if we get back the same body accelerations by feeding in 
    // these udots.
    Vector_<SpatialVec> A_GB, AC_GB;
    matter.calcBodyAccelerationFromUDot(state, udots, A_GB);
    SimTK_TEST_EQ_TOL(A_GB, bodyAccels, Slop);

    // Collect coriolis accelerations.
    AC_GB.resize(matter.getNumBodies());
    for (MobodIndex i(0); i<nb; ++i)
        AC_GB[i] = matter.getTotalCoriolisAcceleration(state, i);

    // Verify that either a zero-length or all-zero udot gives just
    // coriolis accelerations.
    matter.calcBodyAccelerationFromUDot(state, Vector(), A_GB);
    SimTK_TEST_EQ_TOL(A_GB, AC_GB, Slop);

    Vector allZeroUdot(matter.getNumMobilities(), Real(0));
    matter.calcBodyAccelerationFromUDot(state, allZeroUdot, A_GB);
    SimTK_TEST_EQ_TOL(A_GB, AC_GB, Slop);

    // Now let's test noncontiguous input and output vectors.
    Matrix MatUdot(3, nu); // use middle row
    MatUdot.setToNaN();
    MatUdot[1] = ~udots;
    Matrix_<SpatialRow> MatA_GB(3, nb); // use middle row
    MatA_GB.setToNaN();
    matter.calcBodyAccelerationFromUDot(state, ~MatUdot[1], ~MatA_GB[1]);
    SimTK_TEST_EQ_TOL(MatA_GB[1], ~bodyAccels, Slop);

    // Verify that leaving out arguments makes them act like zeroes.
    Vector residualForces1, residualForces2;
    matter.calcResidualForceIgnoringConstraints(state,
        0*mobilityForces, 0*bodyForces, 0*knownUdots, residualForces1);
    // no, the residual is not zero here because of the angular velocities
    matter.calcResidualForceIgnoringConstraints(state,
        Vector(), Vector_<SpatialVec>(), Vector(), residualForces2);

    SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);

    // We just calculated f_residual = M udot + f_inertial - f_applied, with
    // both udot and f_applied zero, i.e. f_residual=f_inertial. That should
    // be the same as what is returned by getTotalCentrifugalForces().
    Vector_<SpatialVec> F_inertial(nb);
    Vector f_inertial;
    for (MobodIndex i(0); i<nb; ++i)
        F_inertial[i] = matter.getTotalCentrifugalForces(state, i);
    matter.multiplyBySystemJacobianTranspose(state, F_inertial, f_inertial);
    SimTK_TEST_EQ_TOL(f_inertial, residualForces1, Slop);

    // This should also match total Mass*Coriolis acceleration + gyro force.
    Vector_<SpatialVec> F_coriolis(nb), F_gyro(nb), F_total(nb);
    Vector f_total;
    for (MobodIndex i(0); i<nb; ++i) {
        if (i==0) F_coriolis[i] = SpatialVec(Vec3(0),Vec3(0));
        else
            F_coriolis[i] = matter.getMobilizedBody(i)
                           .getBodySpatialInertiaInGround(state) * AC_GB[i];
        F_gyro[i] = matter.getGyroscopicForce(state, i);
    }

    F_total = F_coriolis + F_gyro;
    SimTK_TEST_EQ_TOL(F_inertial, F_total, Slop);

    // Same, but leave out combinations of arguments.
    matter.calcResidualForceIgnoringConstraints(state,
        0*mobilityForces, bodyForces, knownUdots, residualForces1);
    matter.calcResidualForceIgnoringConstraints(state,
        Vector(), bodyForces, knownUdots, residualForces2);
    SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces, 0*bodyForces, knownUdots, residualForces1);
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces, Vector_<SpatialVec>(), knownUdots, residualForces2);
    SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces, bodyForces, 0*knownUdots, residualForces1);
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces, bodyForces, Vector(), residualForces2);
    SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
    matter.calcResidualForceIgnoringConstraints(state,
        0*mobilityForces, bodyForces, 0*knownUdots, residualForces1);
    matter.calcResidualForceIgnoringConstraints(state,
        Vector(), bodyForces, Vector(), residualForces2);
    SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces, 0*bodyForces, 0*knownUdots, residualForces1);
    matter.calcResidualForceIgnoringConstraints(state,
        mobilityForces, Vector_<SpatialVec>(), Vector(), residualForces2);
    SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);

    // Check that we object to wrong-length arguments.
    SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
        Vector(3,Zero), bodyForces, knownUdots, residualForces2));
    SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
         mobilityForces, Vector_<SpatialVec>(5), knownUdots, residualForces2));
    SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
         mobilityForces, bodyForces, Vector(2), residualForces2));

}
Example #12
0
// Test calculations of Jacobian "bias" terms, where bias=JDot*u.
// We can estimate JDot using a numerical directional derivative
// since JDot = (DJ/Dq)*qdot ~= (J(q+h*qdot)-J(q-h*qdot))/2h.
// Then we multiply JDot*u and compare with the bias calculations.
// Or, we can estimate JDot*u directly with
//       JDotu ~= (J(q+h*qdot)*u - J(q-h*qdot)*u)/2h
// using the fast "multiply by Jacobian" methods.
// We use both methods below.
void testJacobianBiasTerms() {
    MultibodySystem system;
    MyForceImpl* frcp;
    makeSystem(false, system, frcp);
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

    State state = system.realizeTopology();
    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int nb = matter.getNumBodies();

    system.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);


    const MobilizedBodyIndex whichBod(8);
    const Vec3 whichPt(1,2,3);
    system.realize(state, Stage::Velocity);
    const Vector& q = state.getQ();
    const Vector& u = state.getU();
    const Vector& qdot = state.getQDot();

    // sbias, fbias, sysbias are the JDot*u quantities we want to check.
    const Vec3 sbias =
        matter.calcBiasForStationJacobian(state, whichBod, whichPt);
    const SpatialVec fbias = 
        matter.calcBiasForFrameJacobian(state, whichBod, whichPt);
    Vector_<SpatialVec> sysbias;
    matter.calcBiasForSystemJacobian(state, sysbias);

    // These are for computing JDot first.
    RowVector_<Vec3> JS_P, JS1_P, JS2_P, JSDot_P;
    RowVector_<SpatialVec> JF_P, JF1_P, JF2_P, JFDot_P;
    Matrix_<SpatialVec> J, J1, J2, JDot;

    // These are for computing JDot*u directly.
    Vec3 JS_Pu, JS1_Pu, JS2_Pu, JSDot_Pu;
    SpatialVec JF_Pu, JF1_Pu, JF2_Pu, JFDot_Pu;
    Vector_<SpatialVec> Ju, J1u, J2u, JDotu;

    // Unperturbed:
    matter.calcStationJacobian(state,whichBod,whichPt, JS_P);
    matter.calcFrameJacobian(state,whichBod,whichPt, JF_P);
    matter.calcSystemJacobian(state, J);

    JS_Pu = matter.multiplyByStationJacobian(state,whichBod,whichPt,u);
    JF_Pu = matter.multiplyByFrameJacobian(state,whichBod,whichPt,u);
    matter.multiplyBySystemJacobian(state, u, Ju);

    const Real Delta = 5e-6; // we'll use central difference
    State perturbq = state;
    // Perturbed +:
    perturbq.updQ() = q + Delta*qdot;
    system.realize(perturbq, Stage::Position);
    matter.calcStationJacobian(perturbq,whichBod,whichPt, JS2_P);
    matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF2_P);
    matter.calcSystemJacobian(perturbq, J2);
    JS2_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
    JF2_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
    matter.multiplyBySystemJacobian(perturbq,u, J2u);

    // Perturbed -:
    perturbq.updQ() = q - Delta*qdot;
    system.realize(perturbq, Stage::Position);
    matter.calcStationJacobian(perturbq,whichBod,whichPt, JS1_P);
    matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF1_P);
    matter.calcSystemJacobian(perturbq, J1);
    JS1_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
    JF1_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
    matter.multiplyBySystemJacobian(perturbq,u, J1u);

    // Estimate JDots:
    JSDot_P = (JS2_P-JS1_P)/Delta/2;
    JFDot_P = (JF2_P-JF1_P)/Delta/2;
    JDot    = (J2-J1)/Delta/2;

    // Estimate JDotus:
    JSDot_Pu = (JS2_Pu-JS1_Pu)/Delta/2;
    JFDot_Pu = (JF2_Pu-JF1_Pu)/Delta/2;
    JDotu    = (J2u-J1u)/Delta/2;

    // Calculate errors in JDot*u:
    SimTK_TEST_EQ_TOL((JSDot_P*u-sbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JFDot_P*u-fbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JDot*u-sysbias).norm(), 0, SqrtEps);

    // Calculate errors in JDotu:
    SimTK_TEST_EQ_TOL((JSDot_Pu-sbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JFDot_Pu-fbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JDotu-sysbias).norm(), 0, SqrtEps);
}
Example #13
0
void testTaskJacobians() {
    MultibodySystem system;
    MyForceImpl* frcp;
    makeSystem(false, system, frcp);
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

    State state = system.realizeTopology();
    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int nb = matter.getNumBodies();

    // Attainable accuracy drops with problem size.
    const Real Slop = nu*SignificantReal;

    system.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);

    system.realize(state, Stage::Position);

    Matrix_<SpatialVec> J;
    Matrix Jmat, Jmat2;
    matter.calcSystemJacobian(state, J);
    SimTK_TEST_EQ(J.nrow(), nb); SimTK_TEST_EQ(J.ncol(), nu);
    matter.calcSystemJacobian(state, Jmat);
    SimTK_TEST_EQ(Jmat.nrow(), 6*nb); SimTK_TEST_EQ(Jmat.ncol(), nu);

    // Unpack J into Jmat2 and compare with Jmat.
    Jmat2.resize(6*nb, nu);
    for (int row=0; row < nb; ++row) {
        const int nxtr = 6*row; // row index into scalar matrix
        for (int col=0; col < nu; ++col) {
            for (int k=0; k<3; ++k) {
                Jmat2(nxtr+k, col) = J(row,col)[0][k];
                Jmat2(nxtr+3+k, col) = J(row,col)[1][k];
            }
        }
    }
    // These should be exactly the same.
    SimTK_TEST_EQ_TOL(Jmat2, Jmat, SignificantReal);

    Vector randU = 100.*Test::randVector(nu), resultU1, resultU2;
    Vector_<SpatialVec> randF(nb), resultF1, resultF2;
    for (int i=0; i<nb; ++i) randF[i] = 100.*Test::randSpatialVec();

    matter.multiplyBySystemJacobian(state, randU, resultF1);
    resultF2 = J*randU;
    SimTK_TEST_EQ_TOL(resultF1, resultF2, Slop);

    matter.multiplyBySystemJacobianTranspose(state, randF, resultU1);
    resultU2 = ~J*randF;
    SimTK_TEST_EQ_TOL(resultU1, resultU2, Slop);

    // See if Station Jacobian can be used to duplicate the translation
    // rows of the System Jacobian, and if Frame Jacobian can be used to
    // duplicate the whole thing.
    Array_<MobilizedBodyIndex> allBodies(nb);
    for (int i=0; i<nb; ++i) allBodies[i]=MobilizedBodyIndex(i);
    Array_<Vec3> allOrigins(nb, Vec3(0));

    Matrix_<Vec3> JS, JS2, JSbyrow;
    Matrix_<SpatialVec> JF, JF2, JFbyrow;

    matter.calcStationJacobian(state, allBodies, allOrigins, JS);
    matter.calcFrameJacobian(state, allBodies, allOrigins, JF);
    for (int i=0; i<nb; ++i) {
        for (int j=0; j<nu; ++j) {
            SimTK_TEST_EQ(JS(i,j), J(i,j)[1]);
            SimTK_TEST_EQ(JF(i,j), J(i,j));
        }
    }

    // Now use random stations to calculate JS & JF.
    Array_<Vec3> randS(nb);
    for (int i=0; i<nb; ++i) randS[i] = 10.*Test::randVec3();
    matter.calcStationJacobian(state, allBodies, randS, JS);
    matter.calcFrameJacobian(state, allBodies, randS, JF);

    // Recalculate one row at a time to test non-contiguous memory handling.
    // Do it backwards just to show off.
    JSbyrow.resize(nb, nu); JFbyrow.resize(nb, nu);
    for (int i=nb-1; i >= 0; --i) {
        matter.calcStationJacobian(state, allBodies[i], randS[i], JSbyrow[i]);
        matter.calcFrameJacobian(state, allBodies[i], randS[i], JFbyrow[i]);
    }
    SimTK_TEST_EQ(JS, JSbyrow);
    SimTK_TEST_EQ(JF, JFbyrow);

    // Calculate JS2=JS and JF2=JF again using multiplication by mobility-space 
    // unit vectors.
    JS2.resize(nb, nu); JF2.resize(nb, nu);
    Vector zeroU(nu, 0.);
    for (int i=0; i < nu; ++i) {
        zeroU[i] = 1;
        matter.multiplyByStationJacobian(state, allBodies, randS, zeroU, JS2(i));
        matter.multiplyByFrameJacobian(state, allBodies, randS, zeroU, JF2(i));
        zeroU[i] = 0;
    }
    SimTK_TEST_EQ_TOL(JS2, JS, Slop);
    SimTK_TEST_EQ_TOL(JF2, JF, Slop);

    // Calculate JS2t=~JS using multiplication by force-space unit vectors.
    Matrix_<Row3> JS2t(nu,nb);
    Vector_<Vec3> zeroF(nb, Vec3(0));
    // While we're at it, let's test non-contiguous vectors by filling in
    // this scalar version and using its non-contig rows as column temps.
    Matrix JS3mat(3*nb,nu);
    for (int b=0; b < nb; ++b) {
        for (int k=0; k<3; ++k) {
            zeroF[b][k] = 1;
            RowVectorView JS3matr = JS3mat[3*b+k];
            matter.multiplyByStationJacobianTranspose(state, allBodies, randS, 
                zeroF, ~JS3matr);
            zeroF[b][k] = 0;
            for (int u=0; u < nu; ++u)
                JS2t(u,b)[k] = JS3matr[u];
        }
    }
    SimTK_TEST_EQ_TOL(JS2, ~JS2t, Slop); // we'll check JS3mat below

    // Calculate JF2t=~JF using multiplication by force-space unit vectors.
    Matrix_<SpatialRow> JF2t(nu,nb);
    Vector_<SpatialVec> zeroSF(nb, SpatialVec(Vec3(0)));
    // While we're at it, let's test non-contiguous vectors by filling in
    // this scalar version and using its non-contig rows as column temps.
    Matrix JF3mat(6*nb,nu);
    for (int b=0; b < nb; ++b) {
        for (int k=0; k<6; ++k) {
            zeroSF[b][k/3][k%3] = 1;
            RowVectorView JF3matr = JF3mat[6*b+k];
            matter.multiplyByFrameJacobianTranspose(state, allBodies, randS, 
                zeroSF, ~JF3matr);
            zeroSF[b][k/3][k%3] = 0;
            for (int u=0; u < nu; ++u)
                JF2t(u,b)[k/3][k%3] = JF3matr[u];
        }
    }
    SimTK_TEST_EQ_TOL(JF2, ~JF2t, Slop); // we'll check JS3mat below


    // All three methods match. Now let's see if they are right by shifting
    // the System Jacobian to the new stations.

    for (int i=0; i<nb; ++i) {
        const MobilizedBody& mobod = matter.getMobilizedBody(allBodies[i]);
        const Rotation& R_GB = mobod.getBodyRotation(state);
        const Vec3 S_G = R_GB*randS[i];
        for (int j=0; j<nu; ++j) {
            const Vec3 w = J(i,j)[0];
            const Vec3 v = J(i,j)[1];
            const Vec3 vJ = v + w % S_G; // Shift
            const Vec3 vS = JS2(i,j);
            const SpatialVec vF = JF2(i,j);
            SimTK_TEST_EQ(vS, vJ);
            SimTK_TEST_EQ(vF, SpatialVec(w, vJ));
        }
    }


    // Now create a scalar version of JS and make sure it matches the Vec3 one.
    Matrix JSmat, JSmat2, JFmat, JFmat2;

    matter.calcStationJacobian(state, allBodies, randS, JSmat);
    matter.calcFrameJacobian(state, allBodies, randS, JFmat);
    SimTK_TEST_EQ(JSmat.nrow(), 3*nb); SimTK_TEST_EQ(JSmat.ncol(), nu);
    SimTK_TEST_EQ(JFmat.nrow(), 6*nb); SimTK_TEST_EQ(JFmat.ncol(), nu);

    SimTK_TEST_EQ_TOL(JSmat, JS3mat, Slop); // same as above?
    SimTK_TEST_EQ_TOL(JFmat, JF3mat, Slop); // same as above?

    // Unpack JS into JSmat2 and compare with JSmat.
    JSmat2.resize(3*nb, nu);
    for (int row=0; row < nb; ++row) {
        const int nxtr = 3*row; // row index into scalar matrix
        for (int col=0; col < nu; ++col) {
            for (int k=0; k<3; ++k) {
                JSmat2(nxtr+k, col) = JS(row,col)[k];
            }
        }
    }
    // These should be exactly the same.
    SimTK_TEST_EQ_TOL(JSmat2, JSmat, SignificantReal);

    // Unpack JF into JFmat2 and compare with JFmat.
    JFmat2.resize(6*nb, nu);
    for (int row=0; row < nb; ++row) {
        const int nxtr = 6*row; // row index into scalar matrix
        for (int col=0; col < nu; ++col) {
            for (int k=0; k<6; ++k) {
                JFmat2(nxtr+k, col) = JF(row,col)[k/3][k%3];
            }
        }
    }
    // These should be exactly the same.
    SimTK_TEST_EQ_TOL(JFmat2, JFmat, SignificantReal);
}
Example #14
0
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.

    MultibodySystem         mbs; mbs.setUseUniformBackground(true);
    GeneralForceSubsystem    forces(mbs);
    SimbodyMatterSubsystem  pend(mbs);
    DecorationSubsystem     viz(mbs);
    Force::UniformGravity gravity(forces, pend, Vec3(0, -g, 0));

    MobilizedBody::Ball connector(pend.Ground(), 
                                    Transform(1*Vec3(0, 0, 0)),
                                  MassProperties(1, Vec3(0,0,0), Inertia(10,20,30)),
                                    Transform(1*Vec3(0, .5, 0)));

    connector.setDefaultRadius(0.05); // for the artwork

    //connector.setDefaultRotation( Rotation(Pi/4, Vec3(0,0,1) );
 
    const Real m1 = 5;
    const Real m2 = 1;
    const Real radiusRatio = std::pow(m2/m1, 1./3.);
    const Vec3 weight1Location(0, 0, -d/2); // in local frame of swinging body
    const Vec3 weight2Location(0, 0,  d/2); // in local frame of swinging body
    const Vec3 COM = (m1*weight1Location+m2*weight2Location)/(m1+m2);

    const MassProperties swingerMassProps
        (m1+m2, COM, 1*Inertia(1,1,1) + m1*UnitInertia::pointMassAt(weight1Location)
                                      + m2*UnitInertia::pointMassAt(weight2Location));
    MobilizedBody::Screw swinger(connector, 
                                    Transform( Rotation( 0*.7, Vec3(9,8,7) ),
                                              1*Vec3(0,-.5,0)),
                                 swingerMassProps,
                                    Transform( Rotation(0*1.3, Vec3(0,0,1) ),
                                              COM+0*Vec3(0,0,3)),    // inboard joint location
                                 0.3); // pitch

    // Add a blue sphere around the weight.
    viz.addBodyFixedDecoration(swinger, weight1Location, 
          DecorativeSphere(d/8).setColor(Blue).setOpacity(.2));
    viz.addBodyFixedDecoration(swinger, weight2Location, 
          DecorativeSphere(radiusRatio*d/8).setColor(Green).setOpacity(.2));
    viz.addRubberBandLine(GroundIndex, Vec3(0),
                          swinger, Vec3(0),
                          DecorativeLine().setColor(Blue).setLineThickness(10)
                                          .setRepresentation(DecorativeGeometry::DrawPoints));

    //forces.addMobilityConstantForce(swinger, 0, 10);
    Force::ConstantTorque(forces, swinger, Vec3(0,0,10));
    //forces.addConstantForce(swinger, Vec3(0), Vec3(0,10,0));
    //forces.addConstantForce(swinger, Vec3(0,0,0), Vec3(10,10,0)); // z should do nothing
    //forces.addMobilityConstantForce(swinger, 1, 10);
   // forces.addMobilityConstantForce(swinger, 2, 60-1.2);

    State s = mbs.realizeTopology(); // define appropriate states for this System


    pend.setUseEulerAngles(s, true);
    mbs.realizeModel(s);

    mbs.realize(s);

    // Create a study using the Runge Kutta Merson integrator
    RungeKuttaMersonIntegrator myStudy(mbs);
    myStudy.setAccuracy(1e-6);

    // This will pick up decorative geometry from
    // each subsystem that generates any, including of course the 
    // DecorationSubsystem, but not limited to it.
    Visualizer display(mbs);


    const Real expectedPeriod = 2*Pi*std::sqrt(d/g);
    printf("Expected period: %g seconds\n", expectedPeriod);

    const Real dt = 1./30; // output intervals
    const Real finalTime = 1*expectedPeriod;

    for (Real startAngle = 10; startAngle <= 90; startAngle += 10) {
        s = mbs.getDefaultState();
        connector.setQToFitRotation(s, Rotation(Pi/4, Vec3(1,1,1)) );

        printf("time  theta      energy           *************\n");

        swinger.setQToFitTransform(s, Transform( Rotation( BodyRotationSequence, 0*Pi/2, XAxis, 0*Pi/2, YAxis ), Vec3(0,0,0) ));
        swinger.setUToFitVelocity(s, SpatialVec(0*Vec3(1.1,1.2,1.3),Vec3(0,0,-1)));

        s.updTime() = 0;
        myStudy.initialize(s);

        cout << "MassProperties in B=" << swinger.expressMassPropertiesInAnotherBodyFrame(myStudy.getState(),swinger);
        cout << "MassProperties in G=" << swinger.expressMassPropertiesInGroundFrame(myStudy.getState());
        cout << "Spatial Inertia    =" << swinger.calcBodySpatialInertiaMatrixInGround(myStudy.getState());

        int stepNum = 0;
        for (;;) {
            // Should check for errors and other interesting status returns.
            myStudy.stepTo(myStudy.getTime() + dt);
            const State& s = myStudy.getState(); // s is now the integrator's internal state

            if ((stepNum++%10)==0) {
                // This is so we can calculate potential energy (although logically
                // one should be able to do that at Stage::Position).
                mbs.realize(s, Stage::Dynamics);

                cout << s.getTime() << ": E=" << mbs.calcEnergy(s) 
                     << " connector q=" << connector.getQ(s) 
                     << ": swinger q=" << swinger.getQ(s) << endl;

                // This is so we can look at the UDots.
                mbs.realize(s, Stage::Acceleration);

                cout << "q =" << pend.getQ(s) << endl;
                cout << "u =" << pend.getU(s) << endl;
                cout << "ud=" << pend.getUDot(s) << endl;

                cout << "Connector V=" << connector.getMobilizerVelocity(s) << endl;
                cout << "Swinger V=" << swinger.getMobilizerVelocity(s) << endl;

                const Rotation& R_MbM = swinger.getMobilizerTransform(s).R();
                Vec4 aaMb = R_MbM.convertRotationToAngleAxis();
                cout << "angle=" << aaMb[0] << endl;
                cout << "axisMb=" << aaMb.drop1(0) << endl;
                cout << "axisMb=" << ~R_MbM*aaMb.drop1(0) << endl;
            }

            display.report(s);
            if (s.getTime() >= finalTime)
                break;
        }
    }
} 
catch (const exception& e) {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
}
}
Real ObservedPointFitter::findBestFit
   (const MultibodySystem& system, State& state, 
    const Array_<MobilizedBodyIndex>&  bodyIxs, 
    const Array_<Array_<Vec3> >&       stations, 
    const Array_<Array_<Vec3> >&       targetLocations, 
    const Array_<Array_<Real> >&       weights, 
    Real tolerance) 
{    
    // Verify the inputs.
    
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
    SimTK_APIARGCHECK(bodyIxs.size() == stations.size() && stations.size() == targetLocations.size(), "ObservedPointFitter", "findBestFit", "bodyIxs, stations, and targetLocations must all be the same length");
    int numBodies = matter.getNumBodies();
    for (int i = 0; i < (int)stations.size(); ++i) {
        SimTK_APIARGCHECK(bodyIxs[i] >= 0 && bodyIxs[i] < numBodies, "ObservedPointFitter", "findBestFit", "Illegal body ID");
        SimTK_APIARGCHECK(stations[i].size() == targetLocations[i].size(), "ObservedPointFitter", "findBestFit", "Different number of stations and target locations for body");
    }
    
    // Build a list of children for each body.
    
    Array_<Array_<MobilizedBodyIndex> > children(matter.getNumBodies());
    for (int i = 0; i < matter.getNumBodies(); ++i) {
        const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
        if (!body.isGround())
            children[body.getParentMobilizedBody().getMobilizedBodyIndex()].push_back(body.getMobilizedBodyIndex());
    }

    // Build a mapping of body IDs to indices.
    
    Array_<int> bodyIndex(matter.getNumBodies());
    for (int i = 0; i < (int) bodyIndex.size(); ++i)
        bodyIndex[i] = -1;
    for (int i = 0; i < (int)bodyIxs.size(); ++i)
        bodyIndex[bodyIxs[i]] = i;
    
    // Find the number of stations on each body with a nonzero weight.
    
    Array_<int> numStations(matter.getNumBodies());
    for (int i = 0; i < (int) numStations.size(); ++i)
        numStations[i] = 0;
    for (int i = 0; i < (int)weights.size(); ++i) {
        for (int j = 0; j < (int)weights[i].size(); ++j)
            if (weights[i][j] != 0)
                numStations[bodyIxs[i]]++;
    }

    // Perform the initial estimation of Q for each mobilizer.
    // Our first guess is the passed-in q's, with quaternions converted
    // to Euler angles if necessary. As we solve a subproblem for each
    // of the bodies in ascending order, we'll update tempState's q's
    // for that body to their solved values.
    State tempState;
    if (!matter.getUseEulerAngles(state))
        matter.convertToEulerAngles(state, tempState);
    else tempState = state;
    system.realizeModel(tempState);
    system.realize(tempState, Stage::Position);

    // This will accumulate best-guess spatial poses for the bodies as
    // they are processed. This is useful for when a body is used as
    // an artificial base body; our first guess will to be to place it
    // wherever it was the last time it was used in a subproblem.
    Array_<Transform> guessX_GB(matter.getNumBodies());
    for (MobilizedBodyIndex mbx(1); mbx < guessX_GB.size(); ++mbx)
        guessX_GB[mbx] = matter.getMobilizedBody(mbx).getBodyTransform(tempState);

    for (int i = 0; i < matter.getNumBodies(); ++i) {
        MobilizedBodyIndex id(i);
        const MobilizedBody& body = matter.getMobilizedBody(id);
        if (body.getNumQ(tempState) == 0)
            continue; // No degrees of freedom to determine.
        if (children[id].size() == 0 && numStations[id] == 0)
            continue; // There are no stations whose positions are affected by this.
        Array_<MobilizedBodyIndex> originalBodyIxs;
        int currentBodyIndex = findBodiesForClonedSystem(body.getMobilizedBodyIndex(), numStations, matter, children, originalBodyIxs);
        if (currentBodyIndex == (int)originalBodyIxs.size()-1 
            && (bodyIndex[id] == -1 || stations[bodyIndex[id]].size() == 0))
            continue; // There are no stations whose positions are affected by this.
        MultibodySystem copy;
        Array_<MobilizedBodyIndex> copyBodyIxs;
        bool hasArtificialBaseBody;
        createClonedSystem(system, copy, originalBodyIxs, copyBodyIxs, hasArtificialBaseBody);
        const SimbodyMatterSubsystem& copyMatter = copy.getMatterSubsystem();
        // Construct an initial state.
        State copyState = copy.getDefaultState();
        assert(copyBodyIxs.size() == originalBodyIxs.size());
        for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
            const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
            const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
            if (ob==0 && hasArtificialBaseBody)
                copyMobod.setQToFitTransform(copyState, guessX_GB[origMobod.getMobilizedBodyIndex()]);
            else
                copyMobod.setQFromVector(copyState, origMobod.getQAsVector(tempState));
        }

        Array_<Array_<Vec3> > copyStations(copyMatter.getNumBodies());
        Array_<Array_<Vec3> > copyTargetLocations(copyMatter.getNumBodies());
        Array_<Array_<Real> > copyWeights(copyMatter.getNumBodies());
        for (int j = 0; j < (int)originalBodyIxs.size(); ++j) {
            int index = bodyIndex[originalBodyIxs[j]];
            if (index != -1) {
                copyStations[copyBodyIxs[j]] = stations[index];
                copyTargetLocations[copyBodyIxs[j]] = targetLocations[index];
                copyWeights[copyBodyIxs[j]] = weights[index];
            }
        }
        try {
            OptimizerFunction optimizer(copy, copyState, copyBodyIxs, copyStations, copyTargetLocations, copyWeights);
            Vector q(copyState.getQ());
            //std::cout << "BODY " << i << " q0=" << q << std::endl;
            optimizer.optimize(q, tolerance);
            //std::cout << "  qf=" << q << std::endl;
            copyState.updQ() = q;
            copy.realize(copyState, Stage::Position);
            // Transfer updated state back to tempState as improved initial guesses.
            // However, all but the currentBody will get overwritten later.
            for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
                const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
                guessX_GB[originalBodyIxs[ob]] = copyMobod.getBodyTransform(copyState);

                if (ob==0 && hasArtificialBaseBody) continue; // leave default state
                const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
                origMobod.setQFromVector(tempState, copyMobod.getQAsVector(copyState));
            }
            //body.setQFromVector(tempState, copyMatter.getMobilizedBody(copyBodyIxs[currentBodyIndex]).getQAsVector(copyState));
        }
        catch (Exception::OptimizerFailed ex) {
            std::cout << "Optimization failure for body "<<i<<": "<<ex.getMessage() << std::endl;
            // Just leave this body's state variables set to 0, and rely on the final optimization to fix them.
        }
    }

    // Now do the final optimization of the whole system.

    OptimizerFunction optimizer(system, tempState, bodyIxs, stations, targetLocations, weights);
    Vector q = tempState.getQ();
    optimizer.optimize(q, tolerance);
    if (matter.getUseEulerAngles(state))
        state.updQ() = q;
    else {
        tempState.updQ() = q;
        matter.convertToQuaternions(tempState, state);
    }
    
    // Return the RMS error in the optimized system.
    
    Real error;
    optimizer.objectiveFunc(q, true, error);
    if (UseWeighted)
        return std::sqrt(error - MinimumShift); // already weighted; this makes WRMS

    Real totalWeight = 0;
    for (int i = 0; i < (int)weights.size(); ++i)
        for (int j = 0; j < (int)weights[i].size(); ++j)
            totalWeight += weights[i][j];

    return std::sqrt((error-MinimumShift)/totalWeight);
}
Example #16
0
int main(int argc, char** argv) {
  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem         mbs;

    SimbodyMatterSubsystem  matter(mbs);
    GeneralForceSubsystem    forces(mbs);
    DecorationSubsystem     viz(mbs);
    Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::brick(hl[0],hl[1],hl[2])));
    body.addDecoration(DecorativeBrick(hl).setOpacity(.5));
    body.addDecoration(DecorativeLine(Vec3(0), Vec3(0,1,0)).setColor(Green));

    MobilizedBody::Free mobilizedBody(matter.Ground(), Transform(), body, Transform());
    MobilizedBody::Free mobilizedBody0(mobilizedBody, Transform(Vec3(1,2,0)), body, Transform(Vec3(0,1,0)));
    MobilizedBody::Free mobilizedBody2(mobilizedBody0, Vec3(-5,0,0), body, Transform());

    Body::Rigid gear1body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(.5, .1)));
    gear1body.addDecoration(DecorativeCircle(.5).setColor(Green).setOpacity(.7));
    gear1body.addDecoration(DecorativeLine(Vec3(0), Vec3(.5,0,0)).setColor(Black).setLineThickness(4));
    Body::Rigid gear2body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(1.5, .1)));
    gear2body.addDecoration(Transform(), DecorativeCircle(1.5).setColor(Blue).setOpacity(.7));  
    gear2body.addDecoration(Transform(), DecorativeLine(Vec3(0), Vec3(1.5,0,0)).setColor(Black).setLineThickness(4));

    MobilizedBody::Pin gear1(mobilizedBody2, Vec3(-1,0,0), gear1body, Transform()); // along z
    MobilizedBody::Pin gear2(mobilizedBody2, Vec3(1,0,0), gear2body, Transform()); // along z
    Constraint::NoSlip1D(mobilizedBody2, Vec3(-.5,0,0), UnitVec3(0,1,0), gear1, gear2);

    Constraint::ConstantSpeed(gear1, 100.);
    
    //Constraint::Ball myc2(matter.Ground(), Vec3(-4,2,0),  mobilizedBody2, Vec3(0,1,0));
    Constraint::Weld myc(matter.Ground(), Vec3(1,2,0),  mobilizedBody, Vec3(0,1,0));
    Constraint::Ball ball1(mobilizedBody, Vec3(2,0,0), mobilizedBody0, Vec3(3,1,1));
    Constraint::Ball ball2(mobilizedBody0, Vec3(2,0,0), mobilizedBody2, Vec3(3,0,0));
    //Constraint::PointInPlane pip(mobilizedBody0, UnitVec3(0,-1,0), 3, mobilizedBody2, Vec3(3,0,0));

    //Constraint::ConstantOrientation ori(mobilizedBody, Rotation(), mobilizedBody0, Rotation());
    //Constraint::ConstantOrientation ori2(mobilizedBody2, Rotation(), mobilizedBody0, Rotation());
    //Constraint::Weld weld(mobilizedBody, Transform(Rotation(Pi/4, ZAxis), Vec3(1,1,0)),
      //                    mobilizedBody2, Transform(Rotation(-Pi/4, ZAxis), Vec3(-1,-1,0)));
    
    //MyConstraint xyz(gear1, 100.);

    viz.addBodyFixedDecoration(mobilizedBody, Transform(Vec3(1,2,3)), DecorativeText("hello world").setScale(.1));



/*
    class MyHandler : public ScheduledEventHandler {
    public:
        MyHandler(const Constraint& cons) : c(cons) { }
        Real getNextEventTime(const State&, bool includeCurrentTime) const {
            return .314;
        }
        void handleEvent(State& s, Real acc, const Vector& ywts, const Vector& cwts, Stage& modified,
                         bool& shouldTerminate) const 
        {
            cout << "<<<< TRIGGERED AT T=" << s.getTime() << endl;
            c.enable(s);
            modified = Stage::Model;
        }
    private:
        const Constraint& c;
    };

    mbs.addEventHandler(new MyHandler(xyz));
*/


    State s = mbs.realizeTopology(); // returns a reference to the the default state

    //xyz.disable(s);

    //matter.setUseEulerAngles(s, true);
    mbs.realizeModel(s); // define appropriate states for this System

    //mobilizedBody0.setQ(s, .1);
    //mobilizedBody.setQ(s, .2);

    Visualizer display(mbs);
    display.setBackgroundColor(White);
    display.setBackgroundType(Visualizer::SolidColor);

    mbs.realize(s, Stage::Velocity);
    display.report(s);
    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;
    cout << "qErr=" << s.getQErr() << endl;
    cout << "uErr=" << s.getUErr() << endl;

    for (ConstraintIndex cid(0); cid < matter.getNumConstraints(); ++cid) {
        const Constraint& c = matter.getConstraint(cid);
        int mp,mv,ma;
        c.getNumConstraintEquationsInUse(s, mp,mv,ma);

        cout << "CONSTRAINT " << cid << (c.isDisabled(s) ? "**DISABLED** " : "")
             << " constrained bodies=" << c.getNumConstrainedBodies();
        if (c.getNumConstrainedBodies()) cout << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex();
        cout << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers() 
                                           << "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
             << " mp,mv,ma=" << mp << "," << mv << "," << ma 
             << endl;
        for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
            cout << "  constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex(); 
            cout << endl;
        }
        for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
            cout << "  constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex() 
                  << ", q(" << c.getNumConstrainedQ(s, cmx) << ")="; 
            for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
                cout << " " << c.getConstrainedQIndex(s, cmx, i);                  
            cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
            for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
                cout << " " << c.getConstrainedUIndex(s, cmx, i);
            cout << endl;
        }
        cout << c.getSubtree();
             
        if (mp) {
            cout << "perr=" << c.getPositionErrorsAsVector(s) << endl;
            cout << "   d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
            cout << "  ~d(Pt lambda)/dlambda=" << ~c.calcPositionConstraintMatrixPt(s);
            cout << "   d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);

            Matrix P = c.calcPositionConstraintMatrixP(s);
            Matrix PQ(mp,matter.getNQ(s));
            Vector out(matter.getNQ(s));
            for (int i=0; i<mp; ++i) {
                Vector in = ~P[i];
                matter.multiplyByNInv(s, true, in, out);
                PQ[i] = ~out;
            }
            cout << " calculated d(perr)/dq=" << PQ;
        }


        if (mv) {
            cout << "verr=" << c.getVelocityErrorsAsVector(s) << endl;
            //cout << "   d(verrdot)/dudot=" << c.calcVelocityConstraintMatrixV(s);
            cout << "  ~d(Vt lambda)/dlambda=" << ~c.calcVelocityConstraintMatrixVt(s);
        }

    }
    const Constraint& c = matter.getConstraint(myc.getConstraintIndex());

    cout << "Default configuration shown. Ready? "; getchar();

    mobilizedBody.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,1,1)),Vec3(.1,.2,.3)));
    mobilizedBody0.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,-1,1)),Vec3(.2,.2,.3)));
    mobilizedBody2.setQToFitTransform (s, Transform(Rotation(.05,Vec3(-1,1,1)),Vec3(.1,.2,.1)));
    mobilizedBody.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
    mobilizedBody0.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
    mobilizedBody2.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));

    //gear1.setUToFitAngularVelocity(s, Vec3(0,0,500)); // these should be opposite directions!
    //gear2.setUToFitAngularVelocity(s, Vec3(0,0,100));

    mbs.realize(s, Stage::Velocity);
    display.report(s);

    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;
    cout << "qErr=" << s.getQErr() << endl;
    cout << "uErr=" << s.getUErr() << endl;
    cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
    cout << "v_MbM=" << mobilizedBody.getMobilizerVelocity(s)[1] << endl;
    cout << "Unassembled configuration shown. Ready to assemble? "; getchar();

    // These are the SimTK Simmath integrators:
    RungeKuttaMersonIntegrator myStudy(mbs);
    //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::/*Newton*/Functional);
    //myStudy.setOrderLimit(2); // cpodes only
    //VerletIntegrator myStudy(mbs);
   // ExplicitEulerIntegrator myStudy(mbs, .0005); // fixed step
    //ExplicitEulerIntegrator myStudy(mbs); // variable step


    //myStudy.setMaximumStepSize(0.001);
    myStudy.setAccuracy(1e-6); myStudy.setAccuracy(1e-1);
    //myStudy.setProjectEveryStep(true);
    //myStudy.setProjectInterpolatedStates(false);
    myStudy.setConstraintTolerance(1e-7); myStudy.setConstraintTolerance(1e-2);
    //myStudy.setAllowInterpolation(false);
    //myStudy.setMaximumStepSize(.1);

    const Real dt = .02; // output intervals
    const Real finalTime = 2;

    myStudy.setFinalTime(finalTime);

    std::vector<State> saveEm;
    saveEm.reserve(2000);

    for (int i=0; i<50; ++i)
        saveEm.push_back(s);    // delay


    // Peforms assembly if constraints are violated.
    myStudy.initialize(s);

    for (int i=0; i<50; ++i)
        saveEm.push_back(s);    // delay

    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    {
        const State& s = myStudy.getState();
        display.report(s);
        cout << "q=" << s.getQ() << endl;
        cout << "u=" << s.getU() << endl;
        cout << "qErr=" << s.getQErr() << endl;
        cout << "uErr=" << s.getUErr() << endl;
        cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
        cout << "PE=" << mbs.calcPotentialEnergy(s) << " KE=" << mbs.calcKineticEnergy(s) << " E=" << mbs.calcEnergy(s) << endl;
        cout << "angle=" << std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1)) << endl;
        cout << "Assembled configuration shown. Ready to simulate? "; getchar();
    }

    Integrator::SuccessfulStepStatus status;
    int nextReport = 0;

    mbs.resetAllCountersToZero();

    int stepNum = 0;
    while ((status=myStudy.stepTo(nextReport*dt))
           != Integrator::EndOfSimulation) 
    {
        const State& s = myStudy.getState();
        mbs.realize(s, Stage::Acceleration);

        if ((stepNum++%10)==0) {
            const Real angle = std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1));
            printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(), 
                angle,
                mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
                myStudy.getPreviousStepSizeTaken(),
                Integrator::getSuccessfulStepStatusString(status).c_str(),
                myStudy.isStateInterpolated()?" (INTERP)":"");
            printf("     qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
                matter.getQErr(s).normRMS(),
                matter.getUErr(s).normRMS(),
                s.getSystemStage() >= Stage::Acceleration ? matter.getUDotErr(s).normRMS() : Real(-1));
#ifdef HASC
            cout << "CONSTRAINT perr=" << c.getPositionError(s)
                 << " verr=" << c.getVelocityError(s)
                 << " aerr=" << c.getAccelerationError(s)
                 << endl;
#endif
            //cout << "   d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
            //cout << "  ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
            //cout << "   d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
            cout << "Q=" << matter.getQ(s) << endl;
            cout << "U=" << matter.getU(s) << endl;
            cout << "Multipliers=" << matter.getMultipliers(s) << endl;
        }

        Vector qdot;
        matter.calcQDot(s, s.getU(), qdot);
       // cout << "===> qdot =" << qdot << endl;

        Vector qdot2;
        matter.multiplyByN(s, false, s.getU(), qdot2);
       // cout << "===> qdot2=" << qdot2 << endl;

        Vector u1,u2;
        matter.multiplyByNInv(s, false, qdot, u1);
        matter.multiplyByNInv(s, false, qdot2, u2);
      //  cout << "===> u =" << s.getU() << endl;
      //  cout << "===> u1=" << u1 << endl;
      //  cout << "===> u2=" << u2 << endl;
       // cout << "     norm=" << (s.getU()-u2).normRMS() << endl;

        display.report(s);
        saveEm.push_back(s);

        if (status == Integrator::ReachedReportTime)
            ++nextReport;
    }

    printf("Using Integrator %s:\n", myStudy.getMethodName());
    printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());

    printf("System stats: realize %dP %dV %dA, projectQ %d, projectU %d\n",
        mbs.getNumRealizationsOfThisStage(Stage::Position),
        mbs.getNumRealizationsOfThisStage(Stage::Velocity),
        mbs.getNumRealizationsOfThisStage(Stage::Acceleration),
        mbs.getNumProjectQCalls(), mbs.getNumProjectUCalls());


    while(true) {
        for (int i=0; i < (int)saveEm.size(); ++i) {
            display.report(saveEm[i]);
            //display.report(saveEm[i]); // half speed
        }
        getchar();
    }
  } 
  catch (const exception& e) {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
  }
  catch (...) {
    printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

}
int main() {
    
    // Build a system consisting of a chain of bodies with every possible mobilizer.
    
    MultibodySystem mbs;
    SimbodyMatterSubsystem matter(mbs);
    Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
    body.addDecoration(DecorativeSphere(.1));
    Random::Uniform random(0.0, 2.0);
    MobilizedBody lastBody = MobilizedBody::Pin(matter.Ground(), Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Slider(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Universal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Cylinder(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::BendStretch(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Planar(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Gimbal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Ball(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Translation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Free(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::LineOrientation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::FreeLine(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Weld(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    lastBody = MobilizedBody::Screw(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())), 0.5);
    lastBody = MobilizedBody::Ellipsoid(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
    mbs.realizeTopology();
    State& s = mbs.updDefaultState();
    mbs.realizeModel(s);
    
    // Choose a random initial conformation.
    
    for (int i = 0; i < s.getNQ(); ++i)
        s.updQ()[i] = random.getValue();
    mbs.realize(s, Stage::Instance);
    // The only constraints are the quaternions -- normalize them.
    Vector temp;
    mbs.project(s, 0.01);
    mbs.realize(s, Stage::Position);
    
    // Convert to Euler angles and make sure the positions are all the same.
    
    State euler = s;
    matter.convertToEulerAngles(s, euler);
    mbs.realize(euler, Stage::Position);
    for (int i = 0; i < matter.getNumBodies(); ++i) {
        const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
        Real dist = (body.getBodyOriginLocation(euler)-body.getBodyOriginLocation(s)).norm();
        ASSERT(dist < 1e-5);
    }

    // Now convert back to quaternions and make sure the positions are still the same.
    
    State quaternions = s;
    matter.convertToQuaternions(euler, quaternions);
    mbs.realize(quaternions, Stage::Position);
    for (int i = 0; i < matter.getNumBodies(); ++i) {
        const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
        Real dist = (body.getBodyOriginLocation(quaternions)-body.getBodyOriginLocation(s)).norm();
        ASSERT(dist < 1e-5);
    }
    
    // Compare the state variables to see if they have been accurately reproduced.
    
    mbs.project(s, 0.01); // Normalize the quaternions
    Real diff = std::sqrt((s.getQ()-quaternions.getQ()).normSqr()/s.getNQ());
    ASSERT(diff < 1e-5);

    std::cout << "Done" << std::endl;
}
Example #18
0
int main(int argc, char** argv) {
    std::vector<State> saveEm;
    saveEm.reserve(1000);

try // If anything goes wrong, an exception will be thrown.
  { int nseg = NSegments;
    int shouldFlop = 0;
    if (argc > 1) sscanf(argv[1], "%d", &nseg);
    if (argc > 2) sscanf(argv[2], "%d", &shouldFlop);

    // Create a multibody system using Simbody.
    MultibodySystem mbs;
    MyRNAExample myRNA(mbs, nseg, shouldFlop != 0);
    GeneralForceSubsystem forces(mbs);
    Force::UniformGravity ugs(forces, myRNA, Vec3(0, -g, 0), -0.8);

    const Vec3 attachPt(150, -40, -50);

    Force::TwoPointLinearSpring(forces, myRNA.Ground(), attachPt,
                                   myRNA.getMobilizedBody(MobilizedBodyIndex(myRNA.getNumBodies()-1)), Vec3(0),
                                   1000.,  // stiffness
                                   1.);    // natural length

    Force::GlobalDamper(forces, myRNA, 1000);


    State s = mbs.realizeTopology();


    //myRNA.getConstraint(ConstraintIndex(myRNA.getNConstraints()-4)).disable(s);


    //myRNA.setUseEulerAngles(s,true);
    mbs.realizeModel(s);
    mbs.realize(s, Stage::Position);

    for (ConstraintIndex cid(0); cid < myRNA.getNumConstraints(); ++cid) {
        const Constraint& c = myRNA.getConstraint(cid);
        int mp,mv,ma;
        c.getNumConstraintEquationsInUse(s, mp,mv,ma);

        cout << "CONSTRAINT " << cid 
             << " constrained bodies=" << c.getNumConstrainedBodies() 
             << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex()
             << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers() 
                                           << "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
             << " mp,mv,ma=" << mp << "," << mv << "," << ma 
             << endl;
        for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
            cout << "  constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex(); 
            cout << endl;
        }
        for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
            cout << "  constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex() 
                  << ", q(" << c.getNumConstrainedQ(s, cmx) << ")="; 
            for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
                cout << " " << c.getConstrainedQIndex(s, cmx, i);                  
            cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
            for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
                cout << " " << c.getConstrainedUIndex(s, cmx, i);
            cout << endl;
        }
        cout << c.getSubtree();

        cout << "   d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
        cout << "   d(perrdot)/du=" << ~c.calcPositionConstraintMatrixPt(s);

        cout << "   d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
    }





    SimbodyMatterSubtree sub(myRNA);
    sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(7)));
    sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(10)));
    //sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(20)));
    sub.realizeTopology();
    cout << "sub.ancestor=" << sub.getAncestorMobilizedBodyIndex();
//    cout << "  sub.terminalBodies=" << sub.getTerminalBodies() << endl;
//    cout << "sub.allBodies=" << sub.getAllBodies() << endl;
    for (SubtreeBodyIndex i(0); i < (int)sub.getAllBodies().size(); ++i) {
       cout << "sub.parent[" << i << "]=" << sub.getParentSubtreeBodyIndex(i);
//       cout << "  sub.children[" << i << "]=" << sub.getChildSubtreeBodyIndexs(i) << endl;
    }
   

    printf("# quaternions in use = %d\n", myRNA.getNumQuaternionsInUse(s));
    for (MobilizedBodyIndex i(0); i<myRNA.getNumBodies(); ++i) {
        printf("body %2d: using quat? %s; quat index=%d\n",
            (int)i, myRNA.isUsingQuaternion(s,i) ? "true":"false", 
            (int)myRNA.getQuaternionPoolIndex(s,i));
    }

    // And a study using the Runge Kutta Merson integrator
    bool suppressProject = false;

    RungeKuttaMersonIntegrator myStudy(mbs);
    //RungeKuttaFeldbergIntegrator myStudy(mbs);
    //RungeKutta3Integrator myStudy(mbs);
    //CPodesIntegrator  myStudy(mbs);
    //VerletIntegrator myStudy(mbs);
    //ExplicitEulerIntegrator myStudy(mbs);

    myStudy.setAccuracy(1e-2);
    myStudy.setConstraintTolerance(1e-3); 
    myStudy.setProjectEveryStep(false);

    Visualizer display(mbs);
    display.setBackgroundColor(White);
    display.setBackgroundType(Visualizer::SolidColor);
    display.setMode(Visualizer::RealTime);

    for (MobilizedBodyIndex i(1); i<myRNA.getNumBodies(); ++i)
        myRNA.decorateBody(i, display);
    myRNA.decorateGlobal(display);

    DecorativeLine rbProto; rbProto.setColor(Orange).setLineThickness(3);
    display.addRubberBandLine(GroundIndex, attachPt,MobilizedBodyIndex(myRNA.getNumBodies()-1),Vec3(0), rbProto);
    //display.addRubberBandLine(GroundIndex, -attachPt,myRNA.getNumBodies()-1,Vec3(0), rbProto);

    const Real dt = 1./30; // output intervals

    printf("time  nextStepSize\n");

    s.updTime() = 0;
    for (int i=0; i<50; ++i)
        saveEm.push_back(s);    // delay
    display.report(s);

    myStudy.initialize(s);
    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    saveEm.push_back(myStudy.getState());
    for (int i=0; i<50; ++i)
        saveEm.push_back(myStudy.getState());    // delay
    display.report(myStudy.getState());


    const double startReal = realTime(), startCPU = cpuTime();
    int stepNum = 0;
    for (;;) {
        const State& ss = myStudy.getState();

        mbs.realize(ss);

        if ((stepNum++%100)==0) {
            printf("%5g qerr=%10.4g uerr=%10.4g hNext=%g\n", ss.getTime(), 
                myRNA.getQErr(ss).normRMS(), myRNA.getUErr(ss).normRMS(),
                myStudy.getPredictedNextStepSize());
            printf("      E=%14.8g (pe=%10.4g ke=%10.4g)\n",
                mbs.calcEnergy(ss), mbs.calcPotentialEnergy(ss), mbs.calcKineticEnergy(ss));

            cout << "QERR=" << ss.getQErr() << endl;
            cout << "UERR=" << ss.getUErr() << endl;
        }

        //if (s.getTime() - std::floor(s.getTime()) < 0.2)
        //    display.addEphemeralDecoration( DecorativeSphere(10).setColor(Green) );

        display.report(ss);
        saveEm.push_back(ss);

        if (ss.getTime() >= 10)
            break;

        // TODO: should check for errors or have or teach RKM to throw. 
        myStudy.stepTo(ss.getTime() + dt, Infinity);
    }

    printf("CPU time=%gs, REAL time=%gs\n", cpuTime()-startCPU, realTime()-startReal);
    printf("Using Integrator %s:\n", myStudy.getMethodName());
    printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
    printf("# CONVERGENCE FAILS = %d\n", myStudy.getNumConvergenceTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
    printf("# PROJECTION FAILS = %d\n", myStudy.getNumProjectionFailures());

    display.dumpStats(std::cout);

    while(true) {
        for (int i=0; i < (int)saveEm.size(); ++i) {
            display.report(saveEm[i]);
            //display.report(saveEm[i]); // half speed
        }
        getchar();
    }
  } 
catch (const exception& e)
  {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
  }
}
static void testObservedPointFitter(bool useConstraint) {
    int failures = 0;
    for (int iter = 0; iter < ITERATIONS; ++iter) {
        
        // Build a system consisting of a chain of bodies with occasional side chains, and
        // a variety of mobilizers.

        MultibodySystem mbs;
        SimbodyMatterSubsystem matter(mbs);
        Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
        body.addDecoration(Transform(), DecorativeSphere(.1));
        MobilizedBody* lastBody = &matter.Ground();
        MobilizedBody* lastMainChainBody = &matter.Ground();
        vector<MobilizedBody*> bodies;
        Random::Uniform random(0.0, 1.0);
        random.setSeed(iter);

        for (int i = 0; i < NUM_BODIES; ++i) {
            bool mainChain = random.getValue() < 0.5;
            MobilizedBody* parent = (mainChain ? lastMainChainBody : lastBody);
            int type = (int) (random.getValue()*4);
            MobilizedBody* nextBody;
            if (type == 0) {
                MobilizedBody::Cylinder cylinder(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(cylinder.getMobilizedBodyIndex());
            }
            else if (type == 1) {
                MobilizedBody::Slider slider(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(slider.getMobilizedBodyIndex());
            }
            else if (type == 2) {
                MobilizedBody::Ball ball(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(ball.getMobilizedBodyIndex());
            }
            else {
                MobilizedBody::Pin pin(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
                nextBody = &matter.updMobilizedBody(pin.getMobilizedBodyIndex());
            }
            bodies.push_back(nextBody);
            if (mainChain)
                lastMainChainBody = nextBody;
            lastBody = nextBody;
        }
        mbs.realizeTopology();
        State s = mbs.getDefaultState();
        matter.setUseEulerAngles(s, true);
        mbs.realizeModel(s);

        // Choose a random initial conformation.

        vector<Real> targetQ(s.getNQ(), Real(0));
        for (MobilizedBodyIndex mbx(1); mbx < matter.getNumBodies(); ++mbx) {
            const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
            for (int i = 0; i < mobod.getNumQ(s); ++i) {
                const QIndex qx0 = mobod.getFirstQIndex(s);
                s.updQ()[qx0+i] = targetQ[qx0+i] = 2.0*random.getValue();
            }
        }
        //cout << "q0=" << s.getQ() << endl;
        mbs.realize(s, Stage::Position);

        // Select some random stations on each body.

        vector<vector<Vec3> > stations(NUM_BODIES);
        vector<vector<Vec3> > targetLocations(NUM_BODIES);
        vector<MobilizedBodyIndex> bodyIxs;
        for (int i = 0; i < NUM_BODIES; ++i) {
            MobilizedBodyIndex id = bodies[i]->getMobilizedBodyIndex();
            bodyIxs.push_back(id);
            int numStations = 1 + (int) (random.getValue()*4);
            for (int j = 0; j < numStations; ++j) {
                Vec3 pos(2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0);
                stations[i].push_back(pos);
                targetLocations[i].push_back(bodies[i]->getBodyTransform(s)*pos);
            }
        }

        Real distance = -1;
        if (useConstraint) {
            // Add a constraint fixing the distance between the first and last bodies.
            Real distance = (bodies[0]->getBodyOriginLocation(s)-bodies[NUM_BODIES-1]->getBodyOriginLocation(s)).norm();
            // (sherm 140506) Without this 1.001 this failed on clang.
            Constraint::Rod(*bodies[0], Vec3(0), *bodies[NUM_BODIES-1], Vec3(0), 1.001*distance);
        }
        s = mbs.realizeTopology();
        matter.setUseEulerAngles(s, true);
        mbs.realizeModel(s);

        // Try fitting it.
        State initState = s;
        // (sherm 140506) I raised this from .02 to .03 to make this more robust.
        if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.0, 0.03, distance))
            failures++;

        //cout << "q1=" << s.getQ() << endl;

        // Now add random noise to the target locations, and see if it can still fit decently.

        Random::Gaussian gaussian(0.0, 0.15);
        for (int i = 0; i < (int) targetLocations.size(); ++i) {
            for (int j = 0; j < (int) targetLocations[i].size(); ++j) {
                targetLocations[i][j] += Vec3(gaussian.getValue(), gaussian.getValue(), gaussian.getValue());
            }
        }

        s = initState; // start from same config as before
        if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.1, 0.5, distance))
            failures++;

        //cout << "q2=" << s.getQ() << endl;

    }

    ASSERT(failures == 0); // It found a reasonable fit every time.
    std::cout << "Done" << std::endl;
}
int main(int argc, char** argv) {
    static const Transform GroundFrame;
    static const Rotation ZUp(UnitVec3(XAxis), XAxis, UnitVec3(YAxis), ZAxis);
    static const Vec3 TestLoc(1,0,0);

  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem             mbs;

    SimbodyMatterSubsystem      matter(mbs);
    GeneralForceSubsystem       forces(mbs);
    DecorationSubsystem         viz(mbs);
    //Force::UniformGravity       gravity(forces, matter, Vec3(0, -g, 0));

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid particle = Body::Rigid(MassProperties(m, Vec3(0), Inertia(0)));
    particle.addDecoration(DecorativeSphere(.1).setColor(Red).setOpacity(.3));

    MobilizedBody::SphericalCoords
        anAtom(matter.Ground(), Transform(ZUp, TestLoc),
               particle, Transform(),
               0*Deg2Rad,  false,   // azimuth offset, negated
               0,          false,   // zenith offset, negated
               ZAxis,      true);  // translation axis, negated

    anAtom.setDefaultRadius(.1);
    anAtom.setDefaultAngles(Vec2(0, 30*Deg2Rad));

    viz.addRubberBandLine(matter.Ground(), TestLoc,
                          anAtom, Vec3(0),
                          DecorativeLine().setColor(Orange).setLineThickness(4));

    Force::MobilityLinearSpring(forces, anAtom, 0, 2, -30*Deg2Rad); // harmonic bend
    Force::MobilityLinearSpring(forces, anAtom, 1, 2, 45*Deg2Rad); // harmonic bend
    Force::MobilityLinearSpring(forces, anAtom, 2, 20, .5); // harmonic stretch

    Force::MobilityLinearDamper(forces, anAtom, 0, .1); // harmonic bend
    Force::MobilityLinearDamper(forces, anAtom, 1, .1); // harmonic bend
    Force::MobilityLinearDamper(forces, anAtom, 2, .1); // harmonic stretch

    
    State s = mbs.realizeTopology(); // returns a reference to the the default state
    mbs.realizeModel(s); // define appropriate states for this System
    mbs.realize(s, Stage::Instance); // instantiate constraints if any

    Visualizer display(mbs);
    display.setBackgroundType(Visualizer::SolidColor);

    mbs.realize(s, Stage::Velocity);
    display.report(s);

    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;

    char c;
    cout << "Default configuration shown. 1234 to move on.\n";

    //anAtom.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));

    while (true) {
        Real x;
        cout << "Torsion (deg)? "; cin >> x; if (x==1234) break;
        Vec2 a = anAtom.getAngles(s); a[0]=x*Deg2Rad; anAtom.setAngles(s, a);
        display.report(s);
        cout << "Bend (deg)? "; cin >> x; if (x==1234) break;
        a = anAtom.getAngles(s); a[1]=x*Deg2Rad; anAtom.setAngles(s, a);
        display.report(s);
        cout << "Radius? "; cin >> x; if (x==1234) break;
        anAtom.setRadius(s, x);
        display.report(s);
    }
    anAtom.setUToFitAngularVelocity(s, Vec3(.1,.2,.3));

    //anAtom.setAngle(s, 45*Deg2Rad);
    //anAtom.setTranslation(s, Vec2(.4, .1));

    mbs.realize(s, Stage::Dynamics);
    mbs.realize(s, Stage::Acceleration);

    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;
    cout << "qdot=" << s.getQDot() << endl;
    cout << "udot=" << s.getUDot() << endl;
    cout << "qdotdot=" << s.getQDotDot() << endl;
    display.report(s);

    cout << "Initialized configuration shown. Ready? ";
    cin >> c;

    RungeKuttaMersonIntegrator myStudy(mbs);
    myStudy.setAccuracy(1e-4);

    const Real dt = .02; // output intervals
    const Real finalTime = 20;

    myStudy.setFinalTime(finalTime);

    // Peforms assembly if constraints are violated.
    myStudy.initialize(s);

    cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
    cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
    cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
    cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
    cout << "U WEIGHTS=" << s.getUWeights() << endl;
    cout << "Z WEIGHTS=" << s.getZWeights() << endl;
    cout << "1/QTOLS=" << s.getQErrWeights() << endl;
    cout << "1/UTOLS=" << s.getUErrWeights() << endl;

    Integrator::SuccessfulStepStatus status;
    int nextReport = 0;
    while ((status=myStudy.stepTo(nextReport*dt))
           != Integrator::EndOfSimulation) 
    {
        const State& s = myStudy.getState();
        mbs.realize(s);
        printf("%5g %10.4g %10.4g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(), 
            anAtom.getAngles(s)[0], anAtom.getAngles(s)[1], anAtom.getRadius(s),
            //anAtom.getAngle(s), anAtom.getTranslation(s)[0], anAtom.getTranslation(s)[1],
            //anAtom.getQ(s)[0], anAtom.getQ(s)[1], anAtom.getQ(s)[2],
            mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
            myStudy.getPreviousStepSizeTaken(),
            Integrator::getSuccessfulStepStatusString(status).c_str(),
            myStudy.isStateInterpolated()?" (INTERP)":"");

        display.report(s);

        if (status == Integrator::ReachedReportTime)
            ++nextReport;
    }

    printf("Using Integrator %s:\n", myStudy.getMethodName());
    printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());

  } 
  catch (const std::exception& e) {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
  }
  catch (...) {
    printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

}
Example #21
0
//------------------------------------------------------------------------------
// main program
//------------------------------------------------------------------------------
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.

    int i = 0;

    //--------------------------------------------------------------------------
    // Experimental data points (x,y) of tibia origin (tibial plateau) measured 
    // w.r.t. to origin of the femur (hip joint center) in the femur frame as a 
    // function of knee joint angle. From Yamaguchi and Zajac, 1989.
    //--------------------------------------------------------------------------
    // Tibia X:
    int npx = 12;
    Real 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};
    Real 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};
    // Tibia Y; note that Y data is offset by -0.4 due to body frame placement.
    int npy = 7;
    Real angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399, 
                   -0.174532925199, 0.159148563428, 2.094395102393};
    Real kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000, 
                    -0.396600000000, -0.395264000000, -0.396000000000 };

    // Create SimTK Vectors to hold data points, and initialize from above arrays.
    Vector ka_x (npx, angX); // measured knee angles when X data was collected
    Vector ka_y (npy, angY); // measured knee angles when Y data was collected
    Vector tib_x(npx, kneeX);
    Vector tib_y(npy, kneeY);

    #ifdef SHOULD_EXAGGERATE
        // See above.
        tib_x *= ExaggerateX;
        tib_y = (tib_y+0.4)*ExaggerateY - 0.4; // exaggerate deviation only, not offset
    #endif

    // Generate splines from vectors of data points.
    const int Degree = 3; // use cubics
    SplineFitter<Real> fitterX = SplineFitter<Real>::fitFromGCV(Degree, ka_x, tib_x);
    SplineFitter<Real> fitterY = SplineFitter<Real>::fitFromGCV(Degree, ka_y, tib_y);
    Spline fx = fitterX.getSpline();
    Spline fy = fitterY.getSpline();

    //--------------------------------------------------------------------------
       // Define the 6-spatial functions that specify the motion of the tibia as a 
    // a FunctionBased MobilizedBody (shank) w.r.t. to its parent (the thigh). 
    //--------------------------------------------------------------------------
    // Each function has to return 1 Real value
    std::vector<const Function*> functions(6);
    // as a function of coordIndices (more than one per function) 
    std::vector< std::vector<int> > coordIndices(6);
    // about a body-fixed axis for rotations or in parent translations 
    std::vector<Vec3> axes(6);

    // Set the 1st and 2nd spatial rotation about the orthogonal (X then Y) axes as 
    // constant values. That is they don't contribute to motion nor do they have   
    // any coordinates in the equations of motion.
    // |--------------------------------|
    // | 1st function: rotation about X |
    // |--------------------------------|
    functions[0] = (new Function::Constant(0, 0));
    std::vector<int> noIndex(0);
    coordIndices[0] =(noIndex);

    // |--------------------------------|
    // | 2nd function: rotation about Y |
    // |--------------------------------|
    functions[1] = (new Function::Constant(0, 0));
    coordIndices[1] = (noIndex);

    // Set the spatial rotation about third axis to be the knee-extension
    // angle (the one q) about the Z-axis of the tibia at the femoral condyles
    // Define the coefficients of the linear function of the knee-angle with the
    // spatial rotation about Z.
    Vector coeff(2);
    // Linear function x3 = coeff[0]*q + coeff[1]
    coeff[0] = 1;  coeff[1] = 0;
    // |--------------------------------|
    // | 3rd function: rotation about Z |
    // |--------------------------------|
    functions[2] = new Function::Linear(coeff);
    // function of coordinate 0 (knee extension angle)
    std::vector<int> qIndex(1,0);
    coordIndices[2] = qIndex;

    // Set the spatial translations as a function (splines) along the parent X and Y axes
    // |-----------------------------------|
    // | 4th function: translation about X |
    // |-----------------------------------|
    functions[3] = new Spline(fx); // Give the mobilizer a copy it can own.
    coordIndices[3] =(qIndex);

    // |-----------------------------------|
    // | 5th function: translation about Y |
    // |-----------------------------------|
    functions[4] = new Spline(fy); // Give the mobilizer a copy it can own.
    coordIndices[4] =(qIndex);

    // |-----------------------------------|
    // | 6th function: translation about Z |
    // |-----------------------------------|
    functions[5] = (new Function::Constant(0, 0));
    coordIndices[5] = (noIndex);

    // Construct the multibody system
    const Real grav = 9.80665;
    MultibodySystem system; system.setUseUniformBackground(true);
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::Gravity gravity(forces, matter, -YAxis, grav);
    matter.setShowDefaultGeometry(true);

    //--------------------------------------------------------------------------
    // Define the model's physical (body) properties
    //--------------------------------------------------------------------------
    //Thigh
    Body::Rigid femur(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337))));
    femur.addDecoration(Transform(Vec3(0, -0.21+0.1715, 0)), 
        DecorativeCylinder(0.04, 0.21).setColor(Orange).setOpacity(.5));

    //Shank
    Body::Rigid tibia(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484))));
    tibia.addDecoration(Transform(Vec3(0, -0.235+0.1862, 0)), 
        DecorativeCylinder(0.035, 0.235).setColor(Red));

    //--------------------------------------------------------------------------
    // Build the multibody system by adding mobilized bodies to the matter subsystem
    //--------------------------------------------------------------------------
    // Add the thigh via hip joint
    MobilizedBody::Pin thigh(matter.Ground(), Transform(Vec3(0)), femur, Transform(Vec3(0.0020, 0.1715, 0)));

    // This is how you might model the knee if you could only use a pin joint.
    //MobilizedBody::Pin shank(thigh, Transform(Vec3(0.0033, -0.2294, 0)), 
    //                         tibia, Transform(Vec3(0.0, 0.1862, 0.0)));
    
    // NOTE: function of Y-translation data was defined int the femur frame 
    // according to Yamaguchi and Zajac, which had the orgin at the hip joint 
    // center and the Y along the long-axis of the femur and Z out of the page. 
    MobilizedBody::FunctionBased shank(thigh, Transform(Vec3(0.0020, 0.1715, 0)), 
                                       tibia, Transform(Vec3(0.0, 0.1862, 0.0)), 
                                       1, // # of mobilities (dofs) for this joint
                                       functions, coordIndices);

    // Add some stop springs so the knee angle won't get outside the range of spline 
    // data we have. This custom force element is defined above.
    Force::Custom(forces, new MyStop(shank, -Pi/2, 0*Pi, 100));


    //--------------------------------------------------------------------------
    // Setup reporters so we can get some output.
    //--------------------------------------------------------------------------
    // Vizualizer Animation
    Visualizer viz(system);
    system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
    // Energy -- reporter defined above.
    system.addEventReporter(new MyEnergyReporter(system, 0.01));
    
    //--------------------------------------------------------------------------
    // Complete the construction of the "const" part of the System and
    // allocate the default state.
    //--------------------------------------------------------------------------
    system.realizeTopology();
    // Get a copy of the default state to work with.
    State state = system.getDefaultState();

    //--------------------------------------------------------------------------
    // Set modeling options if any (this one is not actually needed here).
    //--------------------------------------------------------------------------
    matter.setUseEulerAngles(state, true);
    // Complete construction of the model, allocating additional state variables
    // if necessary.
    system.realizeModel(state);

    //--------------------------------------------------------------------------
    // Set initial conditions.
    //--------------------------------------------------------------------------
    // Hip and knee coordinates and speeds similar to early swing
    double hip_angle = -45*Pi/180;
    double knee_angle = 0*Pi/180;
    double hip_vel = 1;
    double knee_vel = -5.0;

    // Set initial states (Q's and U's)
    // Position
    thigh.setOneQ(state, 0, hip_angle);
    shank.setOneQ(state, 0, knee_angle);
    // Speed
    thigh.setOneU(state, 0, hip_vel);
    shank.setOneU(state, 0, knee_vel);
    
    //--------------------------------------------------------------------------
    // Run simulation.
    //--------------------------------------------------------------------------
    RungeKuttaMersonIntegrator integ(system);
    integ.setAccuracy(Accuracy);
    TimeStepper ts(system, integ);
    ts.initialize(state); // set IC's
    ts.stepTo(5.0);
} 
catch (const exception& e) {
    printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);
}

    return 0;
}