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);
}
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 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;
}
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);
}
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 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 testConservationOfEnergy() {
// Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
const Real Mass = 1;
const Vec3 HalfShape = Vec3(1,.5,.25)/2;
const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
//Body::Rigid brickBody(MassProperties(Mass, Vec3(0),
// Mass*UnitInertia::ellipsoid(HalfShape)));
brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
.setOpacity(0.25)
.setColor(Blue));
brickBody.addDecoration(BodyAttach,
DecorativeFrame(0.5).setColor(Red));
const int NBod=50;
MobilizedBody::Free brick1(matter.Ground(), Transform(),
brickBody, BodyAttach);
MobilizedBody::Free brick2(brick1, Transform(),
brickBody, BodyAttach);
MobilizedBody prev=brick2;
MobilizedBody body25;
for (int i=0; i<NBod; ++i) {
MobilizedBody::Ball next(prev, -1*BodyAttach.p(),
brickBody, BodyAttach);
if (i==25) body25=next;
//Force::TwoPointLinearSpring(forces,
// prev, Vec3(0), next, Vec3(0), 1000, 1);
prev=next;
}
Constraint::Ball(matter.Ground(), Vec3(0,1,0)-2*NBod/3*BodyAttach.p(),
prev, BodyAttach.p());
Constraint::Ball(matter.Ground(), Vec3(0,1,0)-1*NBod/3*BodyAttach.p(),
body25, BodyAttach.p());
Vec6 k1(1,100,1,100,100,100), c1(0);
Force::LinearBushing(forces, matter.Ground(), -2*NBod/3*BodyAttach.p(),
prev, BodyAttach.p(), k1, c1);
matter.Ground().addBodyDecoration(-2*NBod/3*BodyAttach.p(),
DecorativeFrame().setColor(Green));
Force::Thermostat thermo(forces, matter,
SimTK_BOLTZMANN_CONSTANT_MD,
5000,
.1);
Vec6 k(1,100,1,100,100,100), c(0);
Force::LinearBushing bushing1(forces, matter.Ground(), -1*NBod/3*BodyAttach.p(),
brick1, BodyAttach, k, c);
Force::LinearBushing bushing2(forces, brick1, Transform(),
brick2, BodyAttach, k, c);
matter.Ground().addBodyDecoration(-1*NBod/3*BodyAttach.p(),
DecorativeFrame().setColor(Green));
Visualizer viz(system);
Visualizer::Reporter* reporter = new Visualizer::Reporter(viz, 1./30);
viz.setBackgroundType(Visualizer::SolidColor);
system.addEventReporter(reporter);
ThermoReporter* thermoReport = new ThermoReporter
(system, thermo, bushing1, bushing2, 1./10);
system.addEventReporter(thermoReport);
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
viz.report(state);
printf("Default state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << brick1.getQAsVector(state) << brick2.getQAsVector(state)
<< " u=" << brick1.getUAsVector(state) << brick2.getUAsVector(state)
<< "\nnChains=" << thermo.getNumChains(state)
<< " T=" << thermo.getBathTemperature(state)
<< "\nt_relax=" << thermo.getRelaxationTime(state)
<< " kB=" << thermo.getBoltzmannsConstant()
<< endl;
getchar();
state.setTime(0);
system.realize(state, Stage::Acceleration);
Vector initU(state.getNU());
initU = Test::randVector(state.getNU());
state.updU()=initU;
RungeKuttaMersonIntegrator integ(system);
//integ.setMinimumStepSize(1e-1);
integ.setAccuracy(1e-2);
TimeStepper ts(system, integ);
ts.initialize(state);
const State& istate = integ.getState();
viz.report(integ.getState());
viz.zoomCameraToShowAllGeometry();
printf("After initialize -- hit ENTER\n");
cout << "t=" << integ.getTime()
<< "\nE=" << system.calcEnergy(istate)
<< "\nEbath=" << thermo.calcBathEnergy(istate)
<< endl;
thermoReport->handleEvent(istate);
getchar();
// Simulate it.
ts.stepTo(20.0);
viz.report(integ.getState());
viz.zoomCameraToShowAllGeometry();
printf("After simulation:\n");
cout << "t=" << integ.getTime()
<< "\nE=" << system.calcEnergy(istate)
<< "\nEbath=" << thermo.calcBathEnergy(istate)
<< "\nNsteps=" << integ.getNumStepsTaken()
<< endl;
thermoReport->handleEvent(istate);
}
//==============================================================================
// MAIN
//==============================================================================
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);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contact(system, tracker);
contact.setTransitionVelocity(transitionVelocity);
Force::Gravity(forces, matter, -YAxis, 9.81);
// Set up visualization and ask for a frame every 1/30 second.
Visualizer viz(system);
viz.setShowSimTime(true); viz.setShowFrameRate(true);
viz.addSlider("Speed", SpeedControlSlider, -10, 10, 0);
Visualizer::InputSilo* silo = new Visualizer::InputSilo();
viz.addInputListener(silo);
#ifdef ANIMATE
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
#endif
DecorativeText help("Any input to start; ESC to quit");
help.setIsScreenText(true);
viz.addDecoration(MobilizedBodyIndex(0),Vec3(0),help);
matter.setShowDefaultGeometry(false);
// Add the Ground contact geometry. Contact half space has -XAxis normal
// (right hand wall) so we have to rotate.
MobilizedBody& Ground = matter.updGround(); // Nicer name for Ground.
Ground.updBody().addContactSurface(Transform(YtoX,Vec3(0)),
ContactSurface(ContactGeometry::HalfSpace(),concrete));
// Add some speed bumps.
const int NBumps = 2; const Vec3 BumpShape(.8,0.2,2);
for (int i=0; i < NBumps; ++i) {
const Real x = -2*(i+1);
Ground.updBody().addContactSurface(Vec3(x,0,0),
ContactSurface(ContactGeometry::Ellipsoid(BumpShape), rubber));
Ground.updBody().addDecoration(Vec3(x,0,0),
DecorativeEllipsoid(BumpShape).setColor(Gray).setResolution(3));
}
// TORSO
const Real TorsoHeight = 1.1;
const Vec3 torsoHDims(1,.08,.8);
const Real torsoVolume = 8*torsoHDims[0]*torsoHDims[1]*torsoHDims[2];
const Real torsoMass = torsoVolume*rubber_density/10;
const Vec3 torsoCOM(0,-.75,0); // put it low for stability
Body::Rigid torsoInfo(MassProperties(torsoMass,torsoCOM,
UnitInertia::brick(torsoHDims).shiftFromCentroidInPlace(-torsoCOM)));
torsoInfo.addDecoration(Vec3(0),
DecorativeBrick(torsoHDims).setColor(Cyan));
// CRANK
const Vec3 crankCenter(0,0,0); // in torso frame
const Vec3 crankOffset(0,0,torsoHDims[2]+LinkDepth); // left/right offset
const Real MLen=15/100.; // crank radius
Body::Rigid crankInfo(MassProperties(.1,Vec3(0),
UnitInertia::cylinderAlongZ(MLen, LinkDepth)));
crankInfo.addDecoration(Vec3(0),
DecorativeBrick(Vec3(LinkWidth,LinkWidth,torsoHDims[2]))
.setColor(Black));
const Vec3 CrankConnect(MLen,0,0); // in crank frame
// Add the torso and crank mobilized bodies.
MobilizedBody::Free torso(Ground,Vec3(0,TorsoHeight,0), torsoInfo,Vec3(0));
MobilizedBody::Pin crank(torso,crankCenter, crankInfo, Vec3(0));
// Add the legs.
for (int i=-1; i<=1; ++i) {
const Vec3 offset = crankCenter + i*crankOffset;
const Vec3 linkSpace(0,0,2*LinkDepth);
const Rotation R_CP(i*2*Pi/3,ZAxis);
// Add crank bars for looks.
crank.addBodyDecoration(
Transform(R_CP, offset+1.5*MLen/2*R_CP.x()+(i==0?linkSpace:Vec3(0))),
DecorativeBrick(Vec3(1.5*MLen/2,LinkWidth,LinkDepth))
.setColor(Yellow));
addOneLeg(viz, torso, offset + i*linkSpace,
crank, R_CP*CrankConnect);
addOneLeg(viz, torso, Transform(Rotation(Pi,YAxis), offset + i*linkSpace),
crank, R_CP*CrankConnect);
}
// Add speed control.
Motion::Steady motor(crank, 0); // User controls speed.
system.addEventHandler
(new UserInputHandler(*silo, motor, Real(0.1))); //check input every 100ms
// Initialize the system and state.
State state = system.realizeTopology();
system.realize(state);
printf("Theo Jansen Strandbeest in 3D:\n");
printf("%d bodies, %d mobilities, -%d constraint equations -%d motions\n",
matter.getNumBodies(), state.getNU(), state.getNMultipliers(),
matter.getMotionMultipliers(state).size());
viz.report(state);
printf("Hit any key to assemble ...");
silo->waitForAnyUserInput(); silo->clear();
Assembler(system).assemble(state);
printf("ASSEMBLED\n");
// Simulate.
SemiExplicitEuler2Integrator integ(system);
integ.setAccuracy(0.1);
integ.setConstraintTolerance(.001);
integ.setMaximumStepSize(1./60);
TimeStepper ts(system, integ);
ts.initialize(state);
viz.report(ts.getState());
printf("Hit ENTER to simulate ... (ESC to quit)\n");
silo->waitForAnyUserInput(); silo->clear();
const double startCPU = cpuTime(), startTime = realTime();
ts.stepTo(Infinity); // RUN
dumpIntegratorStats(startCPU, startTime, integ);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
int main() {
try {
// Create geometry
Real r = 0.5;
//ContactGeometry::Sphere geom(r);
// ContactGeometry::Cylinder geom(r);
ContactGeometry::Torus geom(2*r, r);
Vec3 radii(0.2,0.4,0.6);
//ContactGeometry::Ellipsoid geom(radii);
Real startLength = 0.5;
//startLength=5;
Real phiP = 0.0*Pi;
Real thetaP = 0.0*Pi;
Real phiQ = 0.0*Pi;
Real thetaQ = 1.2*Pi;
Real heightP = 0.5;
Real heightQ = -0.5;
Vec3 P(r*sin(thetaP)*cos(phiP), r*sin(thetaP)*sin(phiP), r*cos(thetaP));
Vec3 Q(r*sin(thetaQ)*cos(phiQ), r*sin(thetaQ)*sin(phiQ), r*cos(thetaQ));
Vec3 O(-2, 0, heightP);
Vec3 I(-2, 0, heightQ);
// move points off surface for testing
Q(0) -= r/2;
Q(1) -= -r*0.5;
P(1) -= r*0.5;
P(0) -= r/2;
//Q=P+Vec3(1.25,-1,0); P+=Vec3(-1,-.9,0);
//Q=P+Vec3(1,-1,-1.5); P+=Vec3(-1,-.9,0);
// project back to surface for testing
Vec3 tmpPt;
tmpPt = geom.projectDownhillToNearestPoint(P);
P = tmpPt;
tmpPt = geom.projectDownhillToNearestPoint(Q);
Q = tmpPt;
Vec3 r_OP = P - O;
Vec3 r_IQ = Q - I;
UnitVec3 e_OP(r_OP);
UnitVec3 e_IQ(r_IQ);
Vec3 r_PQ = Q - P;
int n = 2; // problem size
Vector x(n), dx(n), Fx(n), xold(n);
Matrix J(n, n);
bool inside; UnitVec3 nP, nQ;
cout << "before P,Q=" << P << ", " << Q << " -- "
<< geom.calcSurfaceValue(P) << " " << geom.calcSurfaceValue(Q) << endl;
Vec3 newP = geom.findNearestPoint(P,inside,nP);
UnitVec3 tP = nP.perp();
Vec3 newQ = geom.findNearestPoint(Q,inside,nQ);
UnitVec3 tQ = nQ.perp();
cout << "after newP,Q=" << newP << ", " << newQ << " -- "
<< geom.calcSurfaceValue(newP)
<< " " << geom.calcSurfaceValue(newQ) << endl;
cout << "curvature at newP along " << tP << ": "
<< geom.calcSurfaceCurvatureInDirection(newP,tP) << "\n";
cout << "curvature at newQ along " << tQ << ": "
<< geom.calcSurfaceCurvatureInDirection(newQ,tQ) << "\n";
cout << "gradient at newP " << ": "
<< geom.calcSurfaceGradient(newP) << " |gP|=" <<
geom.calcSurfaceGradient(newP).norm() << "\n";
cout << "gradient at newQ " << ": "
<< geom.calcSurfaceGradient(newQ) << " |gQ|=" <<
geom.calcSurfaceGradient(newQ).norm() << "\n";
Rotation R_GP(nP, ZAxis, tP, XAxis);
for (int i=0; i <=10; ++i) {
Real a = i*(Pi/2)/10;
UnitVec3 u_P(-sin(a), cos(a), 0);
UnitVec3 dir = R_GP*u_P;
cout << a << ": " << geom.calcSurfaceCurvatureInDirection(newP,dir)
<< " 2*sin^2(a)=" << 2*square(sin(a)) << "\n";
}
cout << "Gaussian curvature P,Q="
<< geom.calcGaussianCurvature(newP) << ","
<< geom.calcGaussianCurvature(newQ) << endl;
Geodesic geod;
// Create a dummy mb system for visualization
MultibodySystem dummySystem;
SimbodyMatterSubsystem matter(dummySystem);
// matter.updGround().addBodyDecoration(Transform(), DecorativeEllipsoid(radii)
matter.updGround().addBodyDecoration(Transform(), geom.createDecorativeGeometry()
.setColor(Gray)
.setOpacity(0.5)
.setResolution(5));
matter.updGround().addBodyDecoration(Transform(),
DecorativeLine(Vec3(newP), Vec3(newP)+.5*tP).setColor(Green));
matter.updGround().addBodyDecoration(Transform(),
DecorativeLine(Vec3(newQ), Vec3(newQ)+.5*tQ).setColor(Red));
// 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);
// add vizualization callbacks for geodesics, contact points, etc.
Vector tmp(6); // tmp = ~[P Q]
tmp[0]=P[0]; tmp[1]=P[1]; tmp[2]=P[2]; tmp[3]=Q[0]; tmp[4]=Q[1]; tmp[5]=Q[2];
viz.addDecorationGenerator(new PathDecorator(tmp, O, I, Green));
//viz.addDecorationGenerator(new PlaneDecorator(geom.getPlane(), Gray));
viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodP(), Red));
viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodQ(), Blue));
viz.addDecorationGenerator(new GeodesicDecorator(geod, Orange));
//ExtremePointDecorator* expd = new ExtremePointDecorator(geom, P);
//viz.addDecorationGenerator(expd);
dummySystem.realizeTopology();
State dummyState = dummySystem.getDefaultState();
/* Sherm playing with separation tracking ...
expd->setStartPoint(Vec3(1,0,0));
for (int outer=0; ; ++outer) {
for (int i=0; i <10; ++i) {
Real x = i*.2;
expd->moveLine(Vec3(x,-3,-2), Vec3(0,3,1));
if (outer) expd->setStartPoint(expd->getStartPoint()-Vec3(.1,0,0));
expd->setShowStartFrameOnly(true);
viz.report(dummyState);
if (outer) getchar();
viz.report(dummyState);
if (outer) getchar(); else sleepInSec(.25);
//sleepInSec(.5);
}
for (int i=0; i <10; ++i) {
Real z = 1+i*.2;
Real x = 2-i*.2;
expd->moveLine(Vec3(x,-3,-2), Vec3(0,3,z));
expd->setShowStartFrameOnly(true);
viz.report(dummyState);
viz.report(dummyState); sleepInSec(.25);
//sleepInSec(.5);
}
for (int i=0; i <10; ++i) {
Real z = 3-i*.5;
expd->moveLine(Vec3(0,-3,-2), Vec3(0,3,z));
expd->setShowStartFrameOnly(true);
viz.report(dummyState);
viz.report(dummyState); sleepInSec(.25);
//sleepInSec(.5);
}
}
exit(0);
*/
// calculate the geodesic
//geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, vizInterval));
viz.report(dummyState);
const Real startReal = realTime(), startCpu = cpuTime();
//geom.calcGeodesic(P, Q, e_OP, -e_IQ, geod);
//geom.calcGeodesicAnalytical(P, Q, e_OP, -e_IQ, geod);
//geom.calcGeodesicUsingOrthogonalMethod(P, Q, geod);
//geom.calcGeodesicUsingOrthogonalMethod(P, Q, e_OP, .5, geod);
Rotation R(-Pi/8*0, YAxis); // TODO: 2.7 vs. 2.78
geom.calcGeodesicUsingOrthogonalMethod(P, Q, R*Vec3(0.9,0,-.3),
startLength, geod);
//geom.makeStraightLineGeodesic(P, Q, e_OP, GeodesicOptions(), geod);
cout << "realTime=" << realTime()-startReal
<< " cpuTime=" << cpuTime()-startCpu << endl;
viz.report(dummyState);
printf("Geodesic has %d points; %d geodesics shot\n",
geod.getNumPoints(), geom.getNumGeodesicsShot());
const Array_<Real>& arcLength = geod.getArcLengths();
const Array_<Transform>& frenet = geod.getFrenetFrames();
const Array_<Vec2>& rotPtoQ = geod.getDirectionalSensitivityPtoQ();
const Array_<Vec2>& rotQtoP = geod.getDirectionalSensitivityQtoP();
const Array_<Vec2>& transPtoQ = geod.getPositionalSensitivityPtoQ();
const Array_<Vec2>& transQtoP = geod.getPositionalSensitivityQtoP();
const Array_<Real>& curvature = geod.getCurvatures();
bool showTrans = !transPtoQ.empty();
cout << "torsion at P=" << geod.getTorsionP()
<< " binormal curvature kb at P=" << geod.getBinormalCurvatureP() << endl;
for (int i=0; i < geod.getNumPoints(); ++i) {
cout << "\ns=" << arcLength[i] << " kt=" << curvature[i] << ":\n";
cout << "p=" << frenet[i].p() << "\n";
cout << "t=" << frenet[i].y() << "\n";
cout << "b=" << frenet[i].x() << "\n";
cout << "n=" << frenet[i].z() << "\n";
cout << "jrQ=" << rotPtoQ[i] << " jrP=" << rotQtoP[i] << "\n";
if (showTrans) cout << "jtQ=" << transPtoQ[i] << " jtP=" << transQtoP[i] << "\n";
}
cout << "torsion at Q=" << geod.getTorsionQ()
<< " binormal curvature kb at Q=" << geod.getBinormalCurvatureQ() << endl;
// geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, 1/30.));
// viz.report(dummyState);
// GeodesicOptions opts;
// geom.shootGeodesicInDirectionUntilLengthReached(P, UnitVec3(tP), 20, opts, geod);
// geom.shootGeodesicInDirectionUntilPlaneHit(P, UnitVec3(tP), geom.getPlane(), opts, geod);
viz.report(dummyState);
cout << "geod shooting count = " << geom.getNumGeodesicsShot() << endl;
cout << "num geod pts = " << geod.getFrenetFrames().size() << 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;
}
void testRollingOnSurfaceConstraint()
{
using namespace SimTK;
cout << endl;
cout << "=================================================================" << endl;
cout << " OpenSim RollingOnSurfaceConstraint Simulation " << endl;
cout << "=================================================================" << endl;
// angle of the rot w.r.t. vertical
double theta = -SimTK::Pi / 6; // 30 degs
double omega = -2.1234567890;
double halfRodLength = 1.0 / (omega*omega);
UnitVec3 surfaceNormal(0,1,0);
double planeHeight = 0.0;
Vec3 comInRod(0, halfRodLength, 0);
Vec3 contactPointOnRod(0, 0, 0);
double mass = 7.0;
SimTK::Inertia inertiaAboutCom = mass*SimTK::Inertia::cylinderAlongY(0.1, 1.0);
SimTK::MassProperties rodMass(7.0, comInRod,
inertiaAboutCom.shiftFromMassCenter(comInRod, mass));
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
// Create a free joint between the rod and ground
MobilizedBody::Planar rod(matter.Ground(), Transform(Vec3(0)),
SimTK::Body::Rigid(rodMass), Transform());
// Get underlying mobilized bodies
SimTK::MobilizedBody surface = matter.getGround();
// Add a fictitious massless body to be the "Case" reference body coincident with surface for the no-slip constraint
SimTK::MobilizedBody::Weld cb(surface, SimTK::Body::Massless());
// Constrain the rod to move on the ground surface
SimTK::Constraint::PointInPlane contactY(surface, surfaceNormal, planeHeight, rod, contactPointOnRod);
SimTK::Constraint::ConstantAngle contactTorqueAboutY(surface, SimTK::UnitVec3(1, 0, 0), rod, SimTK::UnitVec3(0, 0, 1));
// Constrain the rod to roll on surface and not slide
SimTK::Constraint::NoSlip1D contactPointXdir(cb, SimTK::Vec3(0), SimTK::UnitVec3(1, 0, 0), surface, rod);
SimTK::Constraint::NoSlip1D contactPointZdir(cb, SimTK::Vec3(0), SimTK::UnitVec3(0, 0, 1), surface, rod);
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
//state = system.realizeTopology();
state.updQ()[0] = theta;
state.updQ()[1] = 0;
state.updQ()[2] = 0;
state.updU()[0] = omega;
system.realize(state, Stage::Acceleration);
state.getUDot().dump("Simbody Accelerations");
Vec3 pcom = system.getMatterSubsystem().calcSystemMassCenterLocationInGround(state);
Vec3 vcom = system.getMatterSubsystem().calcSystemMassCenterVelocityInGround(state);
Vec3 acom = system.getMatterSubsystem().calcSystemMassCenterAccelerationInGround(state);
//==========================================================================================================
// Setup OpenSim model
Model *osimModel = new Model;
osimModel->setGravity(gravity_vec);
//OpenSim bodies
Ground& ground = osimModel->updGround();;
Mesh arrowGeom("arrow.vtp");
arrowGeom.setColor(Vec3(1, 0, 0));
ground.attachGeometry(arrowGeom.clone());
//OpenSim rod
auto osim_rod = new OpenSim::Body("rod", mass, comInRod, inertiaAboutCom);
OpenSim::PhysicalOffsetFrame* cylFrame = new PhysicalOffsetFrame(*osim_rod, Transform(comInRod));
cylFrame->setName("comInRod");
osimModel->addFrame(cylFrame);
Mesh cylGeom("cylinder.vtp");
cylGeom.set_scale_factors(2 * halfRodLength*Vec3(0.1, 1, 0.1));
cylFrame->attachGeometry(cylGeom.clone());
// create rod as a free joint
auto rodJoint = new PlanarJoint("rodToGround", ground, *osim_rod);
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(osim_rod);
osimModel->addJoint(rodJoint);
// add a point on line constraint
auto roll = new RollingOnSurfaceConstraint();
roll->setRollingBodyByName("rod");
roll->setSurfaceBodyByName("ground");
/*double h = */roll->get_surface_height();
osimModel->addConstraint(roll);
osimModel->setGravity(gravity_vec);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
//osimModel->setUseVisualizer(true);
State osim_state = osimModel->initSystem();
roll->setDisabled(osim_state, false);
osim_state.updY() = state.getY();
// compute model accelerations
osimModel->computeStateVariableDerivatives(osim_state);
osim_state.getUDot().dump("Osim Accelerations");
//osimModel->updVisualizer().updSimbodyVisualizer()
// .setBackgroundType(SimTK::Visualizer::GroundAndSky);
//osimModel->getVisualizer().show(osim_state);
Vec3 osim_pcom = osimModel->calcMassCenterPosition(osim_state);
Vec3 osim_vcom = osimModel->calcMassCenterVelocity(osim_state);
Vec3 osim_acom = osimModel->calcMassCenterAcceleration(osim_state);
Vec3 tol(SimTK::SignificantReal);
ASSERT_EQUAL(pcom, osim_pcom, tol);
ASSERT_EQUAL(vcom, osim_vcom, tol);
ASSERT_EQUAL(acom, osim_acom, tol);
//==========================================================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state, "testRollingOnSurfaceConstraint FAILED\n");
}
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) {
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 {
// 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() {
try {
// Create the system, with subsystems for the bodies and some forces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
// Hint to Visualizer: don't show ground plane.
system.setUseUniformBackground(true);
// Add gravity as a force element.
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));
// Create the body and some artwork for it.
const Vec3 halfLengths(.5, .1, .25); // half-size of brick (m)
const Real mass = 2; // total mass of brick (kg)
Body::Rigid pendulumBody(MassProperties(mass, Vec3(0),
mass*UnitInertia::brick(halfLengths)));
pendulumBody.addDecoration(Transform(),
DecorativeBrick(halfLengths).setColor(Red));
// Add an instance of the body to the multibody system by connecting
// it to Ground via a Ball mobilizer.
MobilizedBody::Ball pendulum1(matter.updGround(), Transform(Vec3(-1,-1, 0)),
pendulumBody, Transform(Vec3( 0, 1, 0)));
// Add a second instance of the pendulum nearby.
MobilizedBody::Ball pendulum2(matter.updGround(), Transform(Vec3(1,-1, 0)),
pendulumBody, Transform(Vec3(0, 1, 0)));
// Connect the origins of the two pendulum bodies together with our
// rod-like custom constraint.
const Real d = 1.5; // desired separation distance
Constraint::Custom rod(new ExampleConstraint(pendulum1, pendulum2, d));
// Visualize with default options.
Visualizer viz(system);
// Add a rubber band line connecting the origins of the two bodies to
// represent the rod constraint.
viz.addRubberBandLine(pendulum1, Vec3(0), pendulum2, Vec3(0),
DecorativeLine().setColor(Blue).setLineThickness(3));
// Ask for a report every 1/30 of a second to match the Visualizer's
// default rate of 30 frames per second.
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// Output total energy to the console once per second.
system.addEventReporter(new TextDataEventReporter
(system, new MyEvaluateEnergy(), 1.0));
// Initialize the system and state.
State state = system.realizeTopology();
// Orient the two pendulums asymmetrically so they'll do something more
// interesting than just hang there.
pendulum1.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
pendulum2.setQToFitRotation(state, Rotation(BodyRotationSequence,
Pi/4, ZAxis, Pi/4, YAxis));
// Evaluate the system at the new state and draw one frame manually.
system.realize(state);
viz.report(state);
// Simulate it.
cout << "Hit ENTER to run a short simulation.\n";
cout << "(Energy should be conserved to about four decimal places.)\n";
getchar();
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(1e-4); // ask for about 4 decimal places (default is 3)
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(10.0);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
} catch (...) {
std::cout << "UNKNOWN EXCEPTION\n";
return 1;
}
return 0;
}
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);
system.setUseUniformBackground(true); // request no ground & sky
// 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,-1,0));
// Add some damping.
Force::MobilityLinearDamper damper1(forces, bodyT, MobilizerUIndex(0), 10);
Force::MobilityLinearDamper damper2(forces, leftArm, MobilizerUIndex(0), 30);
Force::MobilityLinearDamper damper3(forces, rtArm, MobilizerUIndex(0), 10);
#ifdef USE_TORQUE_LIMITED_MOTOR
MyTorqueLimitedMotor* motorp =
new MyTorqueLimitedMotor(rtArm, MobilizerUIndex(0), TorqueGain, MaxTorque);
const MyTorqueLimitedMotor& motor = *motorp;
Force::Custom(forces, motorp); // takes over ownership
#else
// Use built-in Steady Motion as a low-budget motor model.
//Motion::Steady motor(rtArm, InitialMotorRate);
// Use built-in ConstantSpeed constraint as a low-budget motor model.
Constraint::ConstantSpeed motor(rtArm, InitialMotorRate);
#endif
// Add a joint stop to the left arm restricting it to q in [0,Pi/5].
Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0),
StopStiffness,
InitialDissipation,
-Pi/8, // lower stop
Pi/8); // upper stop
Visualizer viz(system);
// Add sliders.
viz.addSlider("Motor speed", SliderIdMotorSpeed, -10, 10, InitialMotorRate);
viz.addSlider("Dissipation", SliderIdDissipation, 0, 10, InitialDissipation);
viz.addSlider("Tach", SliderIdTach, -20, 20, 0);
viz.addSlider("Torque", SliderIdTorque, -MaxTorque, MaxTorque, 0);
// Add Run menu.
Array_<std::pair<String,int> > runMenuItems;
runMenuItems.push_back(std::make_pair("Reset", ResetItem));
runMenuItems.push_back(std::make_pair("Quit", QuitItem));
viz.addMenu("Run", MenuIdRun, runMenuItems);
Visualizer::InputSilo* userInput = new Visualizer::InputSilo();
viz.addInputListener(userInput);
// Initialize the system and state.
State initState = system.realizeTopology();
// Simulate forever with a small max step size. Check for user input
// in between steps. Note: an alternate way to do this is to let the
// integrator take whatever steps it wants but use a TimeStepper to
// manage a periodic event handler to poll for user input. Here we're
// treating completion of a step as an event.
const Real MaxStepSize = 0.01*3; // 10ms
const int DrawEveryN = 3/3; // 3 steps = 30ms
//RungeKuttaMersonIntegrator integ(system);
//RungeKutta2Integrator integ(system);
SemiExplicitEuler2Integrator integ(system);
//SemiExplicitEulerIntegrator integ(system, .001);
integ.setAccuracy(1e-1);
//integ.setAccuracy(1e-3);
// Don't permit interpolation because we want the state returned after
// a step to be modifiable.
integ.setAllowInterpolation(false);
integ.initialize(initState);
int stepsSinceViz = DrawEveryN-1;
while (true) {
if (++stepsSinceViz % DrawEveryN == 0) {
const State& s = integ.getState();
viz.report(s);
const Real uActual = rtArm.getOneU(s, MobilizerUIndex(0));
viz.setSliderValue(SliderIdTach, uActual);
#ifdef USE_TORQUE_LIMITED_MOTOR
viz.setSliderValue(SliderIdTorque, motor.getTorque(s));
#else
system.realize(s); // taus are acceleration stage
//viz.setSliderValue(SliderIdTorque,
// rtArm.getOneTau(s, MobilizerUIndex(0)));
viz.setSliderValue(SliderIdTorque, motor.getMultiplier(s));
#endif
stepsSinceViz = 0;
}
// Advance time by MaxStepSize (might take multiple steps to get there).
integ.stepBy(MaxStepSize);
// Now poll for user input.
int whichSlider, whichMenu, whichItem; Real newValue;
// Did a slider move?
if (userInput->takeSliderMove(whichSlider, newValue)) {
State& state = integ.updAdvancedState();
switch(whichSlider) {
case SliderIdMotorSpeed:
// TODO: momentum balance?
//motor.setRate(state, newValue);
motor.setSpeed(state, newValue);
system.realize(state, Stage::Position);
system.prescribeU(state);
system.realize(state, Stage::Velocity);
system.projectU(state);
break;
case SliderIdDissipation:
stop.setMaterialProperties(state, StopStiffness, newValue);
system.realize(state, Stage::Position);
break;
}
}
// Was there a menu pick?
if (userInput->takeMenuPick(whichMenu, whichItem)) {
if (whichItem == QuitItem)
break; // done
// If Reset, stop the motor and restore default dissipation.
// Tell visualizer to update the sliders to match.
// Zero out all the q's and u's.
if (whichItem == ResetItem) {
State& state = integ.updAdvancedState();
//motor.setRate(state, 0);
motor.setSpeed(state, 0);
viz.setSliderValue(SliderIdMotorSpeed, 0);
stop.setMaterialProperties(state, StopStiffness, InitialDissipation);
viz.setSliderValue(SliderIdDissipation, InitialDissipation);
state.updQ() = 0; // all positions to zero
state.updU() = 0; // all velocities to zero
system.realize(state, Stage::Position);
system.prescribeU(state);
system.realize(state, Stage::Velocity);
system.projectU(state);
}
}
}
const int evals = integ.getNumRealizations();
std::cout << "Done -- simulated " << integ.getTime() << "s with "
<< integ.getNumStepsTaken() << " steps, avg step="
<< (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
<< (1000*integ.getTime())/evals << "ms/eval\n";
printf("Used 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());
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
int main() {
try
{ // Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, UnitVec3(.1,-1,0), 9.81);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
contactForces.setTransitionVelocity(1e-3);
const Vec3 hdim(1,2,3);
const Real fFac =.15; // to turn off friction
const Real fDis = .5; // to turn off dissipation
const Real fVis = .1; // to turn off viscous friction
const Real fK = .1*1e6; // pascals
// Halfspace floor
const Rotation R_xdown(-Pi/2,ZAxis);
matter.Ground().updBody().addDecoration(
Transform(Vec3(0,-.5,0)),
DecorativeBrick(Vec3(10,.5,20)).setColor(Green).setOpacity(.1));
matter.Ground().updBody().addContactSurface(
Transform(R_xdown, Vec3(0,0,0)),
ContactSurface(ContactGeometry::HalfSpace(),
ContactMaterial(fK*.1,fDis*.9,
fFac*.8,fFac*.7,fVis*10)));
const Real brickMass = 10;
Body::Rigid brickBody(MassProperties(brickMass, Vec3(0),
UnitInertia::brick(hdim)));
brickBody.addDecoration(Transform(),
DecorativeBrick(hdim).setColor(Cyan).setOpacity(.3));
const int surfx = brickBody.addContactSurface(Transform(),
ContactSurface(ContactGeometry::Brick(hdim),
ContactMaterial(fK,fDis,
fFac*.8,fFac*.7,fVis))
);
//brickBody.addContactSurface(Transform(),
// ContactSurface(ContactGeometry::Ellipsoid(hdim),
// ContactMaterial(fK*.1,fDis*.9,
// .1*fFac*.8,.1*fFac*.7,fVis*1))
// );
const ContactSurface& surf = brickBody.getContactSurface(surfx);
const ContactGeometry& cg = surf.getShape();
const ContactGeometry::Brick& cgbrick = ContactGeometry::Brick::getAs(cg);
cout << "cgbrick.hdim=" << cgbrick.getHalfLengths() << endl;
const Geo::Box& box = cgbrick.getGeoBox();
cout << "box.hdim=" << box.getHalfLengths() << endl;
// Vertices
for (int i=0; i<8; ++i) {
const Vec3 vpos = box.getVertexPos(i);
const UnitVec3 vn = box.getVertexNormal(i);
brickBody.addDecoration
(DecorativePoint(vpos).setColor(Orange));
brickBody.addDecoration
(DecorativeText(String(i)).setTransform(vpos).setColor(White)
.setScale(.5));
brickBody.addDecoration
(DecorativeLine(vpos, vpos + 0.5*vn).setColor(Orange));
printf("vertex %d:\n", i);
int e[3],ew[3],f[3],fw[3];
box.getVertexEdges(i,e,ew);
box.getVertexFaces(i,f,fw);
for (int ex=0; ex<3; ++ex) {
int ev[2]; box.getEdgeVertices(e[ex], ev);
printf(" e%2d(%d) ev=%d\n", e[ex], ew[ex], ev[ew[ex]]);
}
for (int fx=0; fx<3; ++fx) {
int fv[4]; box.getFaceVertices(f[fx], fv);
printf(" f%2d(%d) fv=%d\n", f[fx], fw[fx], fv[fw[fx]]);
}
}
// Edges
for (int i=0; i<12; ++i) {
const UnitVec3 n = box.getEdgeNormal(i);
const UnitVec3 d = box.getEdgeDirection(i);
const Vec3 ctr = box.getEdgeCenter(i);
const Real len = .75;
brickBody.addDecoration
(DecorativePoint(ctr).setColor(Green).setScale(2));
brickBody.addDecoration
(DecorativeText(String(i)).setTransform(ctr+len*n)
.setColor(Green).setScale(.3));
brickBody.addDecoration
(DecorativeLine(ctr, ctr + len*n).setColor(Green));
brickBody.addDecoration
(DecorativeLine(ctr, ctr + len*d).setColor(Green));
printf("edge %d:\n", i);
int f[2],fw[2];
box.getEdgeFaces(i,f,fw);
for (int fx=0; fx<2; ++fx) {
int fe[4]; box.getFaceEdges(f[fx], fe);
printf(" f%2d(%d) fe=%d\n", f[fx], fw[fx], fe[fw[fx]]);
}
}
// Faces
for (int i=0; i<6; ++i) {
int vertices[4]; box.getFaceVertices(i,vertices);
const UnitVec3 n = box.getFaceNormal(i);
const Vec3 ctr = box.getFaceCenter(i);
brickBody.addDecoration
(DecorativePoint(ctr).setColor(Magenta).setScale(3));
brickBody.addDecoration
(Transform(Rotation(n,ZAxis,Vec3(0,1,0),YAxis),ctr),
DecorativeText(String(i)).setColor(Magenta)
.setScale(.75).setFaceCamera(false));
brickBody.addDecoration
(DecorativeLine(ctr, ctr + 1.*n).setColor(Magenta));
}
MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0,3,0)),
brickBody, Transform(Vec3(0)));
Visualizer viz(system);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
viz.setShowShadows(true);
viz.setShowSimTime(true);
viz.setDesiredFrameRate(FrameRate);
viz.setShowFrameRate(true);
viz.setBackgroundType(Visualizer::SolidColor);
viz.setBackgroundColor(White*.9);
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();
brick.setQToFitRotation(state, Rotation(SpaceRotationSequence,
.1, ZAxis, .05, XAxis));
brick.setUToFitLinearVelocity(state, Vec3(2,0,0));
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);
//RungeKutta3Integrator integ(system);
//VerletIntegrator integ(system);
//integ.setMaximumStepSize(1e-0001);
//integ.setAccuracy(1e-3); // minimum for CPodes
integ.setAccuracy(1e-5);
//integ.setAccuracy(.01);
TimeStepper ts(system, integ);
ts.initialize(state);
double cpuStart = cpuTime();
double realStart = realTime();
ts.stepTo(20.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(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
// CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
MultibodySystem mbs;
SimbodyMatterSubsystem matter(mbs);
GeneralForceSubsystem forces(mbs);
DecorationSubsystem viz(mbs);
Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));
// ADD BODIES AND THEIR MOBILIZERS
Body::Rigid body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::brick(hl[0],hl[1],hl[2])));
body.addDecoration(DecorativeBrick(hl).setOpacity(.5));
body.addDecoration(DecorativeLine(Vec3(0), Vec3(0,1,0)).setColor(Green));
MobilizedBody::Free mobilizedBody(matter.Ground(), Transform(), body, Transform());
MobilizedBody::Free mobilizedBody0(mobilizedBody, Transform(Vec3(1,2,0)), body, Transform(Vec3(0,1,0)));
MobilizedBody::Free mobilizedBody2(mobilizedBody0, Vec3(-5,0,0), body, Transform());
Body::Rigid gear1body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(.5, .1)));
gear1body.addDecoration(DecorativeCircle(.5).setColor(Green).setOpacity(.7));
gear1body.addDecoration(DecorativeLine(Vec3(0), Vec3(.5,0,0)).setColor(Black).setLineThickness(4));
Body::Rigid gear2body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(1.5, .1)));
gear2body.addDecoration(Transform(), DecorativeCircle(1.5).setColor(Blue).setOpacity(.7));
gear2body.addDecoration(Transform(), DecorativeLine(Vec3(0), Vec3(1.5,0,0)).setColor(Black).setLineThickness(4));
MobilizedBody::Pin gear1(mobilizedBody2, Vec3(-1,0,0), gear1body, Transform()); // along z
MobilizedBody::Pin gear2(mobilizedBody2, Vec3(1,0,0), gear2body, Transform()); // along z
Constraint::NoSlip1D(mobilizedBody2, Vec3(-.5,0,0), UnitVec3(0,1,0), gear1, gear2);
Constraint::ConstantSpeed(gear1, 100.);
//Constraint::Ball myc2(matter.Ground(), Vec3(-4,2,0), mobilizedBody2, Vec3(0,1,0));
Constraint::Weld myc(matter.Ground(), Vec3(1,2,0), mobilizedBody, Vec3(0,1,0));
Constraint::Ball ball1(mobilizedBody, Vec3(2,0,0), mobilizedBody0, Vec3(3,1,1));
Constraint::Ball ball2(mobilizedBody0, Vec3(2,0,0), mobilizedBody2, Vec3(3,0,0));
//Constraint::PointInPlane pip(mobilizedBody0, UnitVec3(0,-1,0), 3, mobilizedBody2, Vec3(3,0,0));
//Constraint::ConstantOrientation ori(mobilizedBody, Rotation(), mobilizedBody0, Rotation());
//Constraint::ConstantOrientation ori2(mobilizedBody2, Rotation(), mobilizedBody0, Rotation());
//Constraint::Weld weld(mobilizedBody, Transform(Rotation(Pi/4, ZAxis), Vec3(1,1,0)),
// mobilizedBody2, Transform(Rotation(-Pi/4, ZAxis), Vec3(-1,-1,0)));
//MyConstraint xyz(gear1, 100.);
viz.addBodyFixedDecoration(mobilizedBody, Transform(Vec3(1,2,3)), DecorativeText("hello world").setScale(.1));
/*
class MyHandler : public ScheduledEventHandler {
public:
MyHandler(const Constraint& cons) : c(cons) { }
Real getNextEventTime(const State&, bool includeCurrentTime) const {
return .314;
}
void handleEvent(State& s, Real acc, const Vector& ywts, const Vector& cwts, Stage& modified,
bool& shouldTerminate) const
{
cout << "<<<< TRIGGERED AT T=" << s.getTime() << endl;
c.enable(s);
modified = Stage::Model;
}
private:
const Constraint& c;
};
mbs.addEventHandler(new MyHandler(xyz));
*/
State s = mbs.realizeTopology(); // returns a reference to the the default state
//xyz.disable(s);
//matter.setUseEulerAngles(s, true);
mbs.realizeModel(s); // define appropriate states for this System
//mobilizedBody0.setQ(s, .1);
//mobilizedBody.setQ(s, .2);
Visualizer display(mbs);
display.setBackgroundColor(White);
display.setBackgroundType(Visualizer::SolidColor);
mbs.realize(s, Stage::Velocity);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
cout << "qErr=" << s.getQErr() << endl;
cout << "uErr=" << s.getUErr() << endl;
for (ConstraintIndex cid(0); cid < matter.getNumConstraints(); ++cid) {
const Constraint& c = matter.getConstraint(cid);
int mp,mv,ma;
c.getNumConstraintEquationsInUse(s, mp,mv,ma);
cout << "CONSTRAINT " << cid << (c.isDisabled(s) ? "**DISABLED** " : "")
<< " constrained bodies=" << c.getNumConstrainedBodies();
if (c.getNumConstrainedBodies()) cout << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex();
cout << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers()
<< "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
<< " mp,mv,ma=" << mp << "," << mv << "," << ma
<< endl;
for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
cout << " constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex();
cout << endl;
}
for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
cout << " constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex()
<< ", q(" << c.getNumConstrainedQ(s, cmx) << ")=";
for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
cout << " " << c.getConstrainedQIndex(s, cmx, i);
cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
cout << " " << c.getConstrainedUIndex(s, cmx, i);
cout << endl;
}
cout << c.getSubtree();
if (mp) {
cout << "perr=" << c.getPositionErrorsAsVector(s) << endl;
cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
cout << " ~d(Pt lambda)/dlambda=" << ~c.calcPositionConstraintMatrixPt(s);
cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
Matrix P = c.calcPositionConstraintMatrixP(s);
Matrix PQ(mp,matter.getNQ(s));
Vector out(matter.getNQ(s));
for (int i=0; i<mp; ++i) {
Vector in = ~P[i];
matter.multiplyByNInv(s, true, in, out);
PQ[i] = ~out;
}
cout << " calculated d(perr)/dq=" << PQ;
}
if (mv) {
cout << "verr=" << c.getVelocityErrorsAsVector(s) << endl;
//cout << " d(verrdot)/dudot=" << c.calcVelocityConstraintMatrixV(s);
cout << " ~d(Vt lambda)/dlambda=" << ~c.calcVelocityConstraintMatrixVt(s);
}
}
const Constraint& c = matter.getConstraint(myc.getConstraintIndex());
cout << "Default configuration shown. Ready? "; getchar();
mobilizedBody.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,1,1)),Vec3(.1,.2,.3)));
mobilizedBody0.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,-1,1)),Vec3(.2,.2,.3)));
mobilizedBody2.setQToFitTransform (s, Transform(Rotation(.05,Vec3(-1,1,1)),Vec3(.1,.2,.1)));
mobilizedBody.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
mobilizedBody0.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
mobilizedBody2.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
//gear1.setUToFitAngularVelocity(s, Vec3(0,0,500)); // these should be opposite directions!
//gear2.setUToFitAngularVelocity(s, Vec3(0,0,100));
mbs.realize(s, Stage::Velocity);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
cout << "qErr=" << s.getQErr() << endl;
cout << "uErr=" << s.getUErr() << endl;
cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
cout << "v_MbM=" << mobilizedBody.getMobilizerVelocity(s)[1] << endl;
cout << "Unassembled configuration shown. Ready to assemble? "; getchar();
// These are the SimTK Simmath integrators:
RungeKuttaMersonIntegrator myStudy(mbs);
//CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::/*Newton*/Functional);
//myStudy.setOrderLimit(2); // cpodes only
//VerletIntegrator myStudy(mbs);
// ExplicitEulerIntegrator myStudy(mbs, .0005); // fixed step
//ExplicitEulerIntegrator myStudy(mbs); // variable step
//myStudy.setMaximumStepSize(0.001);
myStudy.setAccuracy(1e-6); myStudy.setAccuracy(1e-1);
//myStudy.setProjectEveryStep(true);
//myStudy.setProjectInterpolatedStates(false);
myStudy.setConstraintTolerance(1e-7); myStudy.setConstraintTolerance(1e-2);
//myStudy.setAllowInterpolation(false);
//myStudy.setMaximumStepSize(.1);
const Real dt = .02; // output intervals
const Real finalTime = 2;
myStudy.setFinalTime(finalTime);
std::vector<State> saveEm;
saveEm.reserve(2000);
for (int i=0; i<50; ++i)
saveEm.push_back(s); // delay
// Peforms assembly if constraints are violated.
myStudy.initialize(s);
for (int i=0; i<50; ++i)
saveEm.push_back(s); // delay
cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
cout << "U WEIGHTS=" << s.getUWeights() << endl;
cout << "Z WEIGHTS=" << s.getZWeights() << endl;
cout << "1/QTOLS=" << s.getQErrWeights() << endl;
cout << "1/UTOLS=" << s.getUErrWeights() << endl;
{
const State& s = myStudy.getState();
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
cout << "qErr=" << s.getQErr() << endl;
cout << "uErr=" << s.getUErr() << endl;
cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
cout << "PE=" << mbs.calcPotentialEnergy(s) << " KE=" << mbs.calcKineticEnergy(s) << " E=" << mbs.calcEnergy(s) << endl;
cout << "angle=" << std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1)) << endl;
cout << "Assembled configuration shown. Ready to simulate? "; getchar();
}
Integrator::SuccessfulStepStatus status;
int nextReport = 0;
mbs.resetAllCountersToZero();
int stepNum = 0;
while ((status=myStudy.stepTo(nextReport*dt))
!= Integrator::EndOfSimulation)
{
const State& s = myStudy.getState();
mbs.realize(s, Stage::Acceleration);
if ((stepNum++%10)==0) {
const Real angle = std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1));
printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
angle,
mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
myStudy.getPreviousStepSizeTaken(),
Integrator::getSuccessfulStepStatusString(status).c_str(),
myStudy.isStateInterpolated()?" (INTERP)":"");
printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
matter.getQErr(s).normRMS(),
matter.getUErr(s).normRMS(),
s.getSystemStage() >= Stage::Acceleration ? matter.getUDotErr(s).normRMS() : Real(-1));
#ifdef HASC
cout << "CONSTRAINT perr=" << c.getPositionError(s)
<< " verr=" << c.getVelocityError(s)
<< " aerr=" << c.getAccelerationError(s)
<< endl;
#endif
//cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
//cout << " ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
//cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
cout << "Q=" << matter.getQ(s) << endl;
cout << "U=" << matter.getU(s) << endl;
cout << "Multipliers=" << matter.getMultipliers(s) << endl;
}
Vector qdot;
matter.calcQDot(s, s.getU(), qdot);
// cout << "===> qdot =" << qdot << endl;
Vector qdot2;
matter.multiplyByN(s, false, s.getU(), qdot2);
// cout << "===> qdot2=" << qdot2 << endl;
Vector u1,u2;
matter.multiplyByNInv(s, false, qdot, u1);
matter.multiplyByNInv(s, false, qdot2, u2);
// cout << "===> u =" << s.getU() << endl;
// cout << "===> u1=" << u1 << endl;
// cout << "===> u2=" << u2 << endl;
// cout << " norm=" << (s.getU()-u2).normRMS() << endl;
display.report(s);
saveEm.push_back(s);
if (status == Integrator::ReachedReportTime)
++nextReport;
}
printf("Using Integrator %s:\n", myStudy.getMethodName());
printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
printf("System stats: realize %dP %dV %dA, projectQ %d, projectU %d\n",
mbs.getNumRealizationsOfThisStage(Stage::Position),
mbs.getNumRealizationsOfThisStage(Stage::Velocity),
mbs.getNumRealizationsOfThisStage(Stage::Acceleration),
mbs.getNumProjectQCalls(), mbs.getNumProjectUCalls());
while(true) {
for (int i=0; i < (int)saveEm.size(); ++i) {
display.report(saveEm[i]);
//display.report(saveEm[i]); // half speed
}
getchar();
}
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
catch (...) {
printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
}
int main() {
try
{ std::cout << "Current working directory: "
<< Pathname::getCurrentWorkingDirectory() << std::endl;
// Create the system.
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, 0*Vec3(2, -9.8, 0));
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
GeneralContactSubsystem OLDcontact(system);
const ContactSetIndex OLDcontactSet = OLDcontact.createContactSet();
contactForces.setTransitionVelocity(1e-3);
std::ifstream meshFile1, meshFile2;
PolygonalMesh femurMesh;
meshFile1.open("ContactBigMeshes_Femur.obj");
femurMesh.loadObjFile(meshFile1); meshFile1.close();
PolygonalMesh patellaMesh;
meshFile2.open("ContactBigMeshes_Patella.obj");
patellaMesh.loadObjFile(meshFile2); meshFile2.close();
ContactGeometry::TriangleMesh femurTri(femurMesh);
ContactGeometry::TriangleMesh patellaTri(patellaMesh);
DecorativeMesh showFemur(femurTri.createPolygonalMesh());
Array_<DecorativeLine> femurNormals;
const Real NormalLength = .02;
//for (int fx=0; fx < femurTri.getNumFaces(); ++fx)
// femurNormals.push_back(
// DecorativeLine(femurTri.findCentroid(fx),
// femurTri.findCentroid(fx)
// + NormalLength*femurTri.getFaceNormal(fx)));
DecorativeMesh showPatella(patellaTri.createPolygonalMesh());
Array_<DecorativeLine> patellaNormals;
//for (int fx=0; fx < patellaTri.getNumFaces(); ++fx)
// patellaNormals.push_back(
// DecorativeLine(patellaTri.findCentroid(fx),
// patellaTri.findCentroid(fx)
// + NormalLength*patellaTri.getFaceNormal(fx)));
// This transform has the meshes close enough that their OBBs overlap
// but in the end none of the faces are touching.
const Transform X_FP(
Rotation(Mat33( 0.97107625831404454, 0.23876955530133021, 0,
-0.23876955530133021, 0.97107625831404454, 0,
0, 0, 1), true),
Vec3(0.057400580865008571, 0.43859170879135373,
-0.00016506240185135300)
);
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
// Put femur on ground at origin
matter.Ground().updBody().addDecoration(Vec3(0,0,0),
showFemur.setColor(Cyan).setOpacity(.2));
matter.Ground().updBody().addContactSurface(Vec3(0,0,0),
ContactSurface(femurTri,
ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10),
.01 /*thickness*/));
Body::Rigid patellaBody(MassProperties(1.0, Vec3(0), Inertia(1)));
patellaBody.addDecoration(Transform(),
showPatella.setColor(Red).setOpacity(.2));
patellaBody.addContactSurface(Transform(),
ContactSurface(patellaTri,
ContactMaterial(fK*.001,fDis*.9,fFac*.8,fFac*.7,fVis*10),
.01 /*thickness*/));
MobilizedBody::Free patella(matter.Ground(), Transform(Vec3(0)),
patellaBody, Transform(Vec3(0)));
//// The old way ...
//OLDcontact.addBody(OLDcontactSet, ball,
// pyramid, Transform());
//OLDcontact.addBody(OLDcontactSet, matter.updGround(),
// ContactGeometry::HalfSpace(), Transform(R_xdown, Vec3(0,-3,0)));
//ElasticFoundationForce ef(forces, OLDcontact, OLDcontactSet);
//Real stiffness = 1e6, dissipation = 0.01, us = 0.1,
// ud = 0.05, uv = 0.01, vt = 0.01;
////Real stiffness = 1e6, dissipation = 0.1, us = 0.8,
//// ud = 0.7, uv = 0.01, vt = 0.01;
//ef.setBodyParameters(ContactSurfaceIndex(0),
// stiffness, dissipation, us, ud, uv);
//ef.setTransitionVelocity(vt);
//// end of old way.
Visualizer viz(system);
Visualizer::Reporter& reporter = *new Visualizer::Reporter(viz, ReportInterval);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
MyReporter& myRep = *new MyReporter(system,contactForces,ReportInterval);
system.addEventReporter(&myRep);
system.addEventReporter(&reporter);
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
viz.report(state);
printf("Reference state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << patella.getQAsVector(state)
<< " u=" << patella.getUAsVector(state)
<< endl;
char c=getchar();
patella.setQToFitTransform(state, ~X_FP);
viz.report(state);
printf("Initial state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << patella.getQAsVector(state)
<< " u=" << patella.getUAsVector(state)
<< endl;
c=getchar();
// Simulate it.
const clock_t start = clock();
RungeKutta3Integrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(2.0);
const double timeInSec = (double)(clock()-start)/CLOCKS_PER_SEC;
const int evals = integ.getNumRealizations();
cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
timeInSec << "s for " << ts.getTime() << "s sim (avg step="
<< (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
<< (1000*ts.getTime())/evals << "ms/eval\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());
while(true) {
for (int i=0; i < (int)saveEm.size(); ++i) {
viz.report(saveEm[i]);
}
getchar();
}
} 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;
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;
}
void testConstantDistanceConstraint()
{
using namespace SimTK;
cout << endl;
cout << "==================================================================" << endl;
cout << " OpenSim ConstantDistanceConstraint vs. Simbody Constraint::Rod " << endl;
cout << "==================================================================" << endl;
Random::Uniform randomLocation(-1, 1);
Vec3 pointOnFoot(randomLocation.getValue(), randomLocation.getValue(), randomLocation.getValue());
Vec3 pointOnGround(0,0,0);
/** for some reason, adding another Random::Uniform causes testWeldConstraint to fail.
Why doesn't it cause this test to fail???? */
//Random::Uniform randomLength(0.01, 0.2);
//randomLength.setSeed(1024);
//double rodLength = randomLength.getValue();
double rodLength = 0.05;
//std::cout << "Random Length = " << rodLength2 << ", used length = " << rodLength << std::endl;
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
// Create a free joint between the foot and ground
MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)),
SimTK::Body::Rigid(footMass), Transform(Vec3(0)));
// Constrain foot to point on ground
SimTK::Constraint::Rod simtkRod(matter.Ground(), pointOnGround, foot, pointOnFoot, rodLength);
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//==========================================================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
//OpenSim foot
OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia());
// create foot as a free joint
FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0), osim_foot, Vec3(0), Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_foot);
osimModel->addJoint(&footJoint);
// add a constant distance constraint
ConstantDistanceConstraint rodConstraint(ground, pointOnGround, osim_foot, pointOnFoot,rodLength);
osimModel->addConstraint(&rodConstraint);
// BAD: have to set memoryOwner to false or program will crash when this test is complete.
osimModel->disownAllComponents();
osimModel->setGravity(gravity_vec);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//==========================================================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state, "testConstantDistanceConstraint FAILED\n");
}
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
}
void testWeldConstraint()
{
using namespace SimTK;
cout << endl;
cout << "==================================================================" << endl;
cout << " OpenSim WeldConstraint vs. Simbody Constraint::Weld " << endl;
cout << "==================================================================" << endl;
Random::Uniform randomValue(-0.05, 0.1);
Vec3 weldInGround(randomValue.getValue(), randomValue.getValue(), 0);
Vec3 weldInFoot(0.1*randomValue.getValue(), 0.1*randomValue.getValue(), 0);
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
// Thigh connected by hip
MobilizedBody::Pin thigh(matter.Ground(), SimTK::Transform(hipInGround),
SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur));
// Pin knee connects shank
MobilizedBody::Pin shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia));
// Pin ankle connects foot
MobilizedBody::Pin foot(shank, Transform(ankleInTibia), SimTK::Body::Rigid(footMass), Transform(ankleInFoot));
SimTK::Constraint::Weld weld(matter.Ground(), Transform(weldInGround), foot, Transform(weldInFoot));
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//==========================================================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
//OpenSim thigh
OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM);
// create Pin hip joint
PinJoint hip("hip", ground, hipInGround, Vec3(0),
osim_thigh, hipInFemur, Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_thigh);
osimModel->addJoint(&hip);
//OpenSim shank
OpenSim::Body osim_shank("shank", tibiaMass.getMass(),
tibiaMass.getMassCenter(), tibiaMass.getInertia());
// create Pin knee joint
PinJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0),
osim_shank, kneeInTibia, Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_shank);
osimModel->addJoint(&knee);
//OpenSim foot
OpenSim::Body osim_foot("foot", footMass.getMass(),
footMass.getMassCenter(), footMass.getInertia());
// create Pin ankle joint
PinJoint ankle("ankle", osim_shank, ankleInTibia, Vec3(0),
osim_foot, ankleInFoot, Vec3(0));
// Add the foot body which now also contains the hip joint to the model
osimModel->addBody(&osim_foot);
osimModel->addJoint(&ankle);
// add a point on line constraint
WeldConstraint footConstraint( "footConstraint",
ground, SimTK::Transform(weldInGround),
osim_foot, SimTK::Transform(weldInFoot) );
osimModel->addConstraint(&footConstraint);
// BAD: but if model maintains ownership, it will attempt to delete stack allocated objects
osimModel->disownAllComponents();
osimModel->setGravity(gravity_vec);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
osimModel->setup();
osimModel->print("testWeldConstraint.osim");
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//=========================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state,
"testWeldConstraint FAILED\n");
}
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;
}
void testPointOnLineConstraint()
{
using namespace SimTK;
cout << endl;
cout << "==================================================================" << endl;
cout << "OpenSim PointOnLineConstraint vs. Simbody Constraint::PointOnLine " << endl;
cout << "==================================================================" << endl;
Random::Uniform randomDirection(-1, 1);
Vec3 lineDirection(randomDirection.getValue(), randomDirection.getValue(),
randomDirection.getValue());
UnitVec3 normLineDirection(lineDirection.normalize());
Vec3 pointOnLine(0,0,0);
Vec3 pointOnFollower(0,0,0);
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
// Create a free joint between the foot and ground
MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)),
SimTK::Body::Rigid(footMass), Transform(Vec3(0)));
// Constrain foot to line on ground
SimTK::Constraint::PointOnLine simtkPointOnLine(matter.Ground(),
normLineDirection, pointOnLine, foot, pointOnFollower);
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//=========================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
//OpenSim foot
OpenSim::Body osim_foot("foot", footMass.getMass(),
footMass.getMassCenter(), footMass.getInertia());
// create foot as a free joint
FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0),
osim_foot, Vec3(0), Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_foot);
osimModel->addJoint(&footJoint);
// add a point on line constraint
PointOnLineConstraint lineConstraint(ground, normLineDirection, pointOnLine,
osim_foot, pointOnFollower);
osimModel->addConstraint(&lineConstraint);
// BAD: have to set memoryOwner to false or program will crash when this test is complete.
osimModel->disownAllComponents();
osimModel->setGravity(gravity_vec);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//==========================================================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state, "testPointOnLineConstraint FAILED\n");
} // end testPointOnLineConstraint
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;
}
void testCoordinateCouplerConstraint()
{
using namespace SimTK;
cout << endl;
cout << "=================================================================" << endl;
cout << " OpenSim CoordinateCouplerConstraint vs. FunctionBasedMobilizer " << endl;
cout << "=================================================================" << endl;
// Define spline data for the custom knee joint
int npx = 12;
double angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197, -0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443, 0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393};
double kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000, 0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000, -0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000};
int npy = 7;
double angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399, -0.174532925199, 0.159148563428, 2.094395102393};
double kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000, -0.396600000000, -0.395264000000, -0.396000000000 };
for(int i = 0; i<npy; i++) {
// Spline data points from experiment w.r.t. hip location. Change to make it w.r.t knee location
kneeY[i] += (-kneeInFemur[1]+hipInFemur[1]);
}
SimmSpline tx(npx, angX, kneeX);
SimmSpline ty(npy, angY, kneeY);;
// Define the functions that specify the FunctionBased Mobilized Body.
std::vector<std::vector<int> > coordIndices;
std::vector<const SimTK::Function*> functions;
std::vector<bool> isdof(6,false);
// Set the 1 spatial rotation about Z-axis
isdof[2] = true; //rot Z
int nm = 0;
for(int i=0; i<6; i++){
if(isdof[i]) {
Vector coeff(2);
coeff[0] = 1;
coeff[1] = 0;
std::vector<int> findex(1);
findex[0] = nm++;
functions.push_back(new SimTK::Function::Linear(coeff));
coordIndices.push_back(findex);
}
else if(i==3 || i ==4){
std::vector<int> findex(1,0);
if(i==3)
functions.push_back(tx.createSimTKFunction());
else
functions.push_back(ty.createSimTKFunction());
coordIndices.push_back(findex);
}
else{
std::vector<int> findex(0);
functions.push_back(new SimTK::Function::Constant(0, 0));
coordIndices.push_back(findex);
}
}
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
//system.updDefaultSubsystem().addEventReporter(new VTKEventReporter(system, 0.01));
// Thigh connected by hip
MobilizedBody::Pin thigh(matter.Ground(), Transform(hipInGround),
SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur));
//Function-based knee connects shank
MobilizedBody::FunctionBased shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia), nm, functions, coordIndices);
//MobilizedBody::Pin shank(thigh, SimTK::Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), SimTK::Transform(kneeInTibia));
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//==========================================================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM);
// create hip as a pin joint
PinJoint hip("hip",ground, hipInGround, Vec3(0), osim_thigh, hipInFemur, Vec3(0));
// Rename hip coordinates for a pin joint
hip.getCoordinateSet()[0].setName("hip_flex");
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_thigh);
osimModel->addJoint(&hip);
// Add another body via a knee joint
OpenSim::Body osim_shank("shank", tibiaMass.getMass(),
tibiaMass.getMassCenter(), tibiaMass.getInertia());
// Define knee coordinates and axes for custom joint spatial transform
SpatialTransform kneeTransform;
string knee_q_name = "knee_q";
string tx_name = "knee_tx";
string ty_name = "knee_ty";
Array<string> indepCoords(knee_q_name, 1, 1);
// knee flexion/extension
kneeTransform[2].setCoordinateNames(indepCoords);
kneeTransform[2].setFunction(new LinearFunction());
// translation X
kneeTransform[3].setCoordinateNames(OpenSim::Array<std::string>(tx_name, 1, 1));
kneeTransform[3].setFunction(new LinearFunction());
// translation Y
kneeTransform[4].setCoordinateNames(OpenSim::Array<std::string>(ty_name, 1, 1));
kneeTransform[4].setFunction(new LinearFunction());
// create custom knee joint
CustomJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), osim_shank, kneeInTibia, Vec3(0), kneeTransform);
// Add the shank body which now also contains the knee joint to the model
osimModel->addBody(&osim_shank);
osimModel->addJoint(&knee);
// Constrain the knee translations to follow the desired manifold
CoordinateCouplerConstraint knee_tx_constraint;
CoordinateCouplerConstraint knee_ty_constraint;
knee_tx_constraint.setName("knee_tx_coupler");
knee_ty_constraint.setName("knee_ty_coupler");
knee_tx_constraint.setIndependentCoordinateNames(indepCoords);
knee_ty_constraint.setIndependentCoordinateNames(indepCoords);
knee_tx_constraint.setDependentCoordinateName(tx_name);
knee_ty_constraint.setDependentCoordinateName(ty_name);
knee_tx_constraint.setFunction(tx);
knee_ty_constraint.setFunction(ty);
// Add the constraints
osimModel->addConstraint(&knee_tx_constraint);
osimModel->addConstraint(&knee_ty_constraint);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
// OpenSim model must realize the topology to get valid osim_state
osimModel->setGravity(gravity_vec);
PointKinematics *pointKin = new PointKinematics(osimModel);
// Get the point location of the shank origin in space
pointKin->setBodyPoint("shank", Vec3(0));
osimModel->addAnalysis(pointKin);
// Model cannot own model components created on the stack in this test program
osimModel->disownAllComponents();
// write out the model to file
osimModel->print("testCouplerConstraint.osim");
//wipe-out the model just constructed
delete osimModel;
// reconstruct from the model file
osimModel = new Model("testCouplerConstraint.osim");
ForceReporter *forceReport = new ForceReporter(osimModel);
forceReport->includeConstraintForces(true);
osimModel->addAnalysis(forceReport);
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//==========================================================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state, "testCoordinateCouplerConstraint FAILED\n");
// Forces were held in storage during simulation, now write to file
forceReport->printResults("CouplerModelForces");
}