//------------------------------------------------------------------------------
// ASSEMBLER
//------------------------------------------------------------------------------
Assembler::Assembler(const MultibodySystem& system)
: system(system), accuracy(0), tolerance(0), // i.e., 1e-3, 1e-4
forceNumericalGradient(false), forceNumericalJacobian(false),
useRMSErrorNorm(false), alreadyInitialized(false),
asmSys(0), optimizer(0), nAssemblySteps(0), nInitializations(0)
{
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
matter.convertToEulerAngles(system.getDefaultState(),
internalState);
system.realizeModel(internalState);
// Make sure the System's Constraints are always present; this sets the
// weight to Infinity which makes us treat this as an assembly error
// rather than merely a goal; that can be changed by the user.
systemConstraints = adoptAssemblyError(new BuiltInConstraints());
}
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;
}
void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy,
const Array_<MobilizedBodyIndex>& originalBodyIxs,
Array_<MobilizedBodyIndex>& copyBodyIxs,
bool& hasArtificialBaseBody)
{
const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
SimbodyMatterSubsystem copyMatter(copy);
Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
hasArtificialBaseBody = false;
for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
MobilizedBody* copyBody;
if (i == 0) {
if (originalBody.isGround())
copyBody = ©Matter.Ground();
else {
hasArtificialBaseBody = true; // not using the original joint here
MobilizedBody::Free free(copyMatter.Ground(), body);
copyBody = ©Matter.updMobilizedBody(free.getMobilizedBodyIndex());
}
}
else {
MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
copyBody = &originalBody.cloneForNewParent(parent);
}
copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
}
copy.realizeTopology();
State& s = copy.updDefaultState();
copyMatter.setUseEulerAngles(s, true);
copy.realizeModel(s);
}
void 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");
}
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);
}
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));
}
// 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);
}
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);
}