int main() {
// Create the system.
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
MobilizedBody lastBody = matter.Ground();
for (int i = 0; i < 10; ++i) {
MobilizedBody::Ball pendulum(lastBody, Transform(Vec3(0)),
pendulumBody, Transform(Vec3(0, 1, 0)));
lastBody = pendulum;
}
system.addEventReporter(new Visualizer::Reporter(system, 1./30));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
Random::Gaussian random;
for (int i = 0; i < state.getNQ(); ++i)
state.updQ()[i] = random.getValue();
// Simulate it.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(10.0);
}
// Create a Visualizer and put it in PassThrough mode.
Impl(Visualizer* owner, const MultibodySystem& system,
const Array_<String>& searchPath)
: m_system(system), m_protocol(*owner, searchPath),
m_shutdownWhenDestructed(false), m_upDirection(YAxis), m_groundHeight(0),
m_mode(PassThrough), m_frameRateFPS(DefaultFrameRateFPS),
m_simTimeUnitsPerSec(1),
m_desiredBufferLengthInSec(DefaultDesiredBufferLengthInSec),
m_timeBetweenFramesInNs(secToNs(1/DefaultFrameRateFPS)),
m_allowableFrameJitterInNs(DefaultAllowableFrameJitterInNs),
m_allowableFrameTimeSlopInNs(
secToNs(DefaultSlopAsFractionOfFrameInterval/DefaultFrameRateFPS)),
m_adjustedRealTimeBase(realTimeInNs()),
m_prevFrameSimTime(-1), m_nextFrameDueAdjRT(-1),
m_oldest(0),m_nframe(0),
m_drawThreadIsRunning(false), m_drawThreadShouldSuicide(false),
m_refCount(0)
{
pthread_mutex_init(&m_queueLock, NULL);
pthread_cond_init(&m_queueNotFull, NULL);
pthread_cond_init(&m_queueNotEmpty, NULL);
pthread_cond_init(&m_queueIsEmpty, NULL);
setMode(PassThrough);
clearStats();
m_protocol.setMaxFrameRate(m_frameRateFPS);
m_protocol.setBackgroundColor(White);
m_protocol.setBackgroundType(system.getUseUniformBackground()
? SolidColor : GroundAndSky);
m_protocol.setSystemUpDirection(system.getUpDirection());
}
void testSpeedCoupler2() {
// Create a system involving a constraint that affects three different
// bodies.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
createGimbalSystem(system);
MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
std::vector<MobilizedBodyIndex> bodies(3);
std::vector<MobilizerUIndex> speeds(3);
bodies[0] = MobilizedBodyIndex(1);
bodies[1] = MobilizedBodyIndex(3);
bodies[2] = MobilizedBodyIndex(5);
speeds[0] = MobilizerUIndex(0);
speeds[1] = MobilizerUIndex(0);
speeds[2] = MobilizerUIndex(1);
Function* function = new CompoundFunction();
Constraint::SpeedCoupler coupler(matter, function, bodies, speeds);
State state;
createState(system, state);
// Make sure the constraint is satisfied.
Vector args(function->getArgumentSize());
for (int i = 0; i < args.size(); ++i)
args[i] = matter.getMobilizedBody(bodies[i]).getOneU(state, speeds[i]);
SimTK_TEST_EQ(0.0, function->calcValue(args));
// Simulate it and make sure the constraint is working correctly and
// energy is being conserved. This should be workless and power should
// always be zero (to the extent that the constraint is satisfied).
Real energy0 = system.calcEnergy(state);
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(1e-6);
integ.setReturnEveryInternalStep(true);
integ.initialize(state);
while (integ.getTime() < 10.0) {
integ.stepTo(10.0);
const State& istate = integ.getState();
system.realize(istate, Stage::Acceleration);
const Real energy = system.calcEnergy(istate);
const Real power = coupler.calcPower(istate);
for (int i = 0; i < args.size(); ++i)
args[i] = matter.getMobilizedBody(bodies[i]).getOneU(istate, speeds[i]);
SimTK_TEST_EQ_TOL(0.0, function->calcValue(args),
integ.getConstraintToleranceInUse());
SimTK_TEST_EQ_TOL(0.0, power, 10*integ.getConstraintToleranceInUse());
// Energy conservation depends on global integration accuracy;
// accuracy returned here is local so we'll fudge at 10X.
const Real etol = 10*integ.getAccuracyInUse()
*std::max(std::abs(energy), std::abs(energy0));
SimTK_TEST_EQ_TOL(energy0, energy, etol);
}
}
void testCoordinateCoupler3() {
// Create a system involving a constrained body for which qdot != u.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
createBallSystem(system);
MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
std::vector<MobilizedBodyIndex> bodies(3);
std::vector<MobilizerQIndex> coordinates(3);
bodies[0] = MobilizedBodyIndex(1);
bodies[1] = MobilizedBodyIndex(1);
bodies[2] = MobilizedBodyIndex(1);
coordinates[0] = MobilizerQIndex(0);
coordinates[1] = MobilizerQIndex(1);
coordinates[2] = MobilizerQIndex(2);
Function* function = new CompoundFunction();
Constraint::CoordinateCoupler coupler(matter, function, bodies, coordinates);
State state;
createState(system, state);
// Make sure the constraint is satisfied.
Vector args(function->getArgumentSize());
for (int i = 0; i < args.size(); ++i)
args[i] = matter.getMobilizedBody(bodies[i]).getOneQ(state, coordinates[i]);
SimTK_TEST_EQ(0.0, function->calcValue(args));
// Simulate it and make sure the constraint is working correctly and
// energy is being conserved.
const Real energy0 = system.calcEnergy(state);
RungeKuttaMersonIntegrator integ(system);
integ.setReturnEveryInternalStep(true);
integ.initialize(state);
while (integ.getTime() < 10.0) {
integ.stepTo(10.0);
const State& istate = integ.getState();
const Real energy = system.calcEnergy(istate);
for (int i = 0; i < args.size(); ++i)
args[i] = matter.getMobilizedBody(bodies[i])
.getOneQ(integ.getState(), coordinates[i]);
// Constraints are applied to unnormalized quaternions. When they are
// normalized, that can increase the constraint error. That is why we
// need the factor of 3 in the next line.
// TODO: Huh? (sherm)
SimTK_TEST_EQ_TOL(0.0, function->calcValue(args),
3*integ.getConstraintToleranceInUse());
// Energy conservation depends on global integration accuracy;
// accuracy returned here is local so we'll fudge at 10X.
const Real etol = 10*integ.getAccuracyInUse()
*std::max(std::abs(energy), std::abs(energy0));
SimTK_TEST_EQ_TOL(energy0, energy, etol);
}
}
void testPrescribedMotion1() {
// Create a system requiring simple linear motion of one Q. This
// may require that the constraint do work.
// (The way the cylinder system is structured it only takes work to
// keep body one at a uniform velocity; the rest are in free fall.)
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
createCylinderSystem(system);
MobilizedBodyIndex body = MobilizedBodyIndex(1);
MobilizerQIndex coordinate = MobilizerQIndex(1);
Vector coefficients(2);
coefficients[0] = 0.1;
coefficients[1] = 0.0;
Function* function = new Function::Linear(coefficients);
Constraint::PrescribedMotion constraint(matter, function, body, coordinate);
PowerMeasure<Real> powMeas(matter, constraint);
Measure::Zero zeroMeas(matter);
Measure::Integrate workMeas(matter, powMeas, zeroMeas);
State state;
createState(system, state);
workMeas.setValue(state, 0); // override createState
// Make sure the constraint is satisfied.
Vector args(1, state.getTime());
SimTK_TEST_EQ(function->calcValue(args),
matter.getMobilizedBody(body).getOneQ(state, coordinate));
// Simulate it and make sure the constraint is working correctly.
const Real energy0 = system.calcEnergy(state);
RungeKuttaMersonIntegrator integ(system);
integ.setReturnEveryInternalStep(true);
integ.initialize(state);
while (integ.getTime() < 10.0) {
integ.stepTo(10.0);
const State& istate = integ.getState();
system.realize(istate, Stage::Acceleration);
const Real energy = system.calcEnergy(istate);
const Real power = powMeas.getValue(istate);
const Real work = workMeas.getValue(istate);
Vector args(1, istate.getTime());
const Real q = matter.getMobilizedBody(body).getOneQ(istate, coordinate);
SimTK_TEST_EQ_TOL(function->calcValue(args), q,
integ.getConstraintToleranceInUse());
// Energy conservation depends on global integration accuracy;
// accuracy returned here is local so we'll fudge at 10X.
const Real etol = 10*integ.getAccuracyInUse()
*std::max(std::abs(energy-work), std::abs(energy0));
SimTK_TEST_EQ_TOL(energy0, energy-work, etol)
}
}
void testConstraintForces() {
// Weld one body to ground, push on it, verify that it reacts to match the load.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
MobilizedBody::Weld welded(matter.Ground(), Transform(),
body, Transform());
MobilizedBody::Free loose(matter.Ground(), Transform(),
body, Transform());
Constraint::Weld glue(matter.Ground(), Transform(),
loose, Transform());
// Apply forces to the body welded straight to ground.
Force::ConstantForce(forces, welded, Vec3(0,0,0), Vec3(1,2,3));
Force::ConstantTorque(forces, welded, Vec3(20,30,40));
// Apply the same forces to the "free" body which is welded by constraint.
Force::ConstantForce(forces, loose, Vec3(0,0,0), Vec3(1,2,3));
Force::ConstantTorque(forces, loose, Vec3(20,30,40));
State state = system.realizeTopology();
system.realize(state, Stage::Acceleration);
Vector_<SpatialVec> mobilizerReactions;
matter.calcMobilizerReactionForces(state, mobilizerReactions);
//NOT IMPLEMENTED YET:
//cout << "Weld constraint reaction on Ground: " << glue.getWeldReactionOnBody1(state) << endl;
//cout << "Weld constraint reaction on Body: " << glue.getWeldReactionOnBody2(state) << endl;
// Note that constraint forces have opposite sign to applied forces, because
// we calculate the multiplier lambda from M udot + ~G lambda = f_applied. We'll negate
// the calculated multipliers to turn these into applied forces.
const Vector mults = -state.getMultipliers();
Vector_<SpatialVec> constraintForces;
Vector mobilityForces;
matter.calcConstraintForcesFromMultipliers(state, mults,
constraintForces, mobilityForces);
MACHINE_TEST(constraintForces[loose.getMobilizedBodyIndex()],
mobilizerReactions[welded.getMobilizedBodyIndex()]);
// This returns just the forces on the weld's two bodies: in this
// case Ground and "loose", in that order.
Vector_<SpatialVec> glueForces =
glue.getConstrainedBodyForcesAsVector(state);
MACHINE_TEST(-glueForces[1], // watch the sign!
mobilizerReactions[welded.getMobilizedBodyIndex()]);
}
void testCalculationMethods() {
// Create a system with two bodies.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
MobilizedBody::Free b1(matter.Ground(), body);
MobilizedBody::Free b2(matter.Ground(), body);
// Set all the state variables to random values.
system.realizeTopology();
State state = system.getDefaultState();
Random::Gaussian random;
for (int i = 0; i < state.getNY(); ++i)
state.updY()[i] = random.getValue();
system.realize(state, Stage::Acceleration);
// Test the low level methods for transforming points and vectors.
const Vec3 point(0.5, 1, -1.5);
SimTK_TEST_EQ(b1.findStationLocationInGround(state, Vec3(0)), b1.getBodyOriginLocation(state));
SimTK_TEST_EQ(b1.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), point);
SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), b1.findStationLocationInAnotherBody(state, point, b2));
SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, Vec3(0))).norm(), (b1.getBodyOriginLocation(state)-b2.getBodyOriginLocation(state)).norm());
SimTK_TEST_EQ(b2.findMassCenterLocationInGround(state), b2.findStationLocationInGround(state, b2.getBodyMassCenterStation(state)));
SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, Vec3(0)), Vec3(0));
SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, point), b1.getBodyRotation(state)*point);
SimTK_TEST_EQ(b1.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), point);
SimTK_TEST_EQ(b2.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), b1.expressVectorInAnotherBodyFrame(state, point, b2));
// Test the routines for mapping locations, velocities, and accelerations.
Vec3 r, v, a;
b1.findStationLocationVelocityAndAccelerationInGround(state, point, r, v, a);
SimTK_TEST_EQ(v, b1.findStationVelocityInGround(state, point));
SimTK_TEST_EQ(a, b1.findStationAccelerationInGround(state, point));
{
Vec3 r2, v2;
b1.findStationLocationAndVelocityInGround(state, point, r2, v2);
SimTK_TEST_EQ(r, r2);
SimTK_TEST_EQ(v, v2);
}
SimTK_TEST_EQ(b1.findStationVelocityInGround(state, Vec3(0)), b1.getBodyOriginVelocity(state));
SimTK_TEST_EQ(b1.findStationAccelerationInGround(state, Vec3(0)), b1.getBodyOriginAcceleration(state));
SimTK_TEST_EQ(b1.findStationVelocityInGround(state, point), b1.findStationVelocityInAnotherBody(state, point, matter.Ground()));
}
void createState(MultibodySystem& system, State& state, const Vector& qOverride=Vector()) {
system.realizeTopology();
state = system.getDefaultState();
Random::Uniform random;
for (int i = 0; i < state.getNY(); ++i)
state.updY()[i] = random.getValue();
if (qOverride.size())
state.updQ() = qOverride;
system.realize(state, Stage::Velocity);
Vector dummy;
system.project(state, ConstraintTol);
system.realize(state, Stage::Acceleration);
}
//------------------------------------------------------------------------------
// 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 createState(MultibodySystem& system, State& state, const Vector& y=Vector()) {
system.realizeTopology();
state = system.getDefaultState();
if (y.size() > 0)
state.updY() = y;
else {
Random::Uniform random;
for (int i = 0; i < state.getNY(); ++i)
state.updY()[i] = random.getValue();
}
system.realize(state, Stage::Velocity);
// Solve to tight tolerance here
system.project(state, 1e-12);
system.realize(state, Stage::Acceleration);
}
int main() {
try { // catch errors if any
// Create the system, with subsystems for the bodies and some forces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.8);
// Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
Real mass = 3; Vec3 pos(0,-3,0);
Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));
// Create the tree of mobilized bodies, reusing the above body description.
MobilizedBody::Pin bodyT (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0), bodyInfo, Vec3(0));
MobilizedBody::Pin rtArm (bodyT, Vec3(2, 0, 0), bodyInfo, Vec3(0));
// Add a joint stop to the left arm restricting it to q in [0,Pi/5].
Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0),
10000, // stiffness
0.5, // dissipation coefficient
0*Pi, // lower stop
Pi/5); // upper stop
// Ask for visualization every 1/30 second.
system.setUseUniformBackground(true); // turn off floor
system.addEventReporter(new Visualizer::Reporter(system, 1./30));
// Initialize the system and state.
State state = system.realizeTopology();
leftArm.setAngle(state, Pi/5);
// Simulate for 10 seconds.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(10);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
void testWeld() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0));
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
// Create two pendulums, each with two welded bodies. One uses a Weld MobilizedBody,
// and the other uses a Weld constraint.
Transform inboard(Vec3(0.1, 0.5, -1));
Transform outboard(Vec3(0.2, -0.2, 0));
MobilizedBody::Ball p1(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
MobilizedBody::Ball p2(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
MobilizedBody::Weld c1(p1, inboard, body, outboard);
MobilizedBody::Free c2(p2, inboard, body, outboard);
Constraint::Weld constraint(p2, inboard, c2, outboard);
// It is not a general test unless the Weld mobilizer has children!
MobilizedBody::Pin wchild1(c1, inboard, body, outboard);
MobilizedBody::Pin wchild2(c2, inboard, body, outboard);
Force::MobilityLinearSpring(forces, wchild1, 0, 1000, 0);
Force::MobilityLinearSpring(forces, wchild2, 0, 1000, 0);
State state = system.realizeTopology();
p1.setU(state, Vec3(1, 2, 3));
p2.setU(state, Vec3(1, 2, 3));
system.realize(state, Stage::Velocity);
system.project(state, 1e-10);
SimTK_TEST_EQ(c1.getBodyTransform(state), c2.getBodyTransform(state));
SimTK_TEST_EQ(c1.getBodyVelocity(state), c2.getBodyVelocity(state));
// Simulate it and see if both pendulums behave identically.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(5);
system.realize(integ.getState(), Stage::Velocity);
SimTK_TEST_EQ_TOL(c1.getBodyTransform(integ.getState()),
c2.getBodyTransform(integ.getState()), 1e-10);
SimTK_TEST_EQ_TOL(c1.getBodyVelocity(integ.getState()),
c2.getBodyVelocity(integ.getState()), 1e-10);
}
int main() {
try
{ // Create the system.
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)),
pendulumBody, Transform(Vec3(0, 1, 0)));
//Motion::Steady(pendulum, 1);
Visualizer viz(system);
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
pendulum.setOneU(state, 0, 1.0);
// Simulate it.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(100.0);
} 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;
}
void createPlanarSystem(MultibodySystem& system) {
SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0), 0);
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
for (int i = 0; i < NUM_BODIES; ++i) {
MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
MobilizedBody::Planar b(parent, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
}
}
// Component interface implementation
void extendAddToSystem(MultibodySystem& system) const override {
if (system.hasMatterSubsystem()){
matter = system.updMatterSubsystem();
}
else{
// const Sub& subc = getMemberSubcomponent<Sub>(intSubix);
SimbodyMatterSubsystem* old_matter = matter.release();
delete old_matter;
matter = new SimbodyMatterSubsystem(system);
GeneralForceSubsystem* old_forces = forces.release();
delete old_forces;
forces = new GeneralForceSubsystem(system);
SimTK::Force::UniformGravity gravity(*forces, *matter, Vec3(0, -9.816, 0));
fix = gravity.getForceIndex();
system.updMatterSubsystem().setShowDefaultGeometry(true);
}
}
bool testFitting
(const MultibodySystem& mbs, State& state,
const vector<MobilizedBodyIndex>& bodyIxs,
const vector<vector<Vec3> >& stations,
const vector<vector<Vec3> >& targetLocations,
Real minError, Real maxError, Real endDistance)
{
// Find the best fit.
Real reportedError = ObservedPointFitter::findBestFit(mbs, state, bodyIxs, stations, targetLocations, TOL);
cout << "[min,max]=" << minError << "," << maxError << " actual=" << reportedError << endl;
bool result = (reportedError <= maxError && reportedError >= minError);
// Verify that the error was calculated correctly.
Real error = 0.0;
int numStations = 0;
mbs.realize(state, Stage::Position);
const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
for (int i = 0; i < (int) bodyIxs.size(); ++i) {
MobilizedBodyIndex id = bodyIxs[i];
numStations += (int)stations[i].size();
for (int j = 0; j < (int) stations[i].size(); ++j)
error += (targetLocations[i][j]-matter.getMobilizedBody(id).getBodyTransform(state)*stations[i][j]).normSqr();
}
error = std::sqrt(error/numStations);
cout << "calc wrms=" << error << endl;
ASSERT(std::abs(1.0-error/reportedError) < 0.0001); // should match to machine precision
if (endDistance >= 0) {
// Verify that the ends are the correct distance apart.
Real distance = (matter.getMobilizedBody(bodyIxs[0]).getBodyOriginLocation(state)-matter.getMobilizedBody(bodyIxs[bodyIxs.size()-1]).getBodyOriginLocation(state)).norm();
cout << "required dist=" << endDistance << ", actual=" << distance << endl;
ASSERT(std::abs(1.0-endDistance/distance) < TOL);
}
return result;
}
void createCylinderSystem(MultibodySystem& system) {
SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
GeneralForceSubsystem forces(system);
// Skew gravity so moving takes work.
Force::UniformGravity gravity(forces, matter, Vec3(0, -2, -3));
for (int i = 0; i < NUM_BODIES; ++i) {
MobilizedBody& parent =
matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
const Real mass = 1 + 0.1*i;
Body::Rigid body(MassProperties(mass, Vec3(0), mass*UnitInertia(1)));
MobilizedBody::Cylinder b(parent, Transform(Vec3(.1,.2,.3)),
body, Transform(Vec3(BOND_LENGTH, 0, 0)));
}
}
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);
}
void testCompositeInertia() {
MultibodySystem mbs;
SimbodyMatterSubsystem pend(mbs);
Body::Rigid pointMass(MassProperties(3, Vec3(0), Inertia(0)));
// Point mass at x=1.5 rotating about (0,0,0).
MobilizedBody::Pin
body1( pend.Ground(), Transform(),
pointMass, Vec3(1.5,0,0));
const MobilizedBodyIndex body1x = body1.getMobilizedBodyIndex();
// A second body 2 units further along x, rotating about the
// first point mass origin.
MobilizedBody::Pin
body2( body1, Transform(),
pointMass, Vec3(2,0,0));
const MobilizedBodyIndex body2x = body2.getMobilizedBodyIndex();
State state = mbs.realizeTopology();
mbs.realize(state, Stage::Position);
Array_<SpatialInertia, MobilizedBodyIndex> R(pend.getNumBodies());
pend.calcCompositeBodyInertias(state, R);
// Calculate expected inertias about the joint axes.
Real expInertia2 = body2.getBodyMassProperties(state).getMass()*square(2);
Real expInertia1 = body1.getBodyMassProperties(state).getMass()*square(1.5)
+ body2.getBodyMassProperties(state).getMass()*square(3.5);
// Should be able to recover these inertias by projecting the composite
// body inertias onto the joint axes using H matrices.
const SpatialVec H1 = body1.getHCol(state, MobilizerUIndex(0));
const SpatialVec H2 = body2.getHCol(state, MobilizerUIndex(0));
SimTK_TEST_EQ(~H2*(R[body2x]*H2), expInertia2);
SimTK_TEST_EQ(~H1*(R[body1x]*H1), expInertia1);
// This should force realization of the composite body inertias.
SpatialInertia cbi = pend.getCompositeBodyInertia(state, body1);
body2.setAngle(state, Pi/4);
// This is not allowed until Position stage.
SimTK_TEST_MUST_THROW(pend.getCompositeBodyInertia(state, body1));
mbs.realize(state, Stage::Position);
// Now it should be OK.
cbi = pend.getCompositeBodyInertia(state, body1);
mbs.realize(state, Stage::Acceleration);
//cout << "udots=" << state.getUDot() << endl;
body1.setRate(state, 27);
mbs.realize(state, Stage::Acceleration);
//cout << "udots=" << state.getUDot() << endl;
}
int main() {
try
{ // Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
/// uncoment gravity to get some sort of collision interaction
/// for cylinder mesh
// Force::UniformGravity gravity(forces, matter,Vec3(0,0.001,0), 2);
ContactTrackerSubsystem tracker(system);
//GeneralContactSubsystem contactsys(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
for(SubsystemIndex i(0); i<system.getNumSubsystems(); ++i)
{
fprintf(stderr,"subsytem name %d %s\n", (int)i,
system.getSubsystem((SubsystemIndex)i).getName().c_str());
}
const Real rad = .4;
PolygonalMesh pyramidMesh1,pyramidMesh2;
/// load cylinder forces drawn, but interaction depends on gravity???
const Real fFac =1; // to turn off friction
const Real fDis = .5*0.2; // to turn off dissipation
const Real fVis = .1*.1; // to turn off viscous friction
const Real fK = 100*1e6; // pascals
Body::Rigid pendulumBody3(MassProperties(100.0, Vec3(0), 100*Inertia(1)));
PolygonalMesh body3contact = PolygonalMesh::createSphereMesh(rad, 2);
ContactGeometry::TriangleMesh geo3(body3contact);
const DecorativeMesh mesh3(geo3.createPolygonalMesh());
pendulumBody3.addDecoration(Transform(),
DecorativeMesh(mesh3).setOpacity(.2));
pendulumBody3.addDecoration(Transform(),
DecorativeMesh(mesh3).setColor(Gray)
.setRepresentation(DecorativeGeometry::DrawWireframe)
.setOpacity(.1));
ContactSurface s1(geo3,
ContactMaterial(fK*.1,fDis*.9,fFac*.8,fFac*.7,fVis*10));
s1.setThickness(1);
s1.setShape(geo3);
//ContactGeometry::Sphere geo3(rad);
pendulumBody3.addContactSurface(Transform(),s1);
/*
std::ifstream meshFile1,meshFile2;
meshFile1.open("cyl3.obj");
pyramidMesh1.loadObjFile(meshFile1); meshFile1.close();
*/
pyramidMesh1 = PolygonalMesh::createSphereMesh(rad, 2);
ContactGeometry::TriangleMesh pyramid1(pyramidMesh1);
DecorativeMesh showPyramid1(pyramid1.createPolygonalMesh());
const Real ballMass = 200;
Body::Rigid ballBody(MassProperties(ballMass, Vec3(0),
ballMass*UnitInertia::sphere(1)));
ballBody.addDecoration(Transform(),
showPyramid1.setColor(Cyan).setOpacity(.2));
ballBody.addDecoration(Transform(),
showPyramid1.setColor(Gray)
.setRepresentation(DecorativeGeometry::DrawWireframe));
ContactSurface s2(pyramid1,
ContactMaterial(fK*.1,fDis*.9,
.1*fFac*.8,.1*fFac*.7,fVis*1));
s2.setThickness(1);
s2.setShape(pyramid1);
ballBody.addContactSurface(Transform(),/*ContactSurface(ContactGeometry::Sphere(rad),ContactMaterial(fK*.1,fDis*.9,
.1*fFac*.8,.1*fFac*.7,fVis*1))*/ s2/*.joinClique(clique1)*/);
/* Body::Rigid d(MassProperties(1.0, Vec3(0),Inertia(1)));
MobilizedBody::Pin dud(matter.Ground(),Transform(),d,Transform());
*/
MobilizedBody::Free ball(matter.Ground(), Transform(Vec3(-2,-2,0)),
ballBody, Transform(Vec3(0)));
MobilizedBody::Free ball1(matter.Ground(), Transform(Vec3(0,0,0)),
ballBody, Transform(Vec3(0)));
/*
MobilizedBody::Free ball2(matter.Ground(), Transform(Vec3(-4,0,0)),
ballBody, Transform(Vec3(0)));
*/
MobilizedBody::Free ball3(matter.Ground(), Transform(Vec3(-1,-2,0)),
ballBody, Transform(Vec3(0)));
MobilizedBody::Pin pendulum3(matter.Ground(), Transform(Vec3(-2,0,0)),
pendulumBody3, Transform(Vec3(0, 2, 0)));
ball.updBody();
ball1.updBody();
Visualizer viz(system);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
viz.setMode(Visualizer::RealTime);
viz.setDesiredBufferLengthInSec(1);
viz.setDesiredFrameRate(FrameRate);
viz.setGroundHeight(-3);
viz.setShowShadows(true);
viz.setBackgroundType(Visualizer::SolidColor);
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));
// system.addEventHandler(new TriggeredEventHandler(Stage::Model));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
/*
ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
Vec3(0,-1.8,0)));
*/
//pendulum.setOneQ(state, 0, -Pi/12);
pendulum3.setOneQ(state, 0, -Pi/2);
pendulum3.setOneU(state, 0, Pi/4);
// ball.setOneU(state, 1, 0.1);
viz.report(state);
matter.updAllParticleVelocities(state);
printf("Default state\n");
/* cout << "t=" << state.getTime()
<< " q=" << pendulum.getQAsVector(state) << pendulum2.getQAsVector(state)
<< " u=" << pendulum.getUAsVector(state) << pendulum2.getUAsVector(state)
<< endl;
*/
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.
// The system as parameterized is very stiff (mostly due to friction)
// and thus runs best with CPodes which is extremely stable for
// stiff problems. To get reasonable performance out of the explicit
// integrators (like the RKs) you'll have to run at a very loose
// accuracy like 0.1, or reduce the friction coefficients and
// maybe the stiffnesses.
//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-1);
//integ.setAllowInterpolation(false);
integ.setAccuracy(1e-3); // minimum for CPodes
//integ.setAccuracy(.1);
TimeStepper ts(system, integ);
ts.initialize(state);
double cpuStart = cpuTime();
double realStart = realTime();
ts.stepTo(2000.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;
}
int main() {
try
{ // Create the system.
MultibodySystem system;
system.setUpDirection(ZAxis);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -ZAxis, 9.81);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
contactForces.setTransitionVelocity(1e-3);
const Vec3 hdim(.2,.3,.4); // Brick half dimensions
const Real rad = .1; // Contact sphere radius
const Real brickMass = 2;
#ifdef USE_SHERM_PARAMETERS
const Real mu_d =.3; // dynamic friction
const Real mu_s =.3; // static friction
const Real mu_v = 0; // viscous friction (1/v)
const Real dissipation = .1;
const Real fK = 1e6; // stiffness in pascals
const Real simDuration = 5.;
#endif
#ifdef USE_TOM_PARAMETERS
const Real mu_d =.3; // dynamic friction
const Real mu_s =.3; // static friction
const Real mu_v = 0; // viscous friction (1/v)
const Real dissipation = .1756; //Second impact at 0.685 s.
const Real fK = 1e6; // stiffness in pascals
const Real simDuration = 0.5; //3.0; //0.8;
#endif
const ContactMaterial material(fK,dissipation,mu_s,mu_d,mu_v);
// Halfspace floor
const Rotation R_xdown(Pi/2,YAxis);
matter.Ground().updBody().addContactSurface(
Transform(R_xdown, Vec3(0,0,0)),
ContactSurface(ContactGeometry::HalfSpace(), material));
Body::Rigid brickBody(MassProperties(brickMass, Vec3(0),
UnitInertia::brick(hdim)));
brickBody.addDecoration(Transform(),
DecorativeBrick(hdim).setColor(BrickColor).setOpacity(.7));
for (int i=-1; i<=1; i+=2)
for (int j=-1; j<=1; j+=2)
for (int k=-1; k<=1; k+=2) {
const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(hdim);
brickBody.addContactSurface(pt,
ContactSurface(ContactGeometry::Sphere(rad), material));
brickBody.addDecoration(pt,
DecorativeSphere(rad).setColor(SphereColor));
}
MobilizedBody::Free brick(matter.Ground(), Transform(),
brickBody, Transform());
Visualizer viz(system);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces,
brick));
//viz.addFrameController(new BodyWatcher(brick));
viz.addFrameController(new BodyWatcher(matter.Ground()));
//viz.setShowSimTime(true);
//viz.setShowFrameNumber(true);
viz.setDesiredFrameRate(FrameRate);
//viz.setShowFrameRate(true);
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);
// 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();
// SET INITIAL CONDITIONS
#ifdef USE_SHERM_PARAMETERS
brick.setQToFitTranslation(state, Vec3(0,2,.8));
brick.setQToFitRotation(state, Rotation(BodyRotationSequence,
Pi/4, XAxis, Pi/6, YAxis));
brick.setUToFitLinearVelocity(state, Vec3(-5,0,0));
#endif
#ifdef USE_TOM_PARAMETERS
Vector initQ = Vector(Vec7(1,0,0,0, 0,1,0.8));
initQ(0,4) = Vector(Quaternion(Rotation(SimTK::Pi/4, XAxis) *
Rotation(SimTK::Pi/6, YAxis))
.asVec4());
Vector initU = Vector(Vec6(0,0,0, 0,0,6));
initQ[6] = 1.5;
initU[5] = -3.96; //First impact at 0.181 s.
initU[3] = -5.0;
state.setQ(initQ);
state.setU(initU);
#endif
saveEm.reserve(10000);
viz.report(state);
printf("Default state\n");
cout << "t=" << state.getTime()
<< " q=" << brick.getQAsVector(state)
<< " u=" << brick.getUAsVector(state)
<< endl;
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.
// The system as parameterized is very stiff (mostly due to friction)
// and thus runs best with CPodes which is extremely stable for
// stiff problems. To get reasonable performance out of the explicit
// integrators (like the RKs) you'll have to run at a very loose
// accuracy like 0.1, or reduce the friction coefficients and
// maybe the stiffnesses.
//SemiExplicitEuler2Integrator integ(system);
//CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
RungeKuttaMersonIntegrator integ(system);
integ.setReturnEveryInternalStep(true);
integ.setAllowInterpolation(false);
//RungeKutta3Integrator integ(system);
//VerletIntegrator integ(system);
//integ.setMaximumStepSize(1e-0001);
//integ.setAccuracy(1e-3); // minimum for CPodes
integ.setAccuracy(1e-5);
//integ.setAccuracy(.01);
integ.initialize(state);
double cpuStart = cpuTime();
double realStart = realTime();
Real lastReport = -Infinity;
while (integ.getTime() < simDuration) {
// Advance time by no more than ReportInterval. Might require multiple
// internal steps.
integ.stepBy(ReportInterval);
if (integ.getTime() >= lastReport + VizReportInterval) {
// The state being used by the integrator.
const State& s = integ.getState();
viz.report(s);
saveEm.push_back(s); // save state for playback
lastReport = s.getTime();
}
}
const double timeInSec = realTime() - realStart;
const int evals = integ.getNumRealizations();
cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
timeInSec << "s elapsed for " << integ.getTime() << "s sim (avg step="
<< (1000*integ.getTime())/integ.getNumStepsTaken() << "ms) "
<< (1000*integ.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, 2, 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;
}
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);
}
}
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
}
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);
}
}
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;
}
HuntCrossleyContact::HuntCrossleyContact(MultibodySystem& mbs)
: ForceSubsystem()
{
adoptSubsystemGuts(new HuntCrossleyContactRep());
mbs.addForceSubsystem(*this); // steal ownership
}
GeneralForceSubsystem::GeneralForceSubsystem(MultibodySystem& mbs)
: ForceSubsystem()
{ adoptSubsystemGuts(new GeneralForceSubsystemRep());
mbs.addForceSubsystem(*this); } // steal ownership
CableTrackerSubsystem::CableTrackerSubsystem(MultibodySystem& mbs)
{ adoptSubsystemGuts(new Impl());
mbs.adoptSubsystem(*this); } // steal ownership
int main() {
try {
// setup test problem
double r = .5;
double uP = -Pi/2;
double vP = Pi/3;
double uQ = 0;
double vQ = 2;
Vec3 O(-r, -r, 0.2);
Vec3 I(r, r, -r);
Vec3 P(r*cos(uP)*sin(vP), r*sin(uP)*sin(vP), r*cos(vP));
Vec3 Q(r*cos(uQ)*sin(vQ), r*sin(uQ)*sin(vQ), r*cos(vQ));
Vec3 r_OP = P-O;
Vec3 r_IQ = Q-I;
Vec3 tP = r_OP.normalize();
Vec3 tQ = r_IQ.normalize();
int n = 6; // problem size
Vector x(n), dx(n), Fx(n), xold(n);
Matrix J(n,n);
ContactGeometry::Sphere geom(r);
// r = 2;
// Vec3 radii(1,2,3);
// ContactGeometry::Ellipsoid geom(radii);
Geodesic geod;
// Create a dummy MultibodySystem for visualization purposes
MultibodySystem dummySystem;
SimbodyMatterSubsystem matter(dummySystem);
matter.updGround().addBodyDecoration(Transform(), geom.createDecorativeGeometry()
.setColor(Gray)
.setOpacity(0.5)
.setResolution(5));
// Visualize with default options; ask for a report every 1/30 of a second
// to match the Visualizer's default 30 frames per second rate.
Visualizer viz(dummySystem);
viz.setBackgroundType(Visualizer::SolidColor);
dummySystem.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// add vizualization callbacks for geodesics, contact points, etc.
viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodP(), Red));
viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodQ(), Blue));
viz.addDecorationGenerator(new GeodesicDecorator(geod, Orange));
viz.addDecorationGenerator(new PlaneDecorator(geom.getPlane(), Gray));
viz.addDecorationGenerator(new PathDecorator(x, O, I, Green));
dummySystem.realizeTopology();
State dummyState = dummySystem.getDefaultState();
// calculate the geodesic
geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, vizInterval));
viz.report(dummyState);
// creat path error function
//PathError pathErrorFnc(n, n, geom, geod, O, I);
PathErrorSplit pathErrorFnc(n, n, geom, geod, O, I);
pathErrorFnc.setEstimatedAccuracy(estimatedPathErrorAccuracy);
Differentiator diff(pathErrorFnc);
// set initial conditions
x[0]=P[0]; x[1]=P[1]; x[2]=P[2];
x[3]=Q[0]; x[4]=Q[1]; x[5]=Q[2];
Real f, fold, lam;
pathErrorFnc.f(x, Fx);
viz.report(dummyState);
sleepInSec(pauseBetweenPathIterations);
f = std::sqrt(~Fx*Fx);
for (int i = 0; i < maxNewtonIterations; ++i) {
if (f < ftol) {
std::cout << "path converged in " << i << " iterations" << std::endl;
// cout << "obstacle err = " << Fx << ", x = " << x << endl;
break;
}
diff.calcJacobian(x, Fx, J, Differentiator::ForwardDifference);
dx = J.invert()*Fx;
fold = f;
xold = x;
// backtracking
lam = 1;
while (true) {
x = xold - lam*dx;
cout << "TRY stepsz=" << lam << " sz*dx=" << lam*dx << endl;
pathErrorFnc.f(x, Fx);
f = std::sqrt(~Fx*Fx);
if (f > fold && lam > minlam) {
lam = lam / 2;
} else {
break;
}
}
if (maxabsdiff(x,xold) < xtol) {
std::cout << "converged on step size after " << i << " iterations" << std::endl;
std::cout << "error = " << Fx << std::endl;
break;
}
viz.report(dummyState);
sleepInSec(pauseBetweenPathIterations);
}
cout << "obstacle error = " << Fx << endl;
cout << "num geodP pts = " << geom.getGeodP().getNumPoints() << endl;
} 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;
}
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);
}
}
