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);
}
void testWeld() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0));
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
// Create two pendulums, each with two welded bodies. One uses a Weld MobilizedBody,
// and the other uses a Weld constraint.
Transform inboard(Vec3(0.1, 0.5, -1));
Transform outboard(Vec3(0.2, -0.2, 0));
MobilizedBody::Ball p1(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
MobilizedBody::Ball p2(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
MobilizedBody::Weld c1(p1, inboard, body, outboard);
MobilizedBody::Free c2(p2, inboard, body, outboard);
Constraint::Weld constraint(p2, inboard, c2, outboard);
// It is not a general test unless the Weld mobilizer has children!
MobilizedBody::Pin wchild1(c1, inboard, body, outboard);
MobilizedBody::Pin wchild2(c2, inboard, body, outboard);
Force::MobilityLinearSpring(forces, wchild1, 0, 1000, 0);
Force::MobilityLinearSpring(forces, wchild2, 0, 1000, 0);
State state = system.realizeTopology();
p1.setU(state, Vec3(1, 2, 3));
p2.setU(state, Vec3(1, 2, 3));
system.realize(state, Stage::Velocity);
system.project(state, 1e-10);
SimTK_TEST_EQ(c1.getBodyTransform(state), c2.getBodyTransform(state));
SimTK_TEST_EQ(c1.getBodyVelocity(state), c2.getBodyVelocity(state));
// Simulate it and see if both pendulums behave identically.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(5);
system.realize(integ.getState(), Stage::Velocity);
SimTK_TEST_EQ_TOL(c1.getBodyTransform(integ.getState()),
c2.getBodyTransform(integ.getState()), 1e-10);
SimTK_TEST_EQ_TOL(c1.getBodyVelocity(integ.getState()),
c2.getBodyVelocity(integ.getState()), 1e-10);
}
int main() {
// Build a system consisting of a chain of bodies with every possible mobilizer.
MultibodySystem mbs;
SimbodyMatterSubsystem matter(mbs);
Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
body.addDecoration(DecorativeSphere(.1));
Random::Uniform random(0.0, 2.0);
MobilizedBody lastBody = MobilizedBody::Pin(matter.Ground(), Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Slider(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Universal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Cylinder(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::BendStretch(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Planar(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Gimbal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Ball(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Translation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Free(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::LineOrientation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::FreeLine(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Weld(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Screw(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())), 0.5);
lastBody = MobilizedBody::Ellipsoid(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
mbs.realizeTopology();
State& s = mbs.updDefaultState();
mbs.realizeModel(s);
// Choose a random initial conformation.
for (int i = 0; i < s.getNQ(); ++i)
s.updQ()[i] = random.getValue();
mbs.realize(s, Stage::Instance);
// The only constraints are the quaternions -- normalize them.
Vector temp;
mbs.project(s, 0.01);
mbs.realize(s, Stage::Position);
// Convert to Euler angles and make sure the positions are all the same.
State euler = s;
matter.convertToEulerAngles(s, euler);
mbs.realize(euler, Stage::Position);
for (int i = 0; i < matter.getNumBodies(); ++i) {
const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
Real dist = (body.getBodyOriginLocation(euler)-body.getBodyOriginLocation(s)).norm();
ASSERT(dist < 1e-5);
}
// Now convert back to quaternions and make sure the positions are still the same.
State quaternions = s;
matter.convertToQuaternions(euler, quaternions);
mbs.realize(quaternions, Stage::Position);
for (int i = 0; i < matter.getNumBodies(); ++i) {
const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
Real dist = (body.getBodyOriginLocation(quaternions)-body.getBodyOriginLocation(s)).norm();
ASSERT(dist < 1e-5);
}
// Compare the state variables to see if they have been accurately reproduced.
mbs.project(s, 0.01); // Normalize the quaternions
Real diff = std::sqrt((s.getQ()-quaternions.getQ()).normSqr()/s.getNQ());
ASSERT(diff < 1e-5);
std::cout << "Done" << std::endl;
}
void testBushing() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));
Body::Rigid lumpy(MassProperties(3.1, Vec3(.1, .2, .3),
UnitInertia(1.2,1.1,1.3,.01,.02,.03)));
Body::Rigid massless(MassProperties(0,Vec3(0),UnitInertia(0)));
Vec3 inboard(0.1, 0.5, -1);
Vec3 outboard(0.2, -0.2, 0);
MobilizedBody::Bushing p1(matter.updGround(), inboard, lumpy, outboard);
Rotation axisX(Pi/2, YAxis); // rotate z into x
Rotation axisY(-Pi/2, XAxis); // rotate z into y
Rotation axisZ; // leave z where it is
MobilizedBody::Translation dummy0(matter.updGround(), inboard,
massless, Transform());
MobilizedBody::Pin dummy1(dummy0, Transform(axisX,Vec3(0)),
massless, Transform(axisX, Vec3(0)));
MobilizedBody::Pin dummy2(dummy1, Transform(axisY, Vec3(0)),
massless, Transform(axisY, Vec3(0)));
MobilizedBody::Pin p2(dummy2, Transform(axisZ, Vec3(0)),
lumpy, Transform(axisZ, outboard));
MobilizedBody::Weld c1(p1, inboard, lumpy, outboard);
MobilizedBody::Weld c2(p2, inboard, lumpy, outboard);
c1.addBodyDecoration(Transform(), DecorativeBrick(.1*Vec3(1,2,3))
.setColor(Cyan).setOpacity(0.2));
c2.addBodyDecoration(Transform(), DecorativeBrick(.1*Vec3(1,2,3))
.setColor(Red).setRepresentation(DecorativeGeometry::DrawWireframe));
//Visualizer viz(system); viz.setBackgroundType(Visualizer::SolidColor);
//system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
State state = system.realizeTopology();
p1.setQ(state, Vec6(.1,.2,.3,1,2,3));
dummy0.setMobilizerTranslation(state, Vec3(1,2,3));
dummy1.setAngle(state, .1);
dummy2.setAngle(state, .2);
p2.setAngle(state, .3);
p1.setU(state, Vec6(1, 2, 3, -.1, -.2, -.3));
dummy0.setMobilizerVelocity(state, Vec3(-.1, -.2, -.3));
dummy1.setRate(state, 1);
dummy2.setRate(state, 2);
p2.setRate(state, 3);
system.realize(state, Stage::Velocity);
system.project(state, 1e-10);
SimTK_TEST_EQ(c1.getBodyTransform(state), c2.getBodyTransform(state));
SimTK_TEST_EQ(c1.getBodyVelocity(state), c2.getBodyVelocity(state));
// Simulate it and see if both pendulums behave identically.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(5);
system.realize(integ.getState(), Stage::Velocity);
SimTK_TEST_EQ_TOL(c1.getBodyTransform(integ.getState()),
c2.getBodyTransform(integ.getState()), 1e-10);
SimTK_TEST_EQ_TOL(c1.getBodyVelocity(integ.getState()),
c2.getBodyVelocity(integ.getState()), 1e-10);
}