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 testSolveArbitrary() {
static Random::Uniform uniform(2.0, 7.0);
static Random::Gaussian random(0.0, 100.0);
// Test random polynomials of various degrees with real coefficients.
Vector realCoeff;
Vector_<Complex> roots;
for (int i = 0; i < 1000; ++i) {
int length = uniform.getIntValue();
realCoeff.resize(length);
roots.resize(length-1);
for (int j = 0; j < length; ++j)
realCoeff[j] = random.getValue();
PolynomialRootFinder::findRoots(realCoeff, roots);
verifyRoots(realCoeff, roots);
}
// Test random polynomials of various degrees with complex coefficients.
Vector_<Complex> complexCoeff;
for (int i = 0; i < 1000; ++i) {
int length = uniform.getIntValue();
complexCoeff.resize(length);
roots.resize(length-1);
for (int j = 0; j < length; ++j)
complexCoeff[j] = Complex(random.getValue(), random.getValue());
PolynomialRootFinder::findRoots(complexCoeff, roots);
verifyRoots(complexCoeff, roots);
}
// Verify that if the leading coefficient is zero, it throws an exception.
try {
realCoeff[0] = 0.0;
PolynomialRootFinder::findRoots(realCoeff, roots);
ASSERT(false)
}
catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
}
try {
complexCoeff[0] = 0.0;
PolynomialRootFinder::findRoots(complexCoeff, roots);
ASSERT(false)
}
catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
}
}
void testSolveQuadratic() {
// Try a few fixed tests.
testQuadraticRealRoots(Vec2(0.0, 0.0));
testQuadraticRealRoots(Vec2(1.0, 1.0));
testQuadraticRealRoots(Vec2(0.0, 5.0));
testQuadraticRealRoots(Vec2(10.0, -5.0));
testQuadratic(Vec3(1.0, 0.0, 0.0), Vec<2,Complex>(0.0, 0.0));
// Try cases with a single, repeated real root.
static Random::Gaussian random(0.0, 100.0);
for (int i = 0; i < 1000; ++i) {
Real root = random.getValue();
testQuadraticRealRoots(Vec2(root, root));
}
// Try cases with two distinct real roots.
for (int i = 0; i < 1000; ++i)
testQuadraticRealRoots(Vec2(random.getValue(), random.getValue()));
// Try cases whose roots are complex conjugates.
for (int i = 0; i < 1000; ++i)
testQuadraticConjugateRoots(Complex(random.getValue(), random.getValue()));
// Try cases with two distinct complex roots.
for (int i = 0; i < 1000; ++i)
testQuadraticComplexRoots(Vec<2,Complex>(Complex(random.getValue(), random.getValue()), Complex(random.getValue(), random.getValue())));
// Verify that if the leading coefficient is zero, it throws an exception.
try {
testQuadratic(Vec3(0.0, 1.0, 1.0), Vec<2,Complex>(0.0, 0.0));
ASSERT(false)
}
catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
}
try {
testQuadratic(Vec<3,Complex>(0.0, 1.0, 1.0), Vec<2,Complex>(0.0, 0.0));
ASSERT(false)
}
catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
}
}
void testQuadraticConjugateRoots(Complex root) {
static Random::Gaussian random(0.0, 1000.0);
Vec3 coeff(1.0, -2.0*root.real(), norm(root));
coeff *= random.getValue();
testQuadratic(coeff, Vec<2,Complex>(root, conj(root)));
}
void testQuadraticRealRoots(Vec2 roots) {
static Random::Gaussian random(0.0, 100.0);
Vec3 coeff(1.0, -roots[0]-roots[1], roots[0]*roots[1]);
coeff *= random.getValue();
testQuadratic(coeff, Vec<2,Complex>(roots[0], roots[1]));
}
void testCubicComplexRoots(Vec<3,Complex> roots) {
static Random::Gaussian random(0.0, 100.0);
Vec<4,Complex> coeff(1.0, -roots[0]-roots[1]-roots[2], roots[0]*roots[1]+roots[1]*roots[2]+roots[2]*roots[0], -roots[0]*roots[1]*roots[2]);
coeff *= random.getValue();
testCubic(coeff, Vec<3,Complex>(roots[0], roots[1], roots[2]));
}
void testCubicConjugateRoots(Complex root1, Real root2) {
static Random::Gaussian random(0.0, 1000.0);
Vec4 coeff(1.0, -2.0*root1.real()-root2, norm(root1)+2.0*root1.real()*root2,-norm(root1)*root2);
coeff *= random.getValue();
testCubic(coeff, Vec<3,Complex>(root1, conj(root1), root2));
}
void testQuadraticComplexRoots(Vec<2,Complex>roots) {
static Random::Gaussian random(0.0, 100.0);
Vec<3,Complex> coeff(1.0, -roots[0]-roots[1], roots[0]*roots[1]);
coeff *= Complex(random.getValue(), random.getValue());
testQuadratic(coeff, roots);
}