//==============================================================================
// MAIN
//==============================================================================
// Simulate forever with a maximum step size small enough to provide adequate
// polling for user input and screen updating.
int main() {
try { // catch errors if any
// Create the Simbody MultibodySystem and Visualizer.
MyMechanism mech;
// We want real time performance here and don't need high accuracy, so we'll
// use semi explicit Euler. However, we hope to take fairly large steps so
// need error control to avoid instability. We must also prevent
// interpolation so that the state returned after each step is the
// integrator's "advanced state", which is modifiable, so that we can update
// it in response to user input.
SemiExplicitEuler2Integrator integ(mech.getSystem());
integ.setAccuracy(Real(1e-1)); // 10%
integ.setAllowInterpolation(false);
integ.initialize(mech.getDefaultState());
unsigned stepNum = 0;
while (true) {
// Get access to State being advanced by the integrator. Interpolation
// must be off so that we're modifying the actual trajectory.
State& state = integ.updAdvancedState();
// Output a frame to the Visualizer if it is time.
if (stepNum % DrawEveryN == 0)
mech.draw(state);
// Check for user input periodically.
if (stepNum % PollUserEveryN == 0 && mech.processUserInput(state))
break; // stop if user picked Quit from the Run menu
// Advance time by MaxStepSize. Might take multiple internal steps to
// get there, depending on difficulty and required accuracy.
integ.stepBy(MaxStepSize);
++stepNum;
}
// DONE.
dumpIntegratorStats(integ);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
//==============================================================================
// MAIN
//==============================================================================
// Simulate forever with a small max step size.
int main() {
try { // catch errors if any
// Create the Simbody MultibodySystem and Visualizer.
MyMechanism mech;
// We're forcing very small step sizes so should use very low order
// integration and loose accuracy. We must prevent interpolation so that the
// state returned after each step is the integrator's "advanced state",
// which is modifiable, in case we need to make a state change.
SemiExplicitEuler2Integrator integ(mech.getSystem());
integ.setAccuracy(Real(1e-1)); // 10%
integ.setAllowInterpolation(false);
integ.initialize(mech.getDefaultState());
unsigned stepNum = 0;
while (true) {
// Get access to State being advanced by the integrator. Interpolation
// must be off so that we're modifying the actual trajectory.
State& state = integ.updAdvancedState();
// Output a frame to the Visualizer if it is time.
if (stepNum % DrawEveryN == 0)
mech.draw(state);
// Check for user input periodically.
if (stepNum % PollUserEveryN == 0 && mech.processUserInput(state))
break; // stop if user picked Quit from the Run menu
// Check for speed/torque control transition events and handle.
const Real trqNow = mech.getMotorTorque(state);
if (mech.isSpeedControlEnabled(state)) {
if (std::abs(trqNow) > mech.getTorqueLimit(state)) {
printf("\n%d: SWITCH TO TORQUE CONTROL cuz trqNow=%g\n",
stepNum, trqNow);
mech.switchToTorqueControl(state);
}
} else { // Currently limiting torque.
// If the torque is now in the same direction as the error, try
// to switch back to speed control. If that doesn't work we'll at
// least reverse the applied maximum torque.
const Real errNow = mech.getSpeedError(state);
if (errNow * trqNow > 0) {
printf("\n%d: TRY SPEED CONTROL cuz errNow=%g, trqNow=%g\n",
stepNum, errNow, trqNow);
mech.tryToSwitchToSpeedControl(state);
}
}
// Advance time by MaxStepSize. Might take multiple internal steps to
// get there, depending on required accuracy.
integ.stepBy(MaxStepSize);
++stepNum;
}
// DONE.
dumpIntegratorStats(integ);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
