void testCompositeInertia() {
MultibodySystem mbs;
SimbodyMatterSubsystem pend(mbs);
Body::Rigid pointMass(MassProperties(3, Vec3(0), Inertia(0)));
// Point mass at x=1.5 rotating about (0,0,0).
MobilizedBody::Pin
body1( pend.Ground(), Transform(),
pointMass, Vec3(1.5,0,0));
const MobilizedBodyIndex body1x = body1.getMobilizedBodyIndex();
// A second body 2 units further along x, rotating about the
// first point mass origin.
MobilizedBody::Pin
body2( body1, Transform(),
pointMass, Vec3(2,0,0));
const MobilizedBodyIndex body2x = body2.getMobilizedBodyIndex();
State state = mbs.realizeTopology();
mbs.realize(state, Stage::Position);
Array_<SpatialInertia, MobilizedBodyIndex> R(pend.getNumBodies());
pend.calcCompositeBodyInertias(state, R);
// Calculate expected inertias about the joint axes.
Real expInertia2 = body2.getBodyMassProperties(state).getMass()*square(2);
Real expInertia1 = body1.getBodyMassProperties(state).getMass()*square(1.5)
+ body2.getBodyMassProperties(state).getMass()*square(3.5);
// Should be able to recover these inertias by projecting the composite
// body inertias onto the joint axes using H matrices.
const SpatialVec H1 = body1.getHCol(state, MobilizerUIndex(0));
const SpatialVec H2 = body2.getHCol(state, MobilizerUIndex(0));
SimTK_TEST_EQ(~H2*(R[body2x]*H2), expInertia2);
SimTK_TEST_EQ(~H1*(R[body1x]*H1), expInertia1);
// This should force realization of the composite body inertias.
SpatialInertia cbi = pend.getCompositeBodyInertia(state, body1);
body2.setAngle(state, Pi/4);
// This is not allowed until Position stage.
SimTK_TEST_MUST_THROW(pend.getCompositeBodyInertia(state, body1));
mbs.realize(state, Stage::Position);
// Now it should be OK.
cbi = pend.getCompositeBodyInertia(state, body1);
mbs.realize(state, Stage::Acceleration);
//cout << "udots=" << state.getUDot() << endl;
body1.setRate(state, 27);
mbs.realize(state, Stage::Acceleration);
//cout << "udots=" << state.getUDot() << endl;
}
void 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 testSpeedCoupler2() {
// Create a system involving a constraint that affects three different
// bodies.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
createGimbalSystem(system);
MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
std::vector<MobilizedBodyIndex> bodies(3);
std::vector<MobilizerUIndex> speeds(3);
bodies[0] = MobilizedBodyIndex(1);
bodies[1] = MobilizedBodyIndex(3);
bodies[2] = MobilizedBodyIndex(5);
speeds[0] = MobilizerUIndex(0);
speeds[1] = MobilizerUIndex(0);
speeds[2] = MobilizerUIndex(1);
Function* function = new CompoundFunction();
Constraint::SpeedCoupler coupler(matter, function, bodies, speeds);
State state;
createState(system, state);
// Make sure the constraint is satisfied.
Vector args(function->getArgumentSize());
for (int i = 0; i < args.size(); ++i)
args[i] = matter.getMobilizedBody(bodies[i]).getOneU(state, speeds[i]);
SimTK_TEST_EQ(0.0, function->calcValue(args));
// Simulate it and make sure the constraint is working correctly and
// energy is being conserved. This should be workless and power should
// always be zero (to the extent that the constraint is satisfied).
Real energy0 = system.calcEnergy(state);
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(1e-6);
integ.setReturnEveryInternalStep(true);
integ.initialize(state);
while (integ.getTime() < 10.0) {
integ.stepTo(10.0);
const State& istate = integ.getState();
system.realize(istate, Stage::Acceleration);
const Real energy = system.calcEnergy(istate);
const Real power = coupler.calcPower(istate);
for (int i = 0; i < args.size(); ++i)
args[i] = matter.getMobilizedBody(bodies[i]).getOneU(istate, speeds[i]);
SimTK_TEST_EQ_TOL(0.0, function->calcValue(args),
integ.getConstraintToleranceInUse());
SimTK_TEST_EQ_TOL(0.0, power, 10*integ.getConstraintToleranceInUse());
// Energy conservation depends on global integration accuracy;
// accuracy returned here is local so we'll fudge at 10X.
const Real etol = 10*integ.getAccuracyInUse()
*std::max(std::abs(energy), std::abs(energy0));
SimTK_TEST_EQ_TOL(energy0, energy, etol);
}
}
void 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);
}
void testPrescribedMotion1() {
// Create a system requiring simple linear motion of one Q. This
// may require that the constraint do work.
// (The way the cylinder system is structured it only takes work to
// keep body one at a uniform velocity; the rest are in free fall.)
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
createCylinderSystem(system);
MobilizedBodyIndex body = MobilizedBodyIndex(1);
MobilizerQIndex coordinate = MobilizerQIndex(1);
Vector coefficients(2);
coefficients[0] = 0.1;
coefficients[1] = 0.0;
Function* function = new Function::Linear(coefficients);
Constraint::PrescribedMotion constraint(matter, function, body, coordinate);
PowerMeasure<Real> powMeas(matter, constraint);
Measure::Zero zeroMeas(matter);
Measure::Integrate workMeas(matter, powMeas, zeroMeas);
State state;
createState(system, state);
workMeas.setValue(state, 0); // override createState
// Make sure the constraint is satisfied.
Vector args(1, state.getTime());
SimTK_TEST_EQ(function->calcValue(args),
matter.getMobilizedBody(body).getOneQ(state, coordinate));
// Simulate it and make sure the constraint is working correctly.
const Real energy0 = system.calcEnergy(state);
RungeKuttaMersonIntegrator integ(system);
integ.setReturnEveryInternalStep(true);
integ.initialize(state);
while (integ.getTime() < 10.0) {
integ.stepTo(10.0);
const State& istate = integ.getState();
system.realize(istate, Stage::Acceleration);
const Real energy = system.calcEnergy(istate);
const Real power = powMeas.getValue(istate);
const Real work = workMeas.getValue(istate);
Vector args(1, istate.getTime());
const Real q = matter.getMobilizedBody(body).getOneQ(istate, coordinate);
SimTK_TEST_EQ_TOL(function->calcValue(args), q,
integ.getConstraintToleranceInUse());
// Energy conservation depends on global integration accuracy;
// accuracy returned here is local so we'll fudge at 10X.
const Real etol = 10*integ.getAccuracyInUse()
*std::max(std::abs(energy-work), std::abs(energy0));
SimTK_TEST_EQ_TOL(energy0, energy-work, etol)
}
}
void testConstraintForces() {
// Weld one body to ground, push on it, verify that it reacts to match the load.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
MobilizedBody::Weld welded(matter.Ground(), Transform(),
body, Transform());
MobilizedBody::Free loose(matter.Ground(), Transform(),
body, Transform());
Constraint::Weld glue(matter.Ground(), Transform(),
loose, Transform());
// Apply forces to the body welded straight to ground.
Force::ConstantForce(forces, welded, Vec3(0,0,0), Vec3(1,2,3));
Force::ConstantTorque(forces, welded, Vec3(20,30,40));
// Apply the same forces to the "free" body which is welded by constraint.
Force::ConstantForce(forces, loose, Vec3(0,0,0), Vec3(1,2,3));
Force::ConstantTorque(forces, loose, Vec3(20,30,40));
State state = system.realizeTopology();
system.realize(state, Stage::Acceleration);
Vector_<SpatialVec> mobilizerReactions;
matter.calcMobilizerReactionForces(state, mobilizerReactions);
//NOT IMPLEMENTED YET:
//cout << "Weld constraint reaction on Ground: " << glue.getWeldReactionOnBody1(state) << endl;
//cout << "Weld constraint reaction on Body: " << glue.getWeldReactionOnBody2(state) << endl;
// Note that constraint forces have opposite sign to applied forces, because
// we calculate the multiplier lambda from M udot + ~G lambda = f_applied. We'll negate
// the calculated multipliers to turn these into applied forces.
const Vector mults = -state.getMultipliers();
Vector_<SpatialVec> constraintForces;
Vector mobilityForces;
matter.calcConstraintForcesFromMultipliers(state, mults,
constraintForces, mobilityForces);
MACHINE_TEST(constraintForces[loose.getMobilizedBodyIndex()],
mobilizerReactions[welded.getMobilizedBodyIndex()]);
// This returns just the forces on the weld's two bodies: in this
// case Ground and "loose", in that order.
Vector_<SpatialVec> glueForces =
glue.getConstrainedBodyForcesAsVector(state);
MACHINE_TEST(-glueForces[1], // watch the sign!
mobilizerReactions[welded.getMobilizedBodyIndex()]);
}
void testCalculationMethods() {
// Create a system with two bodies.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
MobilizedBody::Free b1(matter.Ground(), body);
MobilizedBody::Free b2(matter.Ground(), body);
// Set all the state variables to random values.
system.realizeTopology();
State state = system.getDefaultState();
Random::Gaussian random;
for (int i = 0; i < state.getNY(); ++i)
state.updY()[i] = random.getValue();
system.realize(state, Stage::Acceleration);
// Test the low level methods for transforming points and vectors.
const Vec3 point(0.5, 1, -1.5);
SimTK_TEST_EQ(b1.findStationLocationInGround(state, Vec3(0)), b1.getBodyOriginLocation(state));
SimTK_TEST_EQ(b1.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), point);
SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), b1.findStationLocationInAnotherBody(state, point, b2));
SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, Vec3(0))).norm(), (b1.getBodyOriginLocation(state)-b2.getBodyOriginLocation(state)).norm());
SimTK_TEST_EQ(b2.findMassCenterLocationInGround(state), b2.findStationLocationInGround(state, b2.getBodyMassCenterStation(state)));
SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, Vec3(0)), Vec3(0));
SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, point), b1.getBodyRotation(state)*point);
SimTK_TEST_EQ(b1.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), point);
SimTK_TEST_EQ(b2.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), b1.expressVectorInAnotherBodyFrame(state, point, b2));
// Test the routines for mapping locations, velocities, and accelerations.
Vec3 r, v, a;
b1.findStationLocationVelocityAndAccelerationInGround(state, point, r, v, a);
SimTK_TEST_EQ(v, b1.findStationVelocityInGround(state, point));
SimTK_TEST_EQ(a, b1.findStationAccelerationInGround(state, point));
{
Vec3 r2, v2;
b1.findStationLocationAndVelocityInGround(state, point, r2, v2);
SimTK_TEST_EQ(r, r2);
SimTK_TEST_EQ(v, v2);
}
SimTK_TEST_EQ(b1.findStationVelocityInGround(state, Vec3(0)), b1.getBodyOriginVelocity(state));
SimTK_TEST_EQ(b1.findStationAccelerationInGround(state, Vec3(0)), b1.getBodyOriginAcceleration(state));
SimTK_TEST_EQ(b1.findStationVelocityInGround(state, point), b1.findStationVelocityInAnotherBody(state, point, matter.Ground()));
}
bool testFitting
(const MultibodySystem& mbs, State& state,
const vector<MobilizedBodyIndex>& bodyIxs,
const vector<vector<Vec3> >& stations,
const vector<vector<Vec3> >& targetLocations,
Real minError, Real maxError, Real endDistance)
{
// Find the best fit.
Real reportedError = ObservedPointFitter::findBestFit(mbs, state, bodyIxs, stations, targetLocations, TOL);
cout << "[min,max]=" << minError << "," << maxError << " actual=" << reportedError << endl;
bool result = (reportedError <= maxError && reportedError >= minError);
// Verify that the error was calculated correctly.
Real error = 0.0;
int numStations = 0;
mbs.realize(state, Stage::Position);
const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
for (int i = 0; i < (int) bodyIxs.size(); ++i) {
MobilizedBodyIndex id = bodyIxs[i];
numStations += (int)stations[i].size();
for (int j = 0; j < (int) stations[i].size(); ++j)
error += (targetLocations[i][j]-matter.getMobilizedBody(id).getBodyTransform(state)*stations[i][j]).normSqr();
}
error = std::sqrt(error/numStations);
cout << "calc wrms=" << error << endl;
ASSERT(std::abs(1.0-error/reportedError) < 0.0001); // should match to machine precision
if (endDistance >= 0) {
// Verify that the ends are the correct distance apart.
Real distance = (matter.getMobilizedBody(bodyIxs[0]).getBodyOriginLocation(state)-matter.getMobilizedBody(bodyIxs[bodyIxs.size()-1]).getBodyOriginLocation(state)).norm();
cout << "required dist=" << endDistance << ", actual=" << distance << endl;
ASSERT(std::abs(1.0-endDistance/distance) < TOL);
}
return result;
}
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;
}
void testForces() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralContactSubsystem contacts(system);
GeneralForceSubsystem forces(system);
// Create a triangle mesh in the shape of a pyramid, with the
// square base having area 1 (split into two triangles).
vector<Vec3> vertices;
vertices.push_back(Vec3(0, 0, 0));
vertices.push_back(Vec3(1, 0, 0));
vertices.push_back(Vec3(1, 0, 1));
vertices.push_back(Vec3(0, 0, 1));
vertices.push_back(Vec3(0.5, 1, 0.5));
vector<int> faceIndices;
int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
{2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
for (int i = 0; i < 6; i++)
for (int j = 0; j < 3; j++)
faceIndices.push_back(faces[i][j]);
// Create the mobilized bodies and configure the contact model.
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
ContactSetIndex setIndex = contacts.createContactSet();
MobilizedBody::Translation mesh(matter.updGround(), Transform(), body, Transform());
contacts.addBody(setIndex, mesh, ContactGeometry::TriangleMesh(vertices, faceIndices), Transform());
contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0))); // y < 0
ElasticFoundationForce ef(forces, contacts, setIndex);
Real stiffness = 1e9, dissipation = 0.01, us = 0.1, ud = 0.05, uv = 0.01, vt = 0.01;
ef.setBodyParameters(ContactSurfaceIndex(0), stiffness, dissipation, us, ud, uv);
ef.setTransitionVelocity(vt);
ASSERT(ef.getTransitionVelocity() == vt);
State state = system.realizeTopology();
// Position the pyramid at a variety of positions and check the normal
// force.
for (Real depth = -0.1; depth < 0.1; depth += 0.01) {
mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
system.realize(state, Stage::Dynamics);
Real f = 0;
if (depth > 0)
f = stiffness*depth;
assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[matter.getGround().getMobilizedBodyIndex()][1], Vec3(0, -f, 0));
}
// Now do it with a vertical velocity and see if the dissipation force is correct.
for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
for (Real v = -1.0; v <= 1.0; v += 0.1) {
mesh.setUToFitLinearVelocity(state, Vec3(0, -v, 0));
system.realize(state, Stage::Dynamics);
Real f = (depth > 0 ? stiffness*depth*(1+dissipation*v) : 0);
if (f < 0)
f = 0;
assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
}
}
// Do it with a horizontal velocity and see if the friction force is correct.
Vector_<SpatialVec> expectedForce(matter.getNumBodies());
for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
Real fh = 0;
if (depth > 0)
fh = stiffness*depth;
for (Real v = -1.0; v <= 1.0; v += 0.1) {
mesh.setUToFitLinearVelocity(state, Vec3(v, 0, 0));
system.realize(state, Stage::Dynamics);
const Real vrel = std::abs(v/vt);
Real ff = (v < 0 ? 1 : -1)*fh*(std::min(vrel, 1.0)*(ud+2*(us-ud)/(1+vrel*vrel))+uv*std::fabs(v));
const Vec3 totalForce = Vec3(ff, fh, 0);
expectedForce = SpatialVec(Vec3(0), Vec3(0));
Vec3 contactPoint1 = mesh.findStationAtGroundPoint(state, Vec3(2.0/3.0, 0, 1.0/3.0));
mesh.applyForceToBodyPoint(state, contactPoint1, 0.5*totalForce, expectedForce);
Vec3 contactPoint2 = mesh.findStationAtGroundPoint(state, Vec3(1.0/3.0, 0, 2.0/3.0));
mesh.applyForceToBodyPoint(state, contactPoint2, 0.5*totalForce, expectedForce);
SpatialVec actualForce = system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()];
assertEqual(actualForce[0], expectedForce[mesh.getMobilizedBodyIndex()][0]);
assertEqual(actualForce[1], expectedForce[mesh.getMobilizedBodyIndex()][1]);
}
}
}
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;
}
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);
}
}
// Test calculations of Jacobian "bias" terms, where bias=JDot*u.
// We can estimate JDot using a numerical directional derivative
// since JDot = (DJ/Dq)*qdot ~= (J(q+h*qdot)-J(q-h*qdot))/2h.
// Then we multiply JDot*u and compare with the bias calculations.
// Or, we can estimate JDot*u directly with
// JDotu ~= (J(q+h*qdot)*u - J(q-h*qdot)*u)/2h
// using the fast "multiply by Jacobian" methods.
// We use both methods below.
void testJacobianBiasTerms() {
MultibodySystem system;
MyForceImpl* frcp;
makeSystem(false, system, frcp);
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
State state = system.realizeTopology();
const int nq = state.getNQ();
const int nu = state.getNU();
const int nb = matter.getNumBodies();
system.realizeModel(state);
// Randomize state.
state.updQ() = Test::randVector(nq);
state.updU() = Test::randVector(nu);
const MobilizedBodyIndex whichBod(8);
const Vec3 whichPt(1,2,3);
system.realize(state, Stage::Velocity);
const Vector& q = state.getQ();
const Vector& u = state.getU();
const Vector& qdot = state.getQDot();
// sbias, fbias, sysbias are the JDot*u quantities we want to check.
const Vec3 sbias =
matter.calcBiasForStationJacobian(state, whichBod, whichPt);
const SpatialVec fbias =
matter.calcBiasForFrameJacobian(state, whichBod, whichPt);
Vector_<SpatialVec> sysbias;
matter.calcBiasForSystemJacobian(state, sysbias);
// These are for computing JDot first.
RowVector_<Vec3> JS_P, JS1_P, JS2_P, JSDot_P;
RowVector_<SpatialVec> JF_P, JF1_P, JF2_P, JFDot_P;
Matrix_<SpatialVec> J, J1, J2, JDot;
// These are for computing JDot*u directly.
Vec3 JS_Pu, JS1_Pu, JS2_Pu, JSDot_Pu;
SpatialVec JF_Pu, JF1_Pu, JF2_Pu, JFDot_Pu;
Vector_<SpatialVec> Ju, J1u, J2u, JDotu;
// Unperturbed:
matter.calcStationJacobian(state,whichBod,whichPt, JS_P);
matter.calcFrameJacobian(state,whichBod,whichPt, JF_P);
matter.calcSystemJacobian(state, J);
JS_Pu = matter.multiplyByStationJacobian(state,whichBod,whichPt,u);
JF_Pu = matter.multiplyByFrameJacobian(state,whichBod,whichPt,u);
matter.multiplyBySystemJacobian(state, u, Ju);
const Real Delta = 5e-6; // we'll use central difference
State perturbq = state;
// Perturbed +:
perturbq.updQ() = q + Delta*qdot;
system.realize(perturbq, Stage::Position);
matter.calcStationJacobian(perturbq,whichBod,whichPt, JS2_P);
matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF2_P);
matter.calcSystemJacobian(perturbq, J2);
JS2_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
JF2_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
matter.multiplyBySystemJacobian(perturbq,u, J2u);
// Perturbed -:
perturbq.updQ() = q - Delta*qdot;
system.realize(perturbq, Stage::Position);
matter.calcStationJacobian(perturbq,whichBod,whichPt, JS1_P);
matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF1_P);
matter.calcSystemJacobian(perturbq, J1);
JS1_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
JF1_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
matter.multiplyBySystemJacobian(perturbq,u, J1u);
// Estimate JDots:
JSDot_P = (JS2_P-JS1_P)/Delta/2;
JFDot_P = (JF2_P-JF1_P)/Delta/2;
JDot = (J2-J1)/Delta/2;
// Estimate JDotus:
JSDot_Pu = (JS2_Pu-JS1_Pu)/Delta/2;
JFDot_Pu = (JF2_Pu-JF1_Pu)/Delta/2;
JDotu = (J2u-J1u)/Delta/2;
// Calculate errors in JDot*u:
SimTK_TEST_EQ_TOL((JSDot_P*u-sbias).norm(), 0, SqrtEps);
SimTK_TEST_EQ_TOL((JFDot_P*u-fbias).norm(), 0, SqrtEps);
SimTK_TEST_EQ_TOL((JDot*u-sysbias).norm(), 0, SqrtEps);
// Calculate errors in JDotu:
SimTK_TEST_EQ_TOL((JSDot_Pu-sbias).norm(), 0, SqrtEps);
SimTK_TEST_EQ_TOL((JFDot_Pu-fbias).norm(), 0, SqrtEps);
SimTK_TEST_EQ_TOL((JDotu-sysbias).norm(), 0, SqrtEps);
}
//==============================================================================
// MAIN
//==============================================================================
int main() {
try { // catch errors if any
// Create the system, with subsystems for the bodies and some forces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contact(system, tracker);
contact.setTransitionVelocity(transitionVelocity);
Force::Gravity(forces, matter, -YAxis, 9.81);
// Set up visualization and ask for a frame every 1/30 second.
Visualizer viz(system);
viz.setShowSimTime(true); viz.setShowFrameRate(true);
viz.addSlider("Speed", SpeedControlSlider, -10, 10, 0);
Visualizer::InputSilo* silo = new Visualizer::InputSilo();
viz.addInputListener(silo);
#ifdef ANIMATE
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
#endif
DecorativeText help("Any input to start; ESC to quit");
help.setIsScreenText(true);
viz.addDecoration(MobilizedBodyIndex(0),Vec3(0),help);
matter.setShowDefaultGeometry(false);
// Add the Ground contact geometry. Contact half space has -XAxis normal
// (right hand wall) so we have to rotate.
MobilizedBody& Ground = matter.updGround(); // Nicer name for Ground.
Ground.updBody().addContactSurface(Transform(YtoX,Vec3(0)),
ContactSurface(ContactGeometry::HalfSpace(),concrete));
// Add some speed bumps.
const int NBumps = 2; const Vec3 BumpShape(.8,0.2,2);
for (int i=0; i < NBumps; ++i) {
const Real x = -2*(i+1);
Ground.updBody().addContactSurface(Vec3(x,0,0),
ContactSurface(ContactGeometry::Ellipsoid(BumpShape), rubber));
Ground.updBody().addDecoration(Vec3(x,0,0),
DecorativeEllipsoid(BumpShape).setColor(Gray).setResolution(3));
}
// TORSO
const Real TorsoHeight = 1.1;
const Vec3 torsoHDims(1,.08,.8);
const Real torsoVolume = 8*torsoHDims[0]*torsoHDims[1]*torsoHDims[2];
const Real torsoMass = torsoVolume*rubber_density/10;
const Vec3 torsoCOM(0,-.75,0); // put it low for stability
Body::Rigid torsoInfo(MassProperties(torsoMass,torsoCOM,
UnitInertia::brick(torsoHDims).shiftFromCentroidInPlace(-torsoCOM)));
torsoInfo.addDecoration(Vec3(0),
DecorativeBrick(torsoHDims).setColor(Cyan));
// CRANK
const Vec3 crankCenter(0,0,0); // in torso frame
const Vec3 crankOffset(0,0,torsoHDims[2]+LinkDepth); // left/right offset
const Real MLen=15/100.; // crank radius
Body::Rigid crankInfo(MassProperties(.1,Vec3(0),
UnitInertia::cylinderAlongZ(MLen, LinkDepth)));
crankInfo.addDecoration(Vec3(0),
DecorativeBrick(Vec3(LinkWidth,LinkWidth,torsoHDims[2]))
.setColor(Black));
const Vec3 CrankConnect(MLen,0,0); // in crank frame
// Add the torso and crank mobilized bodies.
MobilizedBody::Free torso(Ground,Vec3(0,TorsoHeight,0), torsoInfo,Vec3(0));
MobilizedBody::Pin crank(torso,crankCenter, crankInfo, Vec3(0));
// Add the legs.
for (int i=-1; i<=1; ++i) {
const Vec3 offset = crankCenter + i*crankOffset;
const Vec3 linkSpace(0,0,2*LinkDepth);
const Rotation R_CP(i*2*Pi/3,ZAxis);
// Add crank bars for looks.
crank.addBodyDecoration(
Transform(R_CP, offset+1.5*MLen/2*R_CP.x()+(i==0?linkSpace:Vec3(0))),
DecorativeBrick(Vec3(1.5*MLen/2,LinkWidth,LinkDepth))
.setColor(Yellow));
addOneLeg(viz, torso, offset + i*linkSpace,
crank, R_CP*CrankConnect);
addOneLeg(viz, torso, Transform(Rotation(Pi,YAxis), offset + i*linkSpace),
crank, R_CP*CrankConnect);
}
// Add speed control.
Motion::Steady motor(crank, 0); // User controls speed.
system.addEventHandler
(new UserInputHandler(*silo, motor, Real(0.1))); //check input every 100ms
// Initialize the system and state.
State state = system.realizeTopology();
system.realize(state);
printf("Theo Jansen Strandbeest in 3D:\n");
printf("%d bodies, %d mobilities, -%d constraint equations -%d motions\n",
matter.getNumBodies(), state.getNU(), state.getNMultipliers(),
matter.getMotionMultipliers(state).size());
viz.report(state);
printf("Hit any key to assemble ...");
silo->waitForAnyUserInput(); silo->clear();
Assembler(system).assemble(state);
printf("ASSEMBLED\n");
// Simulate.
SemiExplicitEuler2Integrator integ(system);
integ.setAccuracy(0.1);
integ.setConstraintTolerance(.001);
integ.setMaximumStepSize(1./60);
TimeStepper ts(system, integ);
ts.initialize(state);
viz.report(ts.getState());
printf("Hit ENTER to simulate ... (ESC to quit)\n");
silo->waitForAnyUserInput(); silo->clear();
const double startCPU = cpuTime(), startTime = realTime();
ts.stepTo(Infinity); // RUN
dumpIntegratorStats(startCPU, startTime, integ);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
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);
}
void testTaskJacobians() {
MultibodySystem system;
MyForceImpl* frcp;
makeSystem(false, system, frcp);
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
State state = system.realizeTopology();
const int nq = state.getNQ();
const int nu = state.getNU();
const int nb = matter.getNumBodies();
// Attainable accuracy drops with problem size.
const Real Slop = nu*SignificantReal;
system.realizeModel(state);
// Randomize state.
state.updQ() = Test::randVector(nq);
state.updU() = Test::randVector(nu);
system.realize(state, Stage::Position);
Matrix_<SpatialVec> J;
Matrix Jmat, Jmat2;
matter.calcSystemJacobian(state, J);
SimTK_TEST_EQ(J.nrow(), nb); SimTK_TEST_EQ(J.ncol(), nu);
matter.calcSystemJacobian(state, Jmat);
SimTK_TEST_EQ(Jmat.nrow(), 6*nb); SimTK_TEST_EQ(Jmat.ncol(), nu);
// Unpack J into Jmat2 and compare with Jmat.
Jmat2.resize(6*nb, nu);
for (int row=0; row < nb; ++row) {
const int nxtr = 6*row; // row index into scalar matrix
for (int col=0; col < nu; ++col) {
for (int k=0; k<3; ++k) {
Jmat2(nxtr+k, col) = J(row,col)[0][k];
Jmat2(nxtr+3+k, col) = J(row,col)[1][k];
}
}
}
// These should be exactly the same.
SimTK_TEST_EQ_TOL(Jmat2, Jmat, SignificantReal);
Vector randU = 100.*Test::randVector(nu), resultU1, resultU2;
Vector_<SpatialVec> randF(nb), resultF1, resultF2;
for (int i=0; i<nb; ++i) randF[i] = 100.*Test::randSpatialVec();
matter.multiplyBySystemJacobian(state, randU, resultF1);
resultF2 = J*randU;
SimTK_TEST_EQ_TOL(resultF1, resultF2, Slop);
matter.multiplyBySystemJacobianTranspose(state, randF, resultU1);
resultU2 = ~J*randF;
SimTK_TEST_EQ_TOL(resultU1, resultU2, Slop);
// See if Station Jacobian can be used to duplicate the translation
// rows of the System Jacobian, and if Frame Jacobian can be used to
// duplicate the whole thing.
Array_<MobilizedBodyIndex> allBodies(nb);
for (int i=0; i<nb; ++i) allBodies[i]=MobilizedBodyIndex(i);
Array_<Vec3> allOrigins(nb, Vec3(0));
Matrix_<Vec3> JS, JS2, JSbyrow;
Matrix_<SpatialVec> JF, JF2, JFbyrow;
matter.calcStationJacobian(state, allBodies, allOrigins, JS);
matter.calcFrameJacobian(state, allBodies, allOrigins, JF);
for (int i=0; i<nb; ++i) {
for (int j=0; j<nu; ++j) {
SimTK_TEST_EQ(JS(i,j), J(i,j)[1]);
SimTK_TEST_EQ(JF(i,j), J(i,j));
}
}
// Now use random stations to calculate JS & JF.
Array_<Vec3> randS(nb);
for (int i=0; i<nb; ++i) randS[i] = 10.*Test::randVec3();
matter.calcStationJacobian(state, allBodies, randS, JS);
matter.calcFrameJacobian(state, allBodies, randS, JF);
// Recalculate one row at a time to test non-contiguous memory handling.
// Do it backwards just to show off.
JSbyrow.resize(nb, nu); JFbyrow.resize(nb, nu);
for (int i=nb-1; i >= 0; --i) {
matter.calcStationJacobian(state, allBodies[i], randS[i], JSbyrow[i]);
matter.calcFrameJacobian(state, allBodies[i], randS[i], JFbyrow[i]);
}
SimTK_TEST_EQ(JS, JSbyrow);
SimTK_TEST_EQ(JF, JFbyrow);
// Calculate JS2=JS and JF2=JF again using multiplication by mobility-space
// unit vectors.
JS2.resize(nb, nu); JF2.resize(nb, nu);
Vector zeroU(nu, 0.);
for (int i=0; i < nu; ++i) {
zeroU[i] = 1;
matter.multiplyByStationJacobian(state, allBodies, randS, zeroU, JS2(i));
matter.multiplyByFrameJacobian(state, allBodies, randS, zeroU, JF2(i));
zeroU[i] = 0;
}
SimTK_TEST_EQ_TOL(JS2, JS, Slop);
SimTK_TEST_EQ_TOL(JF2, JF, Slop);
// Calculate JS2t=~JS using multiplication by force-space unit vectors.
Matrix_<Row3> JS2t(nu,nb);
Vector_<Vec3> zeroF(nb, Vec3(0));
// While we're at it, let's test non-contiguous vectors by filling in
// this scalar version and using its non-contig rows as column temps.
Matrix JS3mat(3*nb,nu);
for (int b=0; b < nb; ++b) {
for (int k=0; k<3; ++k) {
zeroF[b][k] = 1;
RowVectorView JS3matr = JS3mat[3*b+k];
matter.multiplyByStationJacobianTranspose(state, allBodies, randS,
zeroF, ~JS3matr);
zeroF[b][k] = 0;
for (int u=0; u < nu; ++u)
JS2t(u,b)[k] = JS3matr[u];
}
}
SimTK_TEST_EQ_TOL(JS2, ~JS2t, Slop); // we'll check JS3mat below
// Calculate JF2t=~JF using multiplication by force-space unit vectors.
Matrix_<SpatialRow> JF2t(nu,nb);
Vector_<SpatialVec> zeroSF(nb, SpatialVec(Vec3(0)));
// While we're at it, let's test non-contiguous vectors by filling in
// this scalar version and using its non-contig rows as column temps.
Matrix JF3mat(6*nb,nu);
for (int b=0; b < nb; ++b) {
for (int k=0; k<6; ++k) {
zeroSF[b][k/3][k%3] = 1;
RowVectorView JF3matr = JF3mat[6*b+k];
matter.multiplyByFrameJacobianTranspose(state, allBodies, randS,
zeroSF, ~JF3matr);
zeroSF[b][k/3][k%3] = 0;
for (int u=0; u < nu; ++u)
JF2t(u,b)[k/3][k%3] = JF3matr[u];
}
}
SimTK_TEST_EQ_TOL(JF2, ~JF2t, Slop); // we'll check JS3mat below
// All three methods match. Now let's see if they are right by shifting
// the System Jacobian to the new stations.
for (int i=0; i<nb; ++i) {
const MobilizedBody& mobod = matter.getMobilizedBody(allBodies[i]);
const Rotation& R_GB = mobod.getBodyRotation(state);
const Vec3 S_G = R_GB*randS[i];
for (int j=0; j<nu; ++j) {
const Vec3 w = J(i,j)[0];
const Vec3 v = J(i,j)[1];
const Vec3 vJ = v + w % S_G; // Shift
const Vec3 vS = JS2(i,j);
const SpatialVec vF = JF2(i,j);
SimTK_TEST_EQ(vS, vJ);
SimTK_TEST_EQ(vF, SpatialVec(w, vJ));
}
}
// Now create a scalar version of JS and make sure it matches the Vec3 one.
Matrix JSmat, JSmat2, JFmat, JFmat2;
matter.calcStationJacobian(state, allBodies, randS, JSmat);
matter.calcFrameJacobian(state, allBodies, randS, JFmat);
SimTK_TEST_EQ(JSmat.nrow(), 3*nb); SimTK_TEST_EQ(JSmat.ncol(), nu);
SimTK_TEST_EQ(JFmat.nrow(), 6*nb); SimTK_TEST_EQ(JFmat.ncol(), nu);
SimTK_TEST_EQ_TOL(JSmat, JS3mat, Slop); // same as above?
SimTK_TEST_EQ_TOL(JFmat, JF3mat, Slop); // same as above?
// Unpack JS into JSmat2 and compare with JSmat.
JSmat2.resize(3*nb, nu);
for (int row=0; row < nb; ++row) {
const int nxtr = 3*row; // row index into scalar matrix
for (int col=0; col < nu; ++col) {
for (int k=0; k<3; ++k) {
JSmat2(nxtr+k, col) = JS(row,col)[k];
}
}
}
// These should be exactly the same.
SimTK_TEST_EQ_TOL(JSmat2, JSmat, SignificantReal);
// Unpack JF into JFmat2 and compare with JFmat.
JFmat2.resize(6*nb, nu);
for (int row=0; row < nb; ++row) {
const int nxtr = 6*row; // row index into scalar matrix
for (int col=0; col < nu; ++col) {
for (int k=0; k<6; ++k) {
JFmat2(nxtr+k, col) = JF(row,col)[k/3][k%3];
}
}
}
// These should be exactly the same.
SimTK_TEST_EQ_TOL(JFmat2, JFmat, SignificantReal);
}
void testConstrainedSystem() {
MultibodySystem mbs;
MyForceImpl* frcp;
makeSystem(true, mbs, frcp);
const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
State state = mbs.realizeTopology();
mbs.realize(state, Stage::Instance); // allocate multipliers, etc.
const int nq = state.getNQ();
const int nu = state.getNU();
const int m = state.getNMultipliers();
const int nb = matter.getNumBodies();
// Attainable accuracy drops with problem size.
const Real Slop = nu*SignificantReal;
mbs.realizeModel(state);
// Randomize state.
state.updQ() = Test::randVector(nq);
state.updU() = Test::randVector(nu);
Vector randMobFrc = 100*Test::randVector(nu);
Vector_<SpatialVec> randBodyFrc(nb);
for (int i=0; i < nb; ++i)
randBodyFrc[i] = Test::randSpatialVec();
// Apply random mobility forces
frcp->setMobilityForces(randMobFrc);
mbs.realize(state); // calculate accelerations and multipliers
Vector udot = state.getUDot();
Vector lambda = state.getMultipliers();
Vector residual;
matter.calcResidualForce(state,randMobFrc,Vector_<SpatialVec>(),
udot, lambda, residual);
// Residual should be zero since we accounted for everything.
SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);
Vector abias, mgbias;
// These are the acceleration error bias terms.
matter.calcBiasForAccelerationConstraints(state, abias);
// These use pverr (velocity-level errors) for holonomic constraints.
matter.calcBiasForMultiplyByG(state, mgbias);
Vector mgGudot; matter.multiplyByG(state, udot, mgbias, mgGudot);
Matrix G; matter.calcG(state, G);
Vector Gudot = G*udot;
SimTK_TEST_EQ_TOL(mgGudot, Gudot, Slop);
Vector aerr = state.getUDotErr(); // won't be zero because bad constraints
Vector GudotPlusBias = Gudot + abias;
SimTK_TEST_EQ_TOL(GudotPlusBias, aerr, Slop);
// Add in some body forces
state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
frcp->setBodyForces(randBodyFrc);
mbs.realize(state);
udot = state.getUDot();
lambda = state.getMultipliers();
matter.calcResidualForce(state,randMobFrc,randBodyFrc,
udot, lambda, residual);
SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);
// Try body forces only.
state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
frcp->setMobilityForces(0*randMobFrc);
mbs.realize(state);
udot = state.getUDot();
lambda = state.getMultipliers();
matter.calcResidualForce(state,Vector(),randBodyFrc,
udot, lambda, residual);
SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);
// Put vectors in noncontiguous storage.
Matrix udotmat(3,nu); // rows are noncontig
Matrix mobFrcMat(11,nu);
Matrix lambdamat(5,m);
Matrix_<SpatialRow> bodyFrcMat(3,nb);
udotmat[2] = ~udot;
lambdamat[3] = ~lambda;
mobFrcMat[8] = ~randMobFrc;
bodyFrcMat[2] = ~randBodyFrc;
Matrix residmat(4,nu);
// We last computed udot,lambda with no mobility forces. This time
// will throw some in and then make sure the residual tries to cancel them.
matter.calcResidualForce(state,~mobFrcMat[8],~bodyFrcMat[2],
~udotmat[2],~lambdamat[3],~residmat[2]);
SimTK_TEST_EQ_TOL(residmat[2], -1*mobFrcMat[8], Slop);
}
void testUnconstrainedSystem() {
MultibodySystem system;
MyForceImpl* frcp;
makeSystem(false, system, frcp);
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
State state = system.realizeTopology();
const int nq = state.getNQ();
const int nu = state.getNU();
const int nb = matter.getNumBodies();
// Attainable accuracy drops with problem size.
const Real Slop = nu*SignificantReal;
system.realizeModel(state);
// Randomize state.
state.updQ() = Test::randVector(nq);
state.updU() = Test::randVector(nu);
Vector randVec = 100*Test::randVector(nu);
Vector result1, result2;
// result1 = M*v
system.realize(state, Stage::Position);
matter.multiplyByM(state, randVec, result1);
SimTK_TEST_EQ(result1.size(), nu);
// result2 = M^-1 * result1 == M^-1 * M * v == v
system.realize(state, Stage::Dynamics);
matter.multiplyByMInv(state, result1, result2);
SimTK_TEST_EQ(result2.size(), nu);
SimTK_TEST_EQ_TOL(result2, randVec, Slop);
Matrix M(nu,nu), MInv(nu,nu);
Vector v(nu, Real(0));
for (int j=0; j < nu; ++j) {
v[j] = 1;
matter.multiplyByM(state, v, M(j));
matter.multiplyByMInv(state, v, MInv(j));
v[j] = 0;
}
Matrix MInvCalc(M);
MInvCalc.invertInPlace();
SimTK_TEST_EQ_SIZE(MInv, MInvCalc, nu);
Matrix identity(nu,nu); identity=1;
SimTK_TEST_EQ_SIZE(M*MInv, identity, nu);
SimTK_TEST_EQ_SIZE(MInv*M, identity, nu);
// Compare above-calculated values with values returned by the
// calcM() and calcMInv() methods.
Matrix MM, MMInv;
matter.calcM(state,MM); matter.calcMInv(state,MMInv);
SimTK_TEST_EQ_SIZE(MM, M, nu);
SimTK_TEST_EQ_SIZE(MMInv, MInv, nu);
//assertIsIdentity(eye);
//assertIsIdentity(MInv*M);
frcp->setMobilityForces(randVec);
//cout << "f=" << randVec << endl;
system.realize(state, Stage::Acceleration);
Vector accel = state.getUDot();
//cout << "v!=0, accel=" << accel << endl;
matter.multiplyByMInv(state, randVec, result1);
//cout << "With velocities, |a - M^-1*f|=" << (accel-result1).norm() << endl;
SimTK_TEST_NOTEQ(accel, result1); // because of the velocities
//SimTK_TEST((accel-result1).norm() > SignificantReal); // because of velocities
// With no velocities M^-1*f should match calculated acceleration.
state.updU() = 0;
system.realize(state, Stage::Acceleration);
accel = state.getUDot();
//cout << "v=0, accel=" << accel << endl;
//cout << "With v=0, |a - M^-1*f|=" << (accel-result1).norm() << endl;
SimTK_TEST_EQ(accel, result1); // because no velocities
// And then M*a should = f.
matter.multiplyByM(state, accel, result2);
//cout << "v=0, M*accel=" << result2 << endl;
//cout << "v=0, |M*accel-f|=" << (result2-randVec).norm() << endl;
// Test forward and inverse dynamics operators.
// Apply random forces and a random prescribed acceleration to
// get back the residual generalized forces. Then applying those
// should result in zero residual, and applying them.
// Randomize state.
state.updQ() = Test::randVector(nq);
state.updU() = Test::randVector(nu);
// Inverse dynamics should require realization only to Velocity stage.
system.realize(state, Stage::Velocity);
// Randomize body forces.
Vector_<SpatialVec> bodyForces(nb);
for (int i=0; i < nb; ++i)
bodyForces[i] = Test::randSpatialVec();
// Random mobility forces and known udots.
Vector mobilityForces = Test::randVector(nu);
Vector knownUdots = Test::randVector(nu);
// Check self consistency: compute residual, apply it, should be no remaining residual.
Vector residualForces, shouldBeZeroResidualForces;
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, bodyForces, knownUdots, residualForces);
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces+residualForces, bodyForces, knownUdots, shouldBeZeroResidualForces);
SimTK_TEST(shouldBeZeroResidualForces.norm() <= Slop);
// Now apply these forces in forward dynamics and see if we get the desired
// acceleration. State must be realized to Dynamics stage.
system.realize(state, Stage::Dynamics);
Vector udots;
Vector_<SpatialVec> bodyAccels;
matter.calcAccelerationIgnoringConstraints(state,
mobilityForces+residualForces, bodyForces, udots, bodyAccels);
SimTK_TEST_EQ_TOL(udots, knownUdots, Slop);
// See if we get back the same body accelerations by feeding in
// these udots.
Vector_<SpatialVec> A_GB, AC_GB;
matter.calcBodyAccelerationFromUDot(state, udots, A_GB);
SimTK_TEST_EQ_TOL(A_GB, bodyAccels, Slop);
// Collect coriolis accelerations.
AC_GB.resize(matter.getNumBodies());
for (MobodIndex i(0); i<nb; ++i)
AC_GB[i] = matter.getTotalCoriolisAcceleration(state, i);
// Verify that either a zero-length or all-zero udot gives just
// coriolis accelerations.
matter.calcBodyAccelerationFromUDot(state, Vector(), A_GB);
SimTK_TEST_EQ_TOL(A_GB, AC_GB, Slop);
Vector allZeroUdot(matter.getNumMobilities(), Real(0));
matter.calcBodyAccelerationFromUDot(state, allZeroUdot, A_GB);
SimTK_TEST_EQ_TOL(A_GB, AC_GB, Slop);
// Now let's test noncontiguous input and output vectors.
Matrix MatUdot(3, nu); // use middle row
MatUdot.setToNaN();
MatUdot[1] = ~udots;
Matrix_<SpatialRow> MatA_GB(3, nb); // use middle row
MatA_GB.setToNaN();
matter.calcBodyAccelerationFromUDot(state, ~MatUdot[1], ~MatA_GB[1]);
SimTK_TEST_EQ_TOL(MatA_GB[1], ~bodyAccels, Slop);
// Verify that leaving out arguments makes them act like zeroes.
Vector residualForces1, residualForces2;
matter.calcResidualForceIgnoringConstraints(state,
0*mobilityForces, 0*bodyForces, 0*knownUdots, residualForces1);
// no, the residual is not zero here because of the angular velocities
matter.calcResidualForceIgnoringConstraints(state,
Vector(), Vector_<SpatialVec>(), Vector(), residualForces2);
SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
// We just calculated f_residual = M udot + f_inertial - f_applied, with
// both udot and f_applied zero, i.e. f_residual=f_inertial. That should
// be the same as what is returned by getTotalCentrifugalForces().
Vector_<SpatialVec> F_inertial(nb);
Vector f_inertial;
for (MobodIndex i(0); i<nb; ++i)
F_inertial[i] = matter.getTotalCentrifugalForces(state, i);
matter.multiplyBySystemJacobianTranspose(state, F_inertial, f_inertial);
SimTK_TEST_EQ_TOL(f_inertial, residualForces1, Slop);
// This should also match total Mass*Coriolis acceleration + gyro force.
Vector_<SpatialVec> F_coriolis(nb), F_gyro(nb), F_total(nb);
Vector f_total;
for (MobodIndex i(0); i<nb; ++i) {
if (i==0) F_coriolis[i] = SpatialVec(Vec3(0),Vec3(0));
else
F_coriolis[i] = matter.getMobilizedBody(i)
.getBodySpatialInertiaInGround(state) * AC_GB[i];
F_gyro[i] = matter.getGyroscopicForce(state, i);
}
F_total = F_coriolis + F_gyro;
SimTK_TEST_EQ_TOL(F_inertial, F_total, Slop);
// Same, but leave out combinations of arguments.
matter.calcResidualForceIgnoringConstraints(state,
0*mobilityForces, bodyForces, knownUdots, residualForces1);
matter.calcResidualForceIgnoringConstraints(state,
Vector(), bodyForces, knownUdots, residualForces2);
SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, 0*bodyForces, knownUdots, residualForces1);
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, Vector_<SpatialVec>(), knownUdots, residualForces2);
SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, bodyForces, 0*knownUdots, residualForces1);
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, bodyForces, Vector(), residualForces2);
SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
matter.calcResidualForceIgnoringConstraints(state,
0*mobilityForces, bodyForces, 0*knownUdots, residualForces1);
matter.calcResidualForceIgnoringConstraints(state,
Vector(), bodyForces, Vector(), residualForces2);
SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, 0*bodyForces, 0*knownUdots, residualForces1);
matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, Vector_<SpatialVec>(), Vector(), residualForces2);
SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
// Check that we object to wrong-length arguments.
SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
Vector(3,Zero), bodyForces, knownUdots, residualForces2));
SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, Vector_<SpatialVec>(5), knownUdots, residualForces2));
SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
mobilityForces, bodyForces, Vector(2), residualForces2));
}
int main() {
try { // catch errors if any
// Create the system, with subsystems for the bodies and some forces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.8);
system.setUseUniformBackground(true); // request no ground & sky
// Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
Real mass = 3; Vec3 pos(0,-3,0);
Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));
// Create the tree of mobilized bodies, reusing the above body description.
MobilizedBody::Pin bodyT (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0), bodyInfo, Vec3(0));
MobilizedBody::Pin rtArm (bodyT, Vec3(2, 0, 0), bodyInfo, Vec3(0,-1,0));
// Add some damping.
Force::MobilityLinearDamper damper1(forces, bodyT, MobilizerUIndex(0), 10);
Force::MobilityLinearDamper damper2(forces, leftArm, MobilizerUIndex(0), 30);
Force::MobilityLinearDamper damper3(forces, rtArm, MobilizerUIndex(0), 10);
#ifdef USE_TORQUE_LIMITED_MOTOR
MyTorqueLimitedMotor* motorp =
new MyTorqueLimitedMotor(rtArm, MobilizerUIndex(0), TorqueGain, MaxTorque);
const MyTorqueLimitedMotor& motor = *motorp;
Force::Custom(forces, motorp); // takes over ownership
#else
// Use built-in Steady Motion as a low-budget motor model.
//Motion::Steady motor(rtArm, InitialMotorRate);
// Use built-in ConstantSpeed constraint as a low-budget motor model.
Constraint::ConstantSpeed motor(rtArm, InitialMotorRate);
#endif
// Add a joint stop to the left arm restricting it to q in [0,Pi/5].
Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0),
StopStiffness,
InitialDissipation,
-Pi/8, // lower stop
Pi/8); // upper stop
Visualizer viz(system);
// Add sliders.
viz.addSlider("Motor speed", SliderIdMotorSpeed, -10, 10, InitialMotorRate);
viz.addSlider("Dissipation", SliderIdDissipation, 0, 10, InitialDissipation);
viz.addSlider("Tach", SliderIdTach, -20, 20, 0);
viz.addSlider("Torque", SliderIdTorque, -MaxTorque, MaxTorque, 0);
// Add Run menu.
Array_<std::pair<String,int> > runMenuItems;
runMenuItems.push_back(std::make_pair("Reset", ResetItem));
runMenuItems.push_back(std::make_pair("Quit", QuitItem));
viz.addMenu("Run", MenuIdRun, runMenuItems);
Visualizer::InputSilo* userInput = new Visualizer::InputSilo();
viz.addInputListener(userInput);
// Initialize the system and state.
State initState = system.realizeTopology();
// Simulate forever with a small max step size. Check for user input
// in between steps. Note: an alternate way to do this is to let the
// integrator take whatever steps it wants but use a TimeStepper to
// manage a periodic event handler to poll for user input. Here we're
// treating completion of a step as an event.
const Real MaxStepSize = 0.01*3; // 10ms
const int DrawEveryN = 3/3; // 3 steps = 30ms
//RungeKuttaMersonIntegrator integ(system);
//RungeKutta2Integrator integ(system);
SemiExplicitEuler2Integrator integ(system);
//SemiExplicitEulerIntegrator integ(system, .001);
integ.setAccuracy(1e-1);
//integ.setAccuracy(1e-3);
// Don't permit interpolation because we want the state returned after
// a step to be modifiable.
integ.setAllowInterpolation(false);
integ.initialize(initState);
int stepsSinceViz = DrawEveryN-1;
while (true) {
if (++stepsSinceViz % DrawEveryN == 0) {
const State& s = integ.getState();
viz.report(s);
const Real uActual = rtArm.getOneU(s, MobilizerUIndex(0));
viz.setSliderValue(SliderIdTach, uActual);
#ifdef USE_TORQUE_LIMITED_MOTOR
viz.setSliderValue(SliderIdTorque, motor.getTorque(s));
#else
system.realize(s); // taus are acceleration stage
//viz.setSliderValue(SliderIdTorque,
// rtArm.getOneTau(s, MobilizerUIndex(0)));
viz.setSliderValue(SliderIdTorque, motor.getMultiplier(s));
#endif
stepsSinceViz = 0;
}
// Advance time by MaxStepSize (might take multiple steps to get there).
integ.stepBy(MaxStepSize);
// Now poll for user input.
int whichSlider, whichMenu, whichItem; Real newValue;
// Did a slider move?
if (userInput->takeSliderMove(whichSlider, newValue)) {
State& state = integ.updAdvancedState();
switch(whichSlider) {
case SliderIdMotorSpeed:
// TODO: momentum balance?
//motor.setRate(state, newValue);
motor.setSpeed(state, newValue);
system.realize(state, Stage::Position);
system.prescribeU(state);
system.realize(state, Stage::Velocity);
system.projectU(state);
break;
case SliderIdDissipation:
stop.setMaterialProperties(state, StopStiffness, newValue);
system.realize(state, Stage::Position);
break;
}
}
// Was there a menu pick?
if (userInput->takeMenuPick(whichMenu, whichItem)) {
if (whichItem == QuitItem)
break; // done
// If Reset, stop the motor and restore default dissipation.
// Tell visualizer to update the sliders to match.
// Zero out all the q's and u's.
if (whichItem == ResetItem) {
State& state = integ.updAdvancedState();
//motor.setRate(state, 0);
motor.setSpeed(state, 0);
viz.setSliderValue(SliderIdMotorSpeed, 0);
stop.setMaterialProperties(state, StopStiffness, InitialDissipation);
viz.setSliderValue(SliderIdDissipation, InitialDissipation);
state.updQ() = 0; // all positions to zero
state.updU() = 0; // all velocities to zero
system.realize(state, Stage::Position);
system.prescribeU(state);
system.realize(state, Stage::Velocity);
system.projectU(state);
}
}
}
const int evals = integ.getNumRealizations();
std::cout << "Done -- simulated " << integ.getTime() << "s with "
<< integ.getNumStepsTaken() << " steps, avg step="
<< (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
<< (1000*integ.getTime())/evals << "ms/eval\n";
printf("Used Integrator %s at accuracy %g:\n",
integ.getMethodName(), integ.getAccuracyInUse());
printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
int main() {
try {
// Create the system, with subsystems for the bodies and some forces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
// Hint to Visualizer: don't show ground plane.
system.setUseUniformBackground(true);
// Add gravity as a force element.
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));
// Create the body and some artwork for it.
const Vec3 halfLengths(.5, .1, .25); // half-size of brick (m)
const Real mass = 2; // total mass of brick (kg)
Body::Rigid pendulumBody(MassProperties(mass, Vec3(0),
mass*UnitInertia::brick(halfLengths)));
pendulumBody.addDecoration(Transform(),
DecorativeBrick(halfLengths).setColor(Red));
// Add an instance of the body to the multibody system by connecting
// it to Ground via a Ball mobilizer.
MobilizedBody::Ball pendulum1(matter.updGround(), Transform(Vec3(-1,-1, 0)),
pendulumBody, Transform(Vec3( 0, 1, 0)));
// Add a second instance of the pendulum nearby.
MobilizedBody::Ball pendulum2(matter.updGround(), Transform(Vec3(1,-1, 0)),
pendulumBody, Transform(Vec3(0, 1, 0)));
// Connect the origins of the two pendulum bodies together with our
// rod-like custom constraint.
const Real d = 1.5; // desired separation distance
Constraint::Custom rod(new ExampleConstraint(pendulum1, pendulum2, d));
// Visualize with default options.
Visualizer viz(system);
// Add a rubber band line connecting the origins of the two bodies to
// represent the rod constraint.
viz.addRubberBandLine(pendulum1, Vec3(0), pendulum2, Vec3(0),
DecorativeLine().setColor(Blue).setLineThickness(3));
// Ask for a report every 1/30 of a second to match the Visualizer's
// default rate of 30 frames per second.
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// Output total energy to the console once per second.
system.addEventReporter(new TextDataEventReporter
(system, new MyEvaluateEnergy(), 1.0));
// Initialize the system and state.
State state = system.realizeTopology();
// Orient the two pendulums asymmetrically so they'll do something more
// interesting than just hang there.
pendulum1.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
pendulum2.setQToFitRotation(state, Rotation(BodyRotationSequence,
Pi/4, ZAxis, Pi/4, YAxis));
// Evaluate the system at the new state and draw one frame manually.
system.realize(state);
viz.report(state);
// Simulate it.
cout << "Hit ENTER to run a short simulation.\n";
cout << "(Energy should be conserved to about four decimal places.)\n";
getchar();
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(1e-4); // ask for about 4 decimal places (default is 3)
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(10.0);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
} catch (...) {
std::cout << "UNKNOWN EXCEPTION\n";
return 1;
}
return 0;
}
int main(int argc, char** argv) {
std::vector<State> saveEm;
saveEm.reserve(1000);
try // If anything goes wrong, an exception will be thrown.
{ int nseg = NSegments;
int shouldFlop = 0;
if (argc > 1) sscanf(argv[1], "%d", &nseg);
if (argc > 2) sscanf(argv[2], "%d", &shouldFlop);
// Create a multibody system using Simbody.
MultibodySystem mbs;
MyRNAExample myRNA(mbs, nseg, shouldFlop != 0);
GeneralForceSubsystem forces(mbs);
Force::UniformGravity ugs(forces, myRNA, Vec3(0, -g, 0), -0.8);
const Vec3 attachPt(150, -40, -50);
Force::TwoPointLinearSpring(forces, myRNA.Ground(), attachPt,
myRNA.getMobilizedBody(MobilizedBodyIndex(myRNA.getNumBodies()-1)), Vec3(0),
1000., // stiffness
1.); // natural length
Force::GlobalDamper(forces, myRNA, 1000);
State s = mbs.realizeTopology();
//myRNA.getConstraint(ConstraintIndex(myRNA.getNConstraints()-4)).disable(s);
//myRNA.setUseEulerAngles(s,true);
mbs.realizeModel(s);
mbs.realize(s, Stage::Position);
for (ConstraintIndex cid(0); cid < myRNA.getNumConstraints(); ++cid) {
const Constraint& c = myRNA.getConstraint(cid);
int mp,mv,ma;
c.getNumConstraintEquationsInUse(s, mp,mv,ma);
cout << "CONSTRAINT " << cid
<< " constrained bodies=" << c.getNumConstrainedBodies()
<< " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex()
<< " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers()
<< "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
<< " mp,mv,ma=" << mp << "," << mv << "," << ma
<< endl;
for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
cout << " constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex();
cout << endl;
}
for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
cout << " constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex()
<< ", q(" << c.getNumConstrainedQ(s, cmx) << ")=";
for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
cout << " " << c.getConstrainedQIndex(s, cmx, i);
cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
cout << " " << c.getConstrainedUIndex(s, cmx, i);
cout << endl;
}
cout << c.getSubtree();
cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
cout << " d(perrdot)/du=" << ~c.calcPositionConstraintMatrixPt(s);
cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
}
SimbodyMatterSubtree sub(myRNA);
sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(7)));
sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(10)));
//sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(20)));
sub.realizeTopology();
cout << "sub.ancestor=" << sub.getAncestorMobilizedBodyIndex();
// cout << " sub.terminalBodies=" << sub.getTerminalBodies() << endl;
// cout << "sub.allBodies=" << sub.getAllBodies() << endl;
for (SubtreeBodyIndex i(0); i < (int)sub.getAllBodies().size(); ++i) {
cout << "sub.parent[" << i << "]=" << sub.getParentSubtreeBodyIndex(i);
// cout << " sub.children[" << i << "]=" << sub.getChildSubtreeBodyIndexs(i) << endl;
}
printf("# quaternions in use = %d\n", myRNA.getNumQuaternionsInUse(s));
for (MobilizedBodyIndex i(0); i<myRNA.getNumBodies(); ++i) {
printf("body %2d: using quat? %s; quat index=%d\n",
(int)i, myRNA.isUsingQuaternion(s,i) ? "true":"false",
(int)myRNA.getQuaternionPoolIndex(s,i));
}
// And a study using the Runge Kutta Merson integrator
bool suppressProject = false;
RungeKuttaMersonIntegrator myStudy(mbs);
//RungeKuttaFeldbergIntegrator myStudy(mbs);
//RungeKutta3Integrator myStudy(mbs);
//CPodesIntegrator myStudy(mbs);
//VerletIntegrator myStudy(mbs);
//ExplicitEulerIntegrator myStudy(mbs);
myStudy.setAccuracy(1e-2);
myStudy.setConstraintTolerance(1e-3);
myStudy.setProjectEveryStep(false);
Visualizer display(mbs);
display.setBackgroundColor(White);
display.setBackgroundType(Visualizer::SolidColor);
display.setMode(Visualizer::RealTime);
for (MobilizedBodyIndex i(1); i<myRNA.getNumBodies(); ++i)
myRNA.decorateBody(i, display);
myRNA.decorateGlobal(display);
DecorativeLine rbProto; rbProto.setColor(Orange).setLineThickness(3);
display.addRubberBandLine(GroundIndex, attachPt,MobilizedBodyIndex(myRNA.getNumBodies()-1),Vec3(0), rbProto);
//display.addRubberBandLine(GroundIndex, -attachPt,myRNA.getNumBodies()-1,Vec3(0), rbProto);
const Real dt = 1./30; // output intervals
printf("time nextStepSize\n");
s.updTime() = 0;
for (int i=0; i<50; ++i)
saveEm.push_back(s); // delay
display.report(s);
myStudy.initialize(s);
cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
cout << "U WEIGHTS=" << s.getUWeights() << endl;
cout << "Z WEIGHTS=" << s.getZWeights() << endl;
cout << "1/QTOLS=" << s.getQErrWeights() << endl;
cout << "1/UTOLS=" << s.getUErrWeights() << endl;
saveEm.push_back(myStudy.getState());
for (int i=0; i<50; ++i)
saveEm.push_back(myStudy.getState()); // delay
display.report(myStudy.getState());
const double startReal = realTime(), startCPU = cpuTime();
int stepNum = 0;
for (;;) {
const State& ss = myStudy.getState();
mbs.realize(ss);
if ((stepNum++%100)==0) {
printf("%5g qerr=%10.4g uerr=%10.4g hNext=%g\n", ss.getTime(),
myRNA.getQErr(ss).normRMS(), myRNA.getUErr(ss).normRMS(),
myStudy.getPredictedNextStepSize());
printf(" E=%14.8g (pe=%10.4g ke=%10.4g)\n",
mbs.calcEnergy(ss), mbs.calcPotentialEnergy(ss), mbs.calcKineticEnergy(ss));
cout << "QERR=" << ss.getQErr() << endl;
cout << "UERR=" << ss.getUErr() << endl;
}
//if (s.getTime() - std::floor(s.getTime()) < 0.2)
// display.addEphemeralDecoration( DecorativeSphere(10).setColor(Green) );
display.report(ss);
saveEm.push_back(ss);
if (ss.getTime() >= 10)
break;
// TODO: should check for errors or have or teach RKM to throw.
myStudy.stepTo(ss.getTime() + dt, Infinity);
}
printf("CPU time=%gs, REAL time=%gs\n", cpuTime()-startCPU, realTime()-startReal);
printf("Using Integrator %s:\n", myStudy.getMethodName());
printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
printf("# CONVERGENCE FAILS = %d\n", myStudy.getNumConvergenceTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
printf("# PROJECTION FAILS = %d\n", myStudy.getNumProjectionFailures());
display.dumpStats(std::cout);
while(true) {
for (int i=0; i < (int)saveEm.size(); ++i) {
display.report(saveEm[i]);
//display.report(saveEm[i]); // half speed
}
getchar();
}
}
catch (const exception& e)
{
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
}
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 testRel2Cart() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
// Pendulum of length 1, initially along -x like this:
// B ----------- * O
// -1,0,0 0,0,0
// At q=0, partial(B)/partial(u) = 0,-1,0.
// At q=pi/2, partial(B)/partial(u) = 1,0,0.
// Then try this with station S=(0,1,0)_B:
// S
// |
// |
// B ------ * O
// Now |OS| = sqrt(2). At q=Pi/4, S will be horizontal
// so partial(S)/partial(u) = (0, -sqrt(2)/2, 0).
//
// In all cases the partial angular velocity is (0,0,1).
//
MobilizedBody::Pin pinBody
(matter.Ground(), Transform(),
MassProperties(1,Vec3(0),Inertia(1)), Vec3(1,0,0));
State s = system.realizeTopology();
RowVector_<Vec3> dvdu, dvdu2, dvdu3;
RowVector_<Vec3> JS;
RowVector_<SpatialVec> dvdu4, JF;
Matrix_<SpatialVec> J, Jn;
Matrix JSf, JFf, Jf; // flat
// We'll compute Jacobians for the body origin Bo in 2 configurations,
// q==0,pi/2 then for station S (0,1,0) at q==pi/4. Not
// much of a test, I know, but at least we know the right answer.
// q == 0; answer is JB == (0,-1,0)
pinBody.setQ(s, 0);
system.realize(s, Stage::Position);
sbrel2cart(s, matter, pinBody, Vec3(0), dvdu);
SimTK_TEST_EQ(dvdu[0], Vec3(0,-1,0));
sbrel2cart2(s, matter, pinBody, Vec3(0), dvdu2);
SimTK_TEST_EQ(dvdu2[0], Vec3(0,-1,0));
sbrel2cart3(s, matter, pinBody, Vec3(0), dvdu3);
SimTK_TEST_EQ(dvdu3[0], Vec3(0,-1,0));
matter.calcStationJacobian(s, pinBody, Vec3(0), JS);
matter.calcStationJacobian(s, pinBody, Vec3(0), JSf);
SimTK_TEST_EQ(JS, dvdu3); // == dvdu2 == dvdu
compareElementwise(JS, JSf);
sbrel2cart4(s, matter, pinBody, Vec3(0), dvdu4);
SimTK_TEST_EQ(dvdu4[0][0], Vec3(0,0,1));
SimTK_TEST_EQ(dvdu4[0][1], Vec3(0,-1,0));
matter.calcFrameJacobian(s, pinBody, Vec3(0), JF);
matter.calcFrameJacobian(s, pinBody, Vec3(0), JFf);
SimTK_TEST_EQ(dvdu4, JF);
compareElementwise(JF, JFf);
// Calculate the whole system Jacobian at q==0 in two different
// representations and make sure they are the same.
matter.calcSystemJacobian(s, J);
matter.calcSystemJacobian(s, Jf);
compareElementwise(J, Jf);
// q == 90 degrees; answer is JB == (1,0,0)
pinBody.setQ(s, Pi/2);
system.realize(s, Stage::Position);
sbrel2cart(s, matter, pinBody, Vec3(0), dvdu);
SimTK_TEST_EQ(dvdu[0], Vec3(1,0,0));
sbrel2cart2(s, matter, pinBody, Vec3(0), dvdu2);
SimTK_TEST_EQ(dvdu2[0], Vec3(1,0,0));
sbrel2cart3(s, matter, pinBody, Vec3(0), dvdu3);
SimTK_TEST_EQ(dvdu3[0], Vec3(1,0,0));
matter.calcStationJacobian(s, pinBody, Vec3(0), JS);
matter.calcStationJacobian(s, pinBody, Vec3(0), JSf);
SimTK_TEST_EQ(JS, dvdu3); // == dvdu2 == dvdu
compareElementwise(JS, JSf);
sbrel2cart4(s, matter, pinBody, Vec3(0), dvdu4);
SimTK_TEST_EQ(dvdu4[0][0], Vec3(0,0,1));
SimTK_TEST_EQ(dvdu4[0][1], Vec3(1,0,0));
matter.calcFrameJacobian(s, pinBody, Vec3(0), JF);
matter.calcFrameJacobian(s, pinBody, Vec3(0), JFf);
SimTK_TEST_EQ(dvdu4, JF);
compareElementwise(JF, JFf);
// Calculate the whole system Jacobian at q==pi/2 in two different
// representations and make sure they are the same.
matter.calcSystemJacobian(s, J);
matter.calcSystemJacobian(s, Jf);
compareElementwise(J, Jf);
// now station S, q == 45 degrees; answer is JS == (0,-sqrt(2),0)
pinBody.setQ(s, Pi/4);
system.realize(s, Stage::Position);
sbrel2cart(s, matter, pinBody, Vec3(0,1,0), dvdu);
SimTK_TEST_EQ(dvdu[0], Vec3(0,-Sqrt2,0));
sbrel2cart2(s, matter, pinBody, Vec3(0,1,0), dvdu2);
SimTK_TEST_EQ(dvdu2[0], Vec3(0,-Sqrt2,0));
sbrel2cart3(s, matter, pinBody, Vec3(0,1,0), dvdu3);
SimTK_TEST_EQ(dvdu3[0], Vec3(0,-Sqrt2,0));
matter.calcStationJacobian(s, pinBody, Vec3(0,1,0), JS);
matter.calcStationJacobian(s, pinBody, Vec3(0,1,0), JSf);
SimTK_TEST_EQ(JS, dvdu3); // == dvdu2 == dvdu
compareElementwise(JS, JSf);
// Calculate station Jacobian JS by multiplication to test that the
// multiplyByStationJacobian[Transpose] methods are working.
Vec3 JSn; // 3xnu
Vector u(1); u[0] = 1.;
JSn = matter.multiplyByStationJacobian(s, pinBody, Vec3(0,1,0), u);
SimTK_TEST_EQ(JSn, dvdu3[0]);
Vec3 FS(0);
Row3 JSnt;
for (int i=0; i<3; ++i) {
FS[i] = 1;
matter.multiplyByStationJacobianTranspose(s,pinBody,Vec3(0,1,0),
FS, u);
FS[i] = 0;
JSnt[i] = u[0];
}
SimTK_TEST_EQ(JSnt, ~dvdu3[0]);
sbrel2cart4(s, matter, pinBody, Vec3(0,1,0), dvdu4);
SimTK_TEST_EQ(dvdu4[0][0], Vec3(0,0,1));
SimTK_TEST_EQ(dvdu4[0][1], Vec3(0,-Sqrt2,0));
matter.calcFrameJacobian(s, pinBody, Vec3(0,1,0), JF);
matter.calcFrameJacobian(s, pinBody, Vec3(0,1,0), JFf);
SimTK_TEST_EQ(dvdu4, JF);
compareElementwise(JF, JFf);
// Calculate frame Jacobian JF by multiplication to test that the
// multiplyByFrameJacobian[Transpose] methods are working.
SpatialVec JFn; // 6xnu
u[0] = 1.;
JFn = matter.multiplyByFrameJacobian(s, pinBody, Vec3(0,1,0), u);
SimTK_TEST_EQ(JFn, dvdu4[0]);
SpatialVec FF(Vec3(0));
SpatialRow JFnt;
for (int i=0; i<6; ++i) {
FF[i/3][i%3] = 1;
matter.multiplyByFrameJacobianTranspose(s,pinBody,Vec3(0,1,0),
FF, u);
FF[i/3][i%3] = 0;
JFnt[i/3][i%3] = u[0];
}
SimTK_TEST_EQ(JFnt, ~dvdu4[0]);
// Calculate the whole system Jacobian at q==pi/4 in two different
// representations and make sure they are the same.
matter.calcSystemJacobian(s, J);
matter.calcSystemJacobian(s, Jf);
compareElementwise(J, Jf);
// Generate the whole system Jacobian one body at a time by multiplication.
Jn.resize(matter.getNumBodies(), matter.getNumMobilities());
u[0] = 1;
for (MobodIndex i(0); i < matter.getNumBodies(); ++i)
Jn[i] = matter.multiplyByFrameJacobian(s,i,Vec3(0),u);
SimTK_TEST_EQ(Jn, J);
}
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 {
// 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(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
// CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
MultibodySystem mbs;
SimbodyMatterSubsystem matter(mbs);
GeneralForceSubsystem forces(mbs);
DecorationSubsystem viz(mbs);
Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));
// ADD BODIES AND THEIR MOBILIZERS
Body::Rigid body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::brick(hl[0],hl[1],hl[2])));
body.addDecoration(DecorativeBrick(hl).setOpacity(.5));
body.addDecoration(DecorativeLine(Vec3(0), Vec3(0,1,0)).setColor(Green));
MobilizedBody::Free mobilizedBody(matter.Ground(), Transform(), body, Transform());
MobilizedBody::Free mobilizedBody0(mobilizedBody, Transform(Vec3(1,2,0)), body, Transform(Vec3(0,1,0)));
MobilizedBody::Free mobilizedBody2(mobilizedBody0, Vec3(-5,0,0), body, Transform());
Body::Rigid gear1body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(.5, .1)));
gear1body.addDecoration(DecorativeCircle(.5).setColor(Green).setOpacity(.7));
gear1body.addDecoration(DecorativeLine(Vec3(0), Vec3(.5,0,0)).setColor(Black).setLineThickness(4));
Body::Rigid gear2body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(1.5, .1)));
gear2body.addDecoration(Transform(), DecorativeCircle(1.5).setColor(Blue).setOpacity(.7));
gear2body.addDecoration(Transform(), DecorativeLine(Vec3(0), Vec3(1.5,0,0)).setColor(Black).setLineThickness(4));
MobilizedBody::Pin gear1(mobilizedBody2, Vec3(-1,0,0), gear1body, Transform()); // along z
MobilizedBody::Pin gear2(mobilizedBody2, Vec3(1,0,0), gear2body, Transform()); // along z
Constraint::NoSlip1D(mobilizedBody2, Vec3(-.5,0,0), UnitVec3(0,1,0), gear1, gear2);
Constraint::ConstantSpeed(gear1, 100.);
//Constraint::Ball myc2(matter.Ground(), Vec3(-4,2,0), mobilizedBody2, Vec3(0,1,0));
Constraint::Weld myc(matter.Ground(), Vec3(1,2,0), mobilizedBody, Vec3(0,1,0));
Constraint::Ball ball1(mobilizedBody, Vec3(2,0,0), mobilizedBody0, Vec3(3,1,1));
Constraint::Ball ball2(mobilizedBody0, Vec3(2,0,0), mobilizedBody2, Vec3(3,0,0));
//Constraint::PointInPlane pip(mobilizedBody0, UnitVec3(0,-1,0), 3, mobilizedBody2, Vec3(3,0,0));
//Constraint::ConstantOrientation ori(mobilizedBody, Rotation(), mobilizedBody0, Rotation());
//Constraint::ConstantOrientation ori2(mobilizedBody2, Rotation(), mobilizedBody0, Rotation());
//Constraint::Weld weld(mobilizedBody, Transform(Rotation(Pi/4, ZAxis), Vec3(1,1,0)),
// mobilizedBody2, Transform(Rotation(-Pi/4, ZAxis), Vec3(-1,-1,0)));
//MyConstraint xyz(gear1, 100.);
viz.addBodyFixedDecoration(mobilizedBody, Transform(Vec3(1,2,3)), DecorativeText("hello world").setScale(.1));
/*
class MyHandler : public ScheduledEventHandler {
public:
MyHandler(const Constraint& cons) : c(cons) { }
Real getNextEventTime(const State&, bool includeCurrentTime) const {
return .314;
}
void handleEvent(State& s, Real acc, const Vector& ywts, const Vector& cwts, Stage& modified,
bool& shouldTerminate) const
{
cout << "<<<< TRIGGERED AT T=" << s.getTime() << endl;
c.enable(s);
modified = Stage::Model;
}
private:
const Constraint& c;
};
mbs.addEventHandler(new MyHandler(xyz));
*/
State s = mbs.realizeTopology(); // returns a reference to the the default state
//xyz.disable(s);
//matter.setUseEulerAngles(s, true);
mbs.realizeModel(s); // define appropriate states for this System
//mobilizedBody0.setQ(s, .1);
//mobilizedBody.setQ(s, .2);
Visualizer display(mbs);
display.setBackgroundColor(White);
display.setBackgroundType(Visualizer::SolidColor);
mbs.realize(s, Stage::Velocity);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
cout << "qErr=" << s.getQErr() << endl;
cout << "uErr=" << s.getUErr() << endl;
for (ConstraintIndex cid(0); cid < matter.getNumConstraints(); ++cid) {
const Constraint& c = matter.getConstraint(cid);
int mp,mv,ma;
c.getNumConstraintEquationsInUse(s, mp,mv,ma);
cout << "CONSTRAINT " << cid << (c.isDisabled(s) ? "**DISABLED** " : "")
<< " constrained bodies=" << c.getNumConstrainedBodies();
if (c.getNumConstrainedBodies()) cout << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex();
cout << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers()
<< "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
<< " mp,mv,ma=" << mp << "," << mv << "," << ma
<< endl;
for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
cout << " constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex();
cout << endl;
}
for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
cout << " constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex()
<< ", q(" << c.getNumConstrainedQ(s, cmx) << ")=";
for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
cout << " " << c.getConstrainedQIndex(s, cmx, i);
cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
cout << " " << c.getConstrainedUIndex(s, cmx, i);
cout << endl;
}
cout << c.getSubtree();
if (mp) {
cout << "perr=" << c.getPositionErrorsAsVector(s) << endl;
cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
cout << " ~d(Pt lambda)/dlambda=" << ~c.calcPositionConstraintMatrixPt(s);
cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
Matrix P = c.calcPositionConstraintMatrixP(s);
Matrix PQ(mp,matter.getNQ(s));
Vector out(matter.getNQ(s));
for (int i=0; i<mp; ++i) {
Vector in = ~P[i];
matter.multiplyByNInv(s, true, in, out);
PQ[i] = ~out;
}
cout << " calculated d(perr)/dq=" << PQ;
}
if (mv) {
cout << "verr=" << c.getVelocityErrorsAsVector(s) << endl;
//cout << " d(verrdot)/dudot=" << c.calcVelocityConstraintMatrixV(s);
cout << " ~d(Vt lambda)/dlambda=" << ~c.calcVelocityConstraintMatrixVt(s);
}
}
const Constraint& c = matter.getConstraint(myc.getConstraintIndex());
cout << "Default configuration shown. Ready? "; getchar();
mobilizedBody.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,1,1)),Vec3(.1,.2,.3)));
mobilizedBody0.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,-1,1)),Vec3(.2,.2,.3)));
mobilizedBody2.setQToFitTransform (s, Transform(Rotation(.05,Vec3(-1,1,1)),Vec3(.1,.2,.1)));
mobilizedBody.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
mobilizedBody0.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
mobilizedBody2.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
//gear1.setUToFitAngularVelocity(s, Vec3(0,0,500)); // these should be opposite directions!
//gear2.setUToFitAngularVelocity(s, Vec3(0,0,100));
mbs.realize(s, Stage::Velocity);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
cout << "qErr=" << s.getQErr() << endl;
cout << "uErr=" << s.getUErr() << endl;
cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
cout << "v_MbM=" << mobilizedBody.getMobilizerVelocity(s)[1] << endl;
cout << "Unassembled configuration shown. Ready to assemble? "; getchar();
// These are the SimTK Simmath integrators:
RungeKuttaMersonIntegrator myStudy(mbs);
//CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::/*Newton*/Functional);
//myStudy.setOrderLimit(2); // cpodes only
//VerletIntegrator myStudy(mbs);
// ExplicitEulerIntegrator myStudy(mbs, .0005); // fixed step
//ExplicitEulerIntegrator myStudy(mbs); // variable step
//myStudy.setMaximumStepSize(0.001);
myStudy.setAccuracy(1e-6); myStudy.setAccuracy(1e-1);
//myStudy.setProjectEveryStep(true);
//myStudy.setProjectInterpolatedStates(false);
myStudy.setConstraintTolerance(1e-7); myStudy.setConstraintTolerance(1e-2);
//myStudy.setAllowInterpolation(false);
//myStudy.setMaximumStepSize(.1);
const Real dt = .02; // output intervals
const Real finalTime = 2;
myStudy.setFinalTime(finalTime);
std::vector<State> saveEm;
saveEm.reserve(2000);
for (int i=0; i<50; ++i)
saveEm.push_back(s); // delay
// Peforms assembly if constraints are violated.
myStudy.initialize(s);
for (int i=0; i<50; ++i)
saveEm.push_back(s); // delay
cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
cout << "U WEIGHTS=" << s.getUWeights() << endl;
cout << "Z WEIGHTS=" << s.getZWeights() << endl;
cout << "1/QTOLS=" << s.getQErrWeights() << endl;
cout << "1/UTOLS=" << s.getUErrWeights() << endl;
{
const State& s = myStudy.getState();
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
cout << "qErr=" << s.getQErr() << endl;
cout << "uErr=" << s.getUErr() << endl;
cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
cout << "PE=" << mbs.calcPotentialEnergy(s) << " KE=" << mbs.calcKineticEnergy(s) << " E=" << mbs.calcEnergy(s) << endl;
cout << "angle=" << std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1)) << endl;
cout << "Assembled configuration shown. Ready to simulate? "; getchar();
}
Integrator::SuccessfulStepStatus status;
int nextReport = 0;
mbs.resetAllCountersToZero();
int stepNum = 0;
while ((status=myStudy.stepTo(nextReport*dt))
!= Integrator::EndOfSimulation)
{
const State& s = myStudy.getState();
mbs.realize(s, Stage::Acceleration);
if ((stepNum++%10)==0) {
const Real angle = std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1));
printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
angle,
mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
myStudy.getPreviousStepSizeTaken(),
Integrator::getSuccessfulStepStatusString(status).c_str(),
myStudy.isStateInterpolated()?" (INTERP)":"");
printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
matter.getQErr(s).normRMS(),
matter.getUErr(s).normRMS(),
s.getSystemStage() >= Stage::Acceleration ? matter.getUDotErr(s).normRMS() : Real(-1));
#ifdef HASC
cout << "CONSTRAINT perr=" << c.getPositionError(s)
<< " verr=" << c.getVelocityError(s)
<< " aerr=" << c.getAccelerationError(s)
<< endl;
#endif
//cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
//cout << " ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
//cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
cout << "Q=" << matter.getQ(s) << endl;
cout << "U=" << matter.getU(s) << endl;
cout << "Multipliers=" << matter.getMultipliers(s) << endl;
}
Vector qdot;
matter.calcQDot(s, s.getU(), qdot);
// cout << "===> qdot =" << qdot << endl;
Vector qdot2;
matter.multiplyByN(s, false, s.getU(), qdot2);
// cout << "===> qdot2=" << qdot2 << endl;
Vector u1,u2;
matter.multiplyByNInv(s, false, qdot, u1);
matter.multiplyByNInv(s, false, qdot2, u2);
// cout << "===> u =" << s.getU() << endl;
// cout << "===> u1=" << u1 << endl;
// cout << "===> u2=" << u2 << endl;
// cout << " norm=" << (s.getU()-u2).normRMS() << endl;
display.report(s);
saveEm.push_back(s);
if (status == Integrator::ReachedReportTime)
++nextReport;
}
printf("Using Integrator %s:\n", myStudy.getMethodName());
printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
printf("System stats: realize %dP %dV %dA, projectQ %d, projectU %d\n",
mbs.getNumRealizationsOfThisStage(Stage::Position),
mbs.getNumRealizationsOfThisStage(Stage::Velocity),
mbs.getNumRealizationsOfThisStage(Stage::Acceleration),
mbs.getNumProjectQCalls(), mbs.getNumProjectUCalls());
while(true) {
for (int i=0; i < (int)saveEm.size(); ++i) {
display.report(saveEm[i]);
//display.report(saveEm[i]); // half speed
}
getchar();
}
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
catch (...) {
printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
}
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) {
static const Transform GroundFrame;
static const Rotation ZUp(UnitVec3(XAxis), XAxis, UnitVec3(YAxis), ZAxis);
static const Vec3 TestLoc(1,0,0);
try { // If anything goes wrong, an exception will be thrown.
// CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
MultibodySystem mbs;
SimbodyMatterSubsystem matter(mbs);
GeneralForceSubsystem forces(mbs);
DecorationSubsystem viz(mbs);
//Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));
// ADD BODIES AND THEIR MOBILIZERS
Body::Rigid particle = Body::Rigid(MassProperties(m, Vec3(0), Inertia(0)));
particle.addDecoration(DecorativeSphere(.1).setColor(Red).setOpacity(.3));
MobilizedBody::SphericalCoords
anAtom(matter.Ground(), Transform(ZUp, TestLoc),
particle, Transform(),
0*Deg2Rad, false, // azimuth offset, negated
0, false, // zenith offset, negated
ZAxis, true); // translation axis, negated
anAtom.setDefaultRadius(.1);
anAtom.setDefaultAngles(Vec2(0, 30*Deg2Rad));
viz.addRubberBandLine(matter.Ground(), TestLoc,
anAtom, Vec3(0),
DecorativeLine().setColor(Orange).setLineThickness(4));
Force::MobilityLinearSpring(forces, anAtom, 0, 2, -30*Deg2Rad); // harmonic bend
Force::MobilityLinearSpring(forces, anAtom, 1, 2, 45*Deg2Rad); // harmonic bend
Force::MobilityLinearSpring(forces, anAtom, 2, 20, .5); // harmonic stretch
Force::MobilityLinearDamper(forces, anAtom, 0, .1); // harmonic bend
Force::MobilityLinearDamper(forces, anAtom, 1, .1); // harmonic bend
Force::MobilityLinearDamper(forces, anAtom, 2, .1); // harmonic stretch
State s = mbs.realizeTopology(); // returns a reference to the the default state
mbs.realizeModel(s); // define appropriate states for this System
mbs.realize(s, Stage::Instance); // instantiate constraints if any
Visualizer display(mbs);
display.setBackgroundType(Visualizer::SolidColor);
mbs.realize(s, Stage::Velocity);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
char c;
cout << "Default configuration shown. 1234 to move on.\n";
//anAtom.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));
while (true) {
Real x;
cout << "Torsion (deg)? "; cin >> x; if (x==1234) break;
Vec2 a = anAtom.getAngles(s); a[0]=x*Deg2Rad; anAtom.setAngles(s, a);
display.report(s);
cout << "Bend (deg)? "; cin >> x; if (x==1234) break;
a = anAtom.getAngles(s); a[1]=x*Deg2Rad; anAtom.setAngles(s, a);
display.report(s);
cout << "Radius? "; cin >> x; if (x==1234) break;
anAtom.setRadius(s, x);
display.report(s);
}
anAtom.setUToFitAngularVelocity(s, Vec3(.1,.2,.3));
//anAtom.setAngle(s, 45*Deg2Rad);
//anAtom.setTranslation(s, Vec2(.4, .1));
mbs.realize(s, Stage::Dynamics);
mbs.realize(s, Stage::Acceleration);
cout << "q=" << s.getQ() << endl;
cout << "u=" << s.getU() << endl;
cout << "qdot=" << s.getQDot() << endl;
cout << "udot=" << s.getUDot() << endl;
cout << "qdotdot=" << s.getQDotDot() << endl;
display.report(s);
cout << "Initialized configuration shown. Ready? ";
cin >> c;
RungeKuttaMersonIntegrator myStudy(mbs);
myStudy.setAccuracy(1e-4);
const Real dt = .02; // output intervals
const Real finalTime = 20;
myStudy.setFinalTime(finalTime);
// Peforms assembly if constraints are violated.
myStudy.initialize(s);
cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
cout << "U WEIGHTS=" << s.getUWeights() << endl;
cout << "Z WEIGHTS=" << s.getZWeights() << endl;
cout << "1/QTOLS=" << s.getQErrWeights() << endl;
cout << "1/UTOLS=" << s.getUErrWeights() << endl;
Integrator::SuccessfulStepStatus status;
int nextReport = 0;
while ((status=myStudy.stepTo(nextReport*dt))
!= Integrator::EndOfSimulation)
{
const State& s = myStudy.getState();
mbs.realize(s);
printf("%5g %10.4g %10.4g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
anAtom.getAngles(s)[0], anAtom.getAngles(s)[1], anAtom.getRadius(s),
//anAtom.getAngle(s), anAtom.getTranslation(s)[0], anAtom.getTranslation(s)[1],
//anAtom.getQ(s)[0], anAtom.getQ(s)[1], anAtom.getQ(s)[2],
mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
myStudy.getPreviousStepSizeTaken(),
Integrator::getSuccessfulStepStatusString(status).c_str(),
myStudy.isStateInterpolated()?" (INTERP)":"");
display.report(s);
if (status == Integrator::ReachedReportTime)
++nextReport;
}
printf("Using Integrator %s:\n", myStudy.getMethodName());
printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
}
catch (const std::exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
catch (...) {
printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
}
OptimizerFunction(const MultibodySystem& system, const State& state, Array_<MobilizedBodyIndex> bodyIxs, Array_<Array_<Vec3> > stations, Array_<Array_<Vec3> > targetLocations, Array_<Array_<Real> > weights) :
OptimizerSystem(state.getNQ()), system(system), state(state), bodyIxs(bodyIxs), stations(stations), targetLocations(targetLocations), weights(weights) {
system.realize(state, Stage::Instance);
setNumEqualityConstraints(state.getNQErr());
}