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 = ©Matter.Ground();
else {
hasArtificialBaseBody = true; // not using the original joint here
MobilizedBody::Free free(copyMatter.Ground(), body);
copyBody = ©Matter.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);
}
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);
}
}
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);
}
}
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() {
// 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;
}
void testRiboseMobilizer()
{
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
DecorationSubsystem decorations(system);
matter.setShowDefaultGeometry(false);
// Put some hastily chosen mass there (doesn't help)
Body::Rigid rigidBody;
rigidBody.setDefaultRigidBodyMassProperties(MassProperties(
mass_t(20.0*daltons),
location_t(Vec3(0,0,0)*nanometers),
Inertia(20.0)
));
// One body anchored at C4 atom,
MobilizedBody::Weld c4Body(
matter.updGround(),
Rotation(-120*degrees, XAxis),
rigidBody,
Transform());
// sphere for C4 atom
decorations.addBodyFixedDecoration(
c4Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for C5 atom
decorations.addBodyFixedDecoration(
c4Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) )
);
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c4Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
// One body anchored at C3 atom -- works
// Pin version
//MobilizedBody::Pin c3Body(
// c4Body,
// Transform(),
// rigidBody,
// Transform(location_t(Vec3(0,0,1.5)*angstroms))
// );
// Function based pin version -- works
//TestPinMobilizer c3Body(
// c4Body,
// Transform(),
// rigidBody,
// Transform(location_t(Vec3(0,0,1.5)*angstroms))
// );
PseudorotationMobilizer c3Body(
c4Body,
Transform(),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(36.4*degrees), // amplitude
angle_t(-161.8*degrees) // phase
);
// sphere for C3 atom
decorations.addBodyFixedDecoration(
c3Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for O3 atom
decorations.addBodyFixedDecoration(
c3Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c3Body.getMobilizedBodyIndex(),
Vec3(0),
c3Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c3Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
PseudorotationMobilizer c2Body(
c3Body,
Rotation( angle_t(-80*degrees), YAxis ),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(35.8*degrees), // amplitude
angle_t(-91.3*degrees) // phase
);
// sphere for C2 atom
decorations.addBodyFixedDecoration(
c2Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for O2 atom
decorations.addBodyFixedDecoration(
c2Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c2Body.getMobilizedBodyIndex(),
Vec3(0),
c2Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c3Body.getMobilizedBodyIndex(),
Vec3(0),
c2Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
PseudorotationMobilizer c1Body(
c2Body,
Rotation( angle_t(-80*degrees), YAxis ),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(37.6*degrees), // amplitude
angle_t(52.8*degrees) // phase
);
// sphere for C1 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for N1 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(0,0,1))
);
// sphere for O4 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c2Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c1Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c1Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
// Prescribed motion
Constraint::ConstantSpeed(c3Body, 0.5);
// Two constraint way works; one constraint way does not
bool useTwoConstraints = true;
if (useTwoConstraints) {
// Constraints to make three generalized coordinates identical
std::vector<MobilizedBodyIndex> c32bodies(2);
c32bodies[0] = c3Body.getMobilizedBodyIndex();
c32bodies[1] = c2Body.getMobilizedBodyIndex();
std::vector<MobilizerQIndex> coordinates(2, MobilizerQIndex(0));
Constraint::CoordinateCoupler(matter, new DifferenceFunction, c32bodies, coordinates);
std::vector<MobilizedBodyIndex> c21bodies(2);
c21bodies[0] = c2Body.getMobilizedBodyIndex();
c21bodies[1] = c1Body.getMobilizedBodyIndex();
Constraint::CoordinateCoupler(matter, new DifferenceFunction, c21bodies, coordinates);
}
else { // trying to get single constraint way to work
// Try one constraint for all three mobilizers
std::vector<MobilizedBodyIndex> c123Bodies(3);
c123Bodies[0] = c1Body.getMobilizedBodyIndex();
c123Bodies[1] = c2Body.getMobilizedBodyIndex();
c123Bodies[2] = c3Body.getMobilizedBodyIndex();
std::vector<MobilizerQIndex> coords3(3, MobilizerQIndex(0));
Constraint::CoordinateCoupler(matter, new ThreeDifferencesFunction, c123Bodies, coords3);
}
Visualizer viz(system);
viz.setBackgroundType(Visualizer::SolidColor);
system.addEventReporter(new Visualizer::Reporter(viz, 0.10));
system.realizeTopology();
State& state = system.updDefaultState();
// Simulate it.
VerletIntegrator integ(system);
//RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(50.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);
}
}