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 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);
}
//------------------------------------------------------------------------------
// ASSEMBLER
//------------------------------------------------------------------------------
Assembler::Assembler(const MultibodySystem& system)
: system(system), accuracy(0), tolerance(0), // i.e., 1e-3, 1e-4
forceNumericalGradient(false), forceNumericalJacobian(false),
useRMSErrorNorm(false), alreadyInitialized(false),
asmSys(0), optimizer(0), nAssemblySteps(0), nInitializations(0)
{
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
matter.convertToEulerAngles(system.getDefaultState(),
internalState);
system.realizeModel(internalState);
// Make sure the System's Constraints are always present; this sets the
// weight to Infinity which makes us treat this as an assembly error
// rather than merely a goal; that can be changed by the user.
systemConstraints = adoptAssemblyError(new BuiltInConstraints());
}
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;
}
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");
}
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");
}
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
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;
SimbodyMatterSubsystem matter(system);
matter.setShowDefaultGeometry(false);
CableTrackerSubsystem cables(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.81);
// Force::GlobalDamper(forces, matter, 5);
system.setUseUniformBackground(true); // no ground plane in display
MobilizedBody Ground = matter.Ground(); // convenient abbreviation
// Read in some bones.
PolygonalMesh femur;
PolygonalMesh tibia;
femur.loadVtpFile("CableOverBicubicSurfaces-femur.vtp");
tibia.loadVtpFile("CableOverBicubicSurfaces-tibia.vtp");
femur.scaleMesh(30);
tibia.scaleMesh(30);
// Build a pendulum
Body::Rigid pendulumBodyFemur( MassProperties(1.0, Vec3(0, -5, 0),
UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
pendulumBodyFemur.addDecoration(Transform(), DecorativeMesh(femur).setColor(Vec3(0.8, 0.8, 0.8)));
Body::Rigid pendulumBodyTibia( MassProperties(1.0, Vec3(0, -5, 0),
UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
pendulumBodyTibia.addDecoration(Transform(), DecorativeMesh(tibia).setColor(Vec3(0.8, 0.8, 0.8)));
Rotation z180(Pi, YAxis);
MobilizedBody::Pin pendulumFemur( matter.updGround(),
Transform(Vec3(0, 0, 0)),
pendulumBodyFemur,
Transform(Vec3(0, 0, 0)) );
Rotation rotZ45(-Pi/4, ZAxis);
MobilizedBody::Pin pendulumTibia( pendulumFemur,
Transform(rotZ45, Vec3(0, -12, 0)),
pendulumBodyTibia,
Transform(Vec3(0, 0, 0)) );
Real initialPendulumOffset = -0.25*Pi;
Constraint::PrescribedMotion pres(matter,
new Function::Sinusoid(0.25*Pi, 0.2*Pi, 0*initialPendulumOffset), pendulumTibia, MobilizerQIndex(0));
// Build a wrapping cable path
CablePath path2(cables, Ground, Vec3(1, 3, 1), // origin
pendulumTibia, Vec3(1, -4, 0)); // termination
// Create a bicubic surface
Vec3 patchOffset(0, -5, -1);
Rotation rotZ90(0.5*Pi, ZAxis);
Rotation rotX90(0.2*Pi, XAxis);
Rotation patchRotation = rotZ90 * rotX90 * rotZ90;
Transform patchTransform(patchRotation, patchOffset);
Real patchScaleX = 2.0;
Real patchScaleY = 2.0;
Real patchScaleF = 0.75;
const int Nx = 4, Ny = 4;
const Real xData[Nx] = { -2, -1, 1, 2 };
const Real yData[Ny] = { -2, -1, 1, 2 };
const Real fData[Nx*Ny] = { 2, 3, 3, 1,
0, 1.5, 1.5, 0,
0, 1.5, 1.5, 0,
2, 3, 3, 1 };
const Vector x_(Nx, xData);
const Vector y_(Ny, yData);
const Matrix f_(Nx, Ny, fData);
Vector x = patchScaleX*x_;
Vector y = patchScaleY*y_;
Matrix f = patchScaleF*f_;
BicubicSurface patch(x, y, f, 0);
Real highRes = 30;
Real lowRes = 1;
PolygonalMesh highResPatchMesh = patch.createPolygonalMesh(highRes);
PolygonalMesh lowResPatchMesh = patch.createPolygonalMesh(lowRes);
pendulumFemur.addBodyDecoration(patchTransform,
DecorativeMesh(highResPatchMesh).setColor(Cyan).setOpacity(.75));
pendulumFemur.addBodyDecoration(patchTransform,
DecorativeMesh(lowResPatchMesh).setRepresentation(DecorativeGeometry::DrawWireframe));
Vec3 patchP(-0.5,-1,2), patchQ(-0.5,1,2);
pendulumFemur.addBodyDecoration(patchTransform,
DecorativePoint(patchP).setColor(Green).setScale(2));
pendulumFemur.addBodyDecoration(patchTransform,
DecorativePoint(patchQ).setColor(Red).setScale(2));
CableObstacle::Surface patchObstacle(path2, pendulumFemur, patchTransform,
ContactGeometry::SmoothHeightMap(patch));
patchObstacle.setContactPointHints(patchP, patchQ);
patchObstacle.setDisabledByDefault(true);
// Sphere
Real sphRadius = 1.5;
Vec3 sphOffset(0, -0.5, 0);
Rotation sphRotation(0*Pi, YAxis);
Transform sphTransform(sphRotation, sphOffset);
CableObstacle::Surface tibiaSphere(path2, pendulumTibia, sphTransform,
ContactGeometry::Sphere(sphRadius));
Vec3 sphP(1.5,-0.5,0), sphQ(1.5,0.5,0);
tibiaSphere.setContactPointHints(sphP, sphQ);
pendulumTibia.addBodyDecoration(sphTransform,
DecorativeSphere(sphRadius).setColor(Red).setOpacity(0.5));
// Make cable a spring
CableSpring cable2(forces, path2, 50., 18., 0.1);
Visualizer viz(system);
viz.setShowFrameNumber(true);
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
system.addEventReporter(new ShowStuff(system, cable2, 0.02));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
system.realize(state, Stage::Position);
viz.report(state);
cout << "path2 init length=" << path2.getCableLength(state) << endl;
cout << "Hit ENTER ...";
getchar();
// path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
// Simulate it.
saveStates.clear(); saveStates.reserve(2000);
// RungeKutta3Integrator integ(system);
RungeKuttaMersonIntegrator integ(system);
// CPodesIntegrator integ(system);
// integ.setAllowInterpolation(false);
integ.setAccuracy(1e-5);
TimeStepper ts(system, integ);
ts.initialize(state);
ShowStuff::showHeading(cout);
const Real finalTime = 10;
const double startTime = realTime();
ts.stepTo(finalTime);
cout << "DONE with " << finalTime
<< "s simulated in " << realTime()-startTime
<< "s elapsed.\n";
while (true) {
cout << "Hit ENTER FOR REPLAY, Q to quit ...";
const char ch = getchar();
if (ch=='q' || ch=='Q') break;
for (unsigned i=0; i < saveStates.size(); ++i)
viz.report(saveStates[i]);
}
} catch (const std::exception& e) {
cout << "EXCEPTION: " << e.what() << "\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;
}
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;
}
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;
}
static void testObservedPointFitter(bool useConstraint) {
int failures = 0;
for (int iter = 0; iter < ITERATIONS; ++iter) {
// Build a system consisting of a chain of bodies with occasional side chains, and
// a variety of mobilizers.
MultibodySystem mbs;
SimbodyMatterSubsystem matter(mbs);
Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
body.addDecoration(Transform(), DecorativeSphere(.1));
MobilizedBody* lastBody = &matter.Ground();
MobilizedBody* lastMainChainBody = &matter.Ground();
vector<MobilizedBody*> bodies;
Random::Uniform random(0.0, 1.0);
random.setSeed(iter);
for (int i = 0; i < NUM_BODIES; ++i) {
bool mainChain = random.getValue() < 0.5;
MobilizedBody* parent = (mainChain ? lastMainChainBody : lastBody);
int type = (int) (random.getValue()*4);
MobilizedBody* nextBody;
if (type == 0) {
MobilizedBody::Cylinder cylinder(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
nextBody = &matter.updMobilizedBody(cylinder.getMobilizedBodyIndex());
}
else if (type == 1) {
MobilizedBody::Slider slider(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
nextBody = &matter.updMobilizedBody(slider.getMobilizedBodyIndex());
}
else if (type == 2) {
MobilizedBody::Ball ball(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
nextBody = &matter.updMobilizedBody(ball.getMobilizedBodyIndex());
}
else {
MobilizedBody::Pin pin(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
nextBody = &matter.updMobilizedBody(pin.getMobilizedBodyIndex());
}
bodies.push_back(nextBody);
if (mainChain)
lastMainChainBody = nextBody;
lastBody = nextBody;
}
mbs.realizeTopology();
State s = mbs.getDefaultState();
matter.setUseEulerAngles(s, true);
mbs.realizeModel(s);
// Choose a random initial conformation.
vector<Real> targetQ(s.getNQ(), Real(0));
for (MobilizedBodyIndex mbx(1); mbx < matter.getNumBodies(); ++mbx) {
const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
for (int i = 0; i < mobod.getNumQ(s); ++i) {
const QIndex qx0 = mobod.getFirstQIndex(s);
s.updQ()[qx0+i] = targetQ[qx0+i] = 2.0*random.getValue();
}
}
//cout << "q0=" << s.getQ() << endl;
mbs.realize(s, Stage::Position);
// Select some random stations on each body.
vector<vector<Vec3> > stations(NUM_BODIES);
vector<vector<Vec3> > targetLocations(NUM_BODIES);
vector<MobilizedBodyIndex> bodyIxs;
for (int i = 0; i < NUM_BODIES; ++i) {
MobilizedBodyIndex id = bodies[i]->getMobilizedBodyIndex();
bodyIxs.push_back(id);
int numStations = 1 + (int) (random.getValue()*4);
for (int j = 0; j < numStations; ++j) {
Vec3 pos(2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0);
stations[i].push_back(pos);
targetLocations[i].push_back(bodies[i]->getBodyTransform(s)*pos);
}
}
Real distance = -1;
if (useConstraint) {
// Add a constraint fixing the distance between the first and last bodies.
Real distance = (bodies[0]->getBodyOriginLocation(s)-bodies[NUM_BODIES-1]->getBodyOriginLocation(s)).norm();
// (sherm 140506) Without this 1.001 this failed on clang.
Constraint::Rod(*bodies[0], Vec3(0), *bodies[NUM_BODIES-1], Vec3(0), 1.001*distance);
}
s = mbs.realizeTopology();
matter.setUseEulerAngles(s, true);
mbs.realizeModel(s);
// Try fitting it.
State initState = s;
// (sherm 140506) I raised this from .02 to .03 to make this more robust.
if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.0, 0.03, distance))
failures++;
//cout << "q1=" << s.getQ() << endl;
// Now add random noise to the target locations, and see if it can still fit decently.
Random::Gaussian gaussian(0.0, 0.15);
for (int i = 0; i < (int) targetLocations.size(); ++i) {
for (int j = 0; j < (int) targetLocations[i].size(); ++j) {
targetLocations[i][j] += Vec3(gaussian.getValue(), gaussian.getValue(), gaussian.getValue());
}
}
s = initState; // start from same config as before
if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.1, 0.5, distance))
failures++;
//cout << "q2=" << s.getQ() << endl;
}
ASSERT(failures == 0); // It found a reasonable fit every time.
std::cout << "Done" << std::endl;
}
Real ObservedPointFitter::findBestFit
(const MultibodySystem& system, State& state,
const Array_<MobilizedBodyIndex>& bodyIxs,
const Array_<Array_<Vec3> >& stations,
const Array_<Array_<Vec3> >& targetLocations,
const Array_<Array_<Real> >& weights,
Real tolerance)
{
// Verify the inputs.
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
SimTK_APIARGCHECK(bodyIxs.size() == stations.size() && stations.size() == targetLocations.size(), "ObservedPointFitter", "findBestFit", "bodyIxs, stations, and targetLocations must all be the same length");
int numBodies = matter.getNumBodies();
for (int i = 0; i < (int)stations.size(); ++i) {
SimTK_APIARGCHECK(bodyIxs[i] >= 0 && bodyIxs[i] < numBodies, "ObservedPointFitter", "findBestFit", "Illegal body ID");
SimTK_APIARGCHECK(stations[i].size() == targetLocations[i].size(), "ObservedPointFitter", "findBestFit", "Different number of stations and target locations for body");
}
// Build a list of children for each body.
Array_<Array_<MobilizedBodyIndex> > children(matter.getNumBodies());
for (int i = 0; i < matter.getNumBodies(); ++i) {
const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
if (!body.isGround())
children[body.getParentMobilizedBody().getMobilizedBodyIndex()].push_back(body.getMobilizedBodyIndex());
}
// Build a mapping of body IDs to indices.
Array_<int> bodyIndex(matter.getNumBodies());
for (int i = 0; i < (int) bodyIndex.size(); ++i)
bodyIndex[i] = -1;
for (int i = 0; i < (int)bodyIxs.size(); ++i)
bodyIndex[bodyIxs[i]] = i;
// Find the number of stations on each body with a nonzero weight.
Array_<int> numStations(matter.getNumBodies());
for (int i = 0; i < (int) numStations.size(); ++i)
numStations[i] = 0;
for (int i = 0; i < (int)weights.size(); ++i) {
for (int j = 0; j < (int)weights[i].size(); ++j)
if (weights[i][j] != 0)
numStations[bodyIxs[i]]++;
}
// Perform the initial estimation of Q for each mobilizer.
// Our first guess is the passed-in q's, with quaternions converted
// to Euler angles if necessary. As we solve a subproblem for each
// of the bodies in ascending order, we'll update tempState's q's
// for that body to their solved values.
State tempState;
if (!matter.getUseEulerAngles(state))
matter.convertToEulerAngles(state, tempState);
else tempState = state;
system.realizeModel(tempState);
system.realize(tempState, Stage::Position);
// This will accumulate best-guess spatial poses for the bodies as
// they are processed. This is useful for when a body is used as
// an artificial base body; our first guess will to be to place it
// wherever it was the last time it was used in a subproblem.
Array_<Transform> guessX_GB(matter.getNumBodies());
for (MobilizedBodyIndex mbx(1); mbx < guessX_GB.size(); ++mbx)
guessX_GB[mbx] = matter.getMobilizedBody(mbx).getBodyTransform(tempState);
for (int i = 0; i < matter.getNumBodies(); ++i) {
MobilizedBodyIndex id(i);
const MobilizedBody& body = matter.getMobilizedBody(id);
if (body.getNumQ(tempState) == 0)
continue; // No degrees of freedom to determine.
if (children[id].size() == 0 && numStations[id] == 0)
continue; // There are no stations whose positions are affected by this.
Array_<MobilizedBodyIndex> originalBodyIxs;
int currentBodyIndex = findBodiesForClonedSystem(body.getMobilizedBodyIndex(), numStations, matter, children, originalBodyIxs);
if (currentBodyIndex == (int)originalBodyIxs.size()-1
&& (bodyIndex[id] == -1 || stations[bodyIndex[id]].size() == 0))
continue; // There are no stations whose positions are affected by this.
MultibodySystem copy;
Array_<MobilizedBodyIndex> copyBodyIxs;
bool hasArtificialBaseBody;
createClonedSystem(system, copy, originalBodyIxs, copyBodyIxs, hasArtificialBaseBody);
const SimbodyMatterSubsystem& copyMatter = copy.getMatterSubsystem();
// Construct an initial state.
State copyState = copy.getDefaultState();
assert(copyBodyIxs.size() == originalBodyIxs.size());
for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
if (ob==0 && hasArtificialBaseBody)
copyMobod.setQToFitTransform(copyState, guessX_GB[origMobod.getMobilizedBodyIndex()]);
else
copyMobod.setQFromVector(copyState, origMobod.getQAsVector(tempState));
}
Array_<Array_<Vec3> > copyStations(copyMatter.getNumBodies());
Array_<Array_<Vec3> > copyTargetLocations(copyMatter.getNumBodies());
Array_<Array_<Real> > copyWeights(copyMatter.getNumBodies());
for (int j = 0; j < (int)originalBodyIxs.size(); ++j) {
int index = bodyIndex[originalBodyIxs[j]];
if (index != -1) {
copyStations[copyBodyIxs[j]] = stations[index];
copyTargetLocations[copyBodyIxs[j]] = targetLocations[index];
copyWeights[copyBodyIxs[j]] = weights[index];
}
}
try {
OptimizerFunction optimizer(copy, copyState, copyBodyIxs, copyStations, copyTargetLocations, copyWeights);
Vector q(copyState.getQ());
//std::cout << "BODY " << i << " q0=" << q << std::endl;
optimizer.optimize(q, tolerance);
//std::cout << " qf=" << q << std::endl;
copyState.updQ() = q;
copy.realize(copyState, Stage::Position);
// Transfer updated state back to tempState as improved initial guesses.
// However, all but the currentBody will get overwritten later.
for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
guessX_GB[originalBodyIxs[ob]] = copyMobod.getBodyTransform(copyState);
if (ob==0 && hasArtificialBaseBody) continue; // leave default state
const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
origMobod.setQFromVector(tempState, copyMobod.getQAsVector(copyState));
}
//body.setQFromVector(tempState, copyMatter.getMobilizedBody(copyBodyIxs[currentBodyIndex]).getQAsVector(copyState));
}
catch (Exception::OptimizerFailed ex) {
std::cout << "Optimization failure for body "<<i<<": "<<ex.getMessage() << std::endl;
// Just leave this body's state variables set to 0, and rely on the final optimization to fix them.
}
}
// Now do the final optimization of the whole system.
OptimizerFunction optimizer(system, tempState, bodyIxs, stations, targetLocations, weights);
Vector q = tempState.getQ();
optimizer.optimize(q, tolerance);
if (matter.getUseEulerAngles(state))
state.updQ() = q;
else {
tempState.updQ() = q;
matter.convertToQuaternions(tempState, state);
}
// Return the RMS error in the optimized system.
Real error;
optimizer.objectiveFunc(q, true, error);
if (UseWeighted)
return std::sqrt(error - MinimumShift); // already weighted; this makes WRMS
Real totalWeight = 0;
for (int i = 0; i < (int)weights.size(); ++i)
for (int j = 0; j < (int)weights[i].size(); ++j)
totalWeight += weights[i][j];
return std::sqrt((error-MinimumShift)/totalWeight);
}
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
MultibodySystem mbs; mbs.setUseUniformBackground(true);
GeneralForceSubsystem forces(mbs);
SimbodyMatterSubsystem pend(mbs);
DecorationSubsystem viz(mbs);
Force::UniformGravity gravity(forces, pend, Vec3(0, -g, 0));
MobilizedBody::Ball connector(pend.Ground(),
Transform(1*Vec3(0, 0, 0)),
MassProperties(1, Vec3(0,0,0), Inertia(10,20,30)),
Transform(1*Vec3(0, .5, 0)));
connector.setDefaultRadius(0.05); // for the artwork
//connector.setDefaultRotation( Rotation(Pi/4, Vec3(0,0,1) );
const Real m1 = 5;
const Real m2 = 1;
const Real radiusRatio = std::pow(m2/m1, 1./3.);
const Vec3 weight1Location(0, 0, -d/2); // in local frame of swinging body
const Vec3 weight2Location(0, 0, d/2); // in local frame of swinging body
const Vec3 COM = (m1*weight1Location+m2*weight2Location)/(m1+m2);
const MassProperties swingerMassProps
(m1+m2, COM, 1*Inertia(1,1,1) + m1*UnitInertia::pointMassAt(weight1Location)
+ m2*UnitInertia::pointMassAt(weight2Location));
MobilizedBody::Screw swinger(connector,
Transform( Rotation( 0*.7, Vec3(9,8,7) ),
1*Vec3(0,-.5,0)),
swingerMassProps,
Transform( Rotation(0*1.3, Vec3(0,0,1) ),
COM+0*Vec3(0,0,3)), // inboard joint location
0.3); // pitch
// Add a blue sphere around the weight.
viz.addBodyFixedDecoration(swinger, weight1Location,
DecorativeSphere(d/8).setColor(Blue).setOpacity(.2));
viz.addBodyFixedDecoration(swinger, weight2Location,
DecorativeSphere(radiusRatio*d/8).setColor(Green).setOpacity(.2));
viz.addRubberBandLine(GroundIndex, Vec3(0),
swinger, Vec3(0),
DecorativeLine().setColor(Blue).setLineThickness(10)
.setRepresentation(DecorativeGeometry::DrawPoints));
//forces.addMobilityConstantForce(swinger, 0, 10);
Force::ConstantTorque(forces, swinger, Vec3(0,0,10));
//forces.addConstantForce(swinger, Vec3(0), Vec3(0,10,0));
//forces.addConstantForce(swinger, Vec3(0,0,0), Vec3(10,10,0)); // z should do nothing
//forces.addMobilityConstantForce(swinger, 1, 10);
// forces.addMobilityConstantForce(swinger, 2, 60-1.2);
State s = mbs.realizeTopology(); // define appropriate states for this System
pend.setUseEulerAngles(s, true);
mbs.realizeModel(s);
mbs.realize(s);
// Create a study using the Runge Kutta Merson integrator
RungeKuttaMersonIntegrator myStudy(mbs);
myStudy.setAccuracy(1e-6);
// This will pick up decorative geometry from
// each subsystem that generates any, including of course the
// DecorationSubsystem, but not limited to it.
Visualizer display(mbs);
const Real expectedPeriod = 2*Pi*std::sqrt(d/g);
printf("Expected period: %g seconds\n", expectedPeriod);
const Real dt = 1./30; // output intervals
const Real finalTime = 1*expectedPeriod;
for (Real startAngle = 10; startAngle <= 90; startAngle += 10) {
s = mbs.getDefaultState();
connector.setQToFitRotation(s, Rotation(Pi/4, Vec3(1,1,1)) );
printf("time theta energy *************\n");
swinger.setQToFitTransform(s, Transform( Rotation( BodyRotationSequence, 0*Pi/2, XAxis, 0*Pi/2, YAxis ), Vec3(0,0,0) ));
swinger.setUToFitVelocity(s, SpatialVec(0*Vec3(1.1,1.2,1.3),Vec3(0,0,-1)));
s.updTime() = 0;
myStudy.initialize(s);
cout << "MassProperties in B=" << swinger.expressMassPropertiesInAnotherBodyFrame(myStudy.getState(),swinger);
cout << "MassProperties in G=" << swinger.expressMassPropertiesInGroundFrame(myStudy.getState());
cout << "Spatial Inertia =" << swinger.calcBodySpatialInertiaMatrixInGround(myStudy.getState());
int stepNum = 0;
for (;;) {
// Should check for errors and other interesting status returns.
myStudy.stepTo(myStudy.getTime() + dt);
const State& s = myStudy.getState(); // s is now the integrator's internal state
if ((stepNum++%10)==0) {
// This is so we can calculate potential energy (although logically
// one should be able to do that at Stage::Position).
mbs.realize(s, Stage::Dynamics);
cout << s.getTime() << ": E=" << mbs.calcEnergy(s)
<< " connector q=" << connector.getQ(s)
<< ": swinger q=" << swinger.getQ(s) << endl;
// This is so we can look at the UDots.
mbs.realize(s, Stage::Acceleration);
cout << "q =" << pend.getQ(s) << endl;
cout << "u =" << pend.getU(s) << endl;
cout << "ud=" << pend.getUDot(s) << endl;
cout << "Connector V=" << connector.getMobilizerVelocity(s) << endl;
cout << "Swinger V=" << swinger.getMobilizerVelocity(s) << endl;
const Rotation& R_MbM = swinger.getMobilizerTransform(s).R();
Vec4 aaMb = R_MbM.convertRotationToAngleAxis();
cout << "angle=" << aaMb[0] << endl;
cout << "axisMb=" << aaMb.drop1(0) << endl;
cout << "axisMb=" << ~R_MbM*aaMb.drop1(0) << endl;
}
display.report(s);
if (s.getTime() >= finalTime)
break;
}
}
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
}
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);
}
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;
}
//------------------------------------------------------------------------------
// main program
//------------------------------------------------------------------------------
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
int i = 0;
//--------------------------------------------------------------------------
// Experimental data points (x,y) of tibia origin (tibial plateau) measured
// w.r.t. to origin of the femur (hip joint center) in the femur frame as a
// function of knee joint angle. From Yamaguchi and Zajac, 1989.
//--------------------------------------------------------------------------
// Tibia X:
int npx = 12;
Real angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197,
-0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443,
0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393};
Real kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000,
0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000,
-0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000};
// Tibia Y; note that Y data is offset by -0.4 due to body frame placement.
int npy = 7;
Real angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399,
-0.174532925199, 0.159148563428, 2.094395102393};
Real kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000,
-0.396600000000, -0.395264000000, -0.396000000000 };
// Create SimTK Vectors to hold data points, and initialize from above arrays.
Vector ka_x (npx, angX); // measured knee angles when X data was collected
Vector ka_y (npy, angY); // measured knee angles when Y data was collected
Vector tib_x(npx, kneeX);
Vector tib_y(npy, kneeY);
#ifdef SHOULD_EXAGGERATE
// See above.
tib_x *= ExaggerateX;
tib_y = (tib_y+0.4)*ExaggerateY - 0.4; // exaggerate deviation only, not offset
#endif
// Generate splines from vectors of data points.
const int Degree = 3; // use cubics
SplineFitter<Real> fitterX = SplineFitter<Real>::fitFromGCV(Degree, ka_x, tib_x);
SplineFitter<Real> fitterY = SplineFitter<Real>::fitFromGCV(Degree, ka_y, tib_y);
Spline fx = fitterX.getSpline();
Spline fy = fitterY.getSpline();
//--------------------------------------------------------------------------
// Define the 6-spatial functions that specify the motion of the tibia as a
// a FunctionBased MobilizedBody (shank) w.r.t. to its parent (the thigh).
//--------------------------------------------------------------------------
// Each function has to return 1 Real value
std::vector<const Function*> functions(6);
// as a function of coordIndices (more than one per function)
std::vector< std::vector<int> > coordIndices(6);
// about a body-fixed axis for rotations or in parent translations
std::vector<Vec3> axes(6);
// Set the 1st and 2nd spatial rotation about the orthogonal (X then Y) axes as
// constant values. That is they don't contribute to motion nor do they have
// any coordinates in the equations of motion.
// |--------------------------------|
// | 1st function: rotation about X |
// |--------------------------------|
functions[0] = (new Function::Constant(0, 0));
std::vector<int> noIndex(0);
coordIndices[0] =(noIndex);
// |--------------------------------|
// | 2nd function: rotation about Y |
// |--------------------------------|
functions[1] = (new Function::Constant(0, 0));
coordIndices[1] = (noIndex);
// Set the spatial rotation about third axis to be the knee-extension
// angle (the one q) about the Z-axis of the tibia at the femoral condyles
// Define the coefficients of the linear function of the knee-angle with the
// spatial rotation about Z.
Vector coeff(2);
// Linear function x3 = coeff[0]*q + coeff[1]
coeff[0] = 1; coeff[1] = 0;
// |--------------------------------|
// | 3rd function: rotation about Z |
// |--------------------------------|
functions[2] = new Function::Linear(coeff);
// function of coordinate 0 (knee extension angle)
std::vector<int> qIndex(1,0);
coordIndices[2] = qIndex;
// Set the spatial translations as a function (splines) along the parent X and Y axes
// |-----------------------------------|
// | 4th function: translation about X |
// |-----------------------------------|
functions[3] = new Spline(fx); // Give the mobilizer a copy it can own.
coordIndices[3] =(qIndex);
// |-----------------------------------|
// | 5th function: translation about Y |
// |-----------------------------------|
functions[4] = new Spline(fy); // Give the mobilizer a copy it can own.
coordIndices[4] =(qIndex);
// |-----------------------------------|
// | 6th function: translation about Z |
// |-----------------------------------|
functions[5] = (new Function::Constant(0, 0));
coordIndices[5] = (noIndex);
// Construct the multibody system
const Real grav = 9.80665;
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, grav);
matter.setShowDefaultGeometry(true);
//--------------------------------------------------------------------------
// Define the model's physical (body) properties
//--------------------------------------------------------------------------
//Thigh
Body::Rigid femur(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337))));
femur.addDecoration(Transform(Vec3(0, -0.21+0.1715, 0)),
DecorativeCylinder(0.04, 0.21).setColor(Orange).setOpacity(.5));
//Shank
Body::Rigid tibia(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484))));
tibia.addDecoration(Transform(Vec3(0, -0.235+0.1862, 0)),
DecorativeCylinder(0.035, 0.235).setColor(Red));
//--------------------------------------------------------------------------
// Build the multibody system by adding mobilized bodies to the matter subsystem
//--------------------------------------------------------------------------
// Add the thigh via hip joint
MobilizedBody::Pin thigh(matter.Ground(), Transform(Vec3(0)), femur, Transform(Vec3(0.0020, 0.1715, 0)));
// This is how you might model the knee if you could only use a pin joint.
//MobilizedBody::Pin shank(thigh, Transform(Vec3(0.0033, -0.2294, 0)),
// tibia, Transform(Vec3(0.0, 0.1862, 0.0)));
// NOTE: function of Y-translation data was defined int the femur frame
// according to Yamaguchi and Zajac, which had the orgin at the hip joint
// center and the Y along the long-axis of the femur and Z out of the page.
MobilizedBody::FunctionBased shank(thigh, Transform(Vec3(0.0020, 0.1715, 0)),
tibia, Transform(Vec3(0.0, 0.1862, 0.0)),
1, // # of mobilities (dofs) for this joint
functions, coordIndices);
// Add some stop springs so the knee angle won't get outside the range of spline
// data we have. This custom force element is defined above.
Force::Custom(forces, new MyStop(shank, -Pi/2, 0*Pi, 100));
//--------------------------------------------------------------------------
// Setup reporters so we can get some output.
//--------------------------------------------------------------------------
// Vizualizer Animation
Visualizer viz(system);
system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
// Energy -- reporter defined above.
system.addEventReporter(new MyEnergyReporter(system, 0.01));
//--------------------------------------------------------------------------
// Complete the construction of the "const" part of the System and
// allocate the default state.
//--------------------------------------------------------------------------
system.realizeTopology();
// Get a copy of the default state to work with.
State state = system.getDefaultState();
//--------------------------------------------------------------------------
// Set modeling options if any (this one is not actually needed here).
//--------------------------------------------------------------------------
matter.setUseEulerAngles(state, true);
// Complete construction of the model, allocating additional state variables
// if necessary.
system.realizeModel(state);
//--------------------------------------------------------------------------
// Set initial conditions.
//--------------------------------------------------------------------------
// Hip and knee coordinates and speeds similar to early swing
double hip_angle = -45*Pi/180;
double knee_angle = 0*Pi/180;
double hip_vel = 1;
double knee_vel = -5.0;
// Set initial states (Q's and U's)
// Position
thigh.setOneQ(state, 0, hip_angle);
shank.setOneQ(state, 0, knee_angle);
// Speed
thigh.setOneU(state, 0, hip_vel);
shank.setOneU(state, 0, knee_vel);
//--------------------------------------------------------------------------
// Run simulation.
//--------------------------------------------------------------------------
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(Accuracy);
TimeStepper ts(system, integ);
ts.initialize(state); // set IC's
ts.stepTo(5.0);
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
return 0;
}
void main_simulation()
#endif
{
// inputs
double fitness;
#ifdef OPTI
double *optiParams;
#endif
Loop_inputs *loop_inputs;
// initialization
loop_inputs = init_simulation();
// optimization init
#ifdef OPTI
optiParams = (double*) malloc(NB_PARAMS_TO_OPTIMIZE*sizeof(double));
get_real_params_to_opt(optiNorms, optiParams);
erase_for_opti(optiParams, loop_inputs->MBSdata);
free(optiParams);
#endif
// -- Simbody -- //
#ifdef SIMBODY
// / Create the system. Define all "system" objects - system, matterm forces, tracker, contactForces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
system.setUpDirection(+ZAxis); // that is for visualization only. The default direction is +X
SimbodyVariables simbodyVariables;
// set all the mechanical parameters of the contact
simbodyVariables.p_system = &system;
simbodyVariables.p_matter = &matter;
simbodyVariables.p_tracker = &tracker;
simbodyVariables.p_contactForces = &contactForces;
// cout<<"BoxInd in Main = "<<BoxInd<<" -- should be default \n";
//init_Simbody(&simbodyVariables);
init_Simbody(&simbodyVariables, loop_inputs->MBSdata->user_IO->simbodyBodies);
//it is "system" commands. We cannot avoid them.
system.realizeTopology();
State state = system.getDefaultState();
simbodyVariables.p_state = &state;
//it is "system" command. We cannot avoid them.
system.realizeModel(state);
p_simbodyVariables = &simbodyVariables;
#endif
// loop
fitness = loop_simulation(loop_inputs);
// end of the simulation
finish_simulation(loop_inputs);
#ifdef OPTI
return fitness;
#else
#if defined(PRINT_REPORT)
printf("fitness: %f\n", fitness);
#endif
#endif
}
int main() {
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);
/// 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 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");
}