//------------------------------------------------------------------------------
// 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());
}
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 testCoordinateCouplerConstraint()
{
using namespace SimTK;
cout << endl;
cout << "=================================================================" << endl;
cout << " OpenSim CoordinateCouplerConstraint vs. FunctionBasedMobilizer " << endl;
cout << "=================================================================" << endl;
// Define spline data for the custom knee joint
int npx = 12;
double angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197, -0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443, 0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393};
double kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000, 0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000, -0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000};
int npy = 7;
double angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399, -0.174532925199, 0.159148563428, 2.094395102393};
double kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000, -0.396600000000, -0.395264000000, -0.396000000000 };
for(int i = 0; i<npy; i++) {
// Spline data points from experiment w.r.t. hip location. Change to make it w.r.t knee location
kneeY[i] += (-kneeInFemur[1]+hipInFemur[1]);
}
SimmSpline tx(npx, angX, kneeX);
SimmSpline ty(npy, angY, kneeY);;
// Define the functions that specify the FunctionBased Mobilized Body.
std::vector<std::vector<int> > coordIndices;
std::vector<const SimTK::Function*> functions;
std::vector<bool> isdof(6,false);
// Set the 1 spatial rotation about Z-axis
isdof[2] = true; //rot Z
int nm = 0;
for(int i=0; i<6; i++){
if(isdof[i]) {
Vector coeff(2);
coeff[0] = 1;
coeff[1] = 0;
std::vector<int> findex(1);
findex[0] = nm++;
functions.push_back(new SimTK::Function::Linear(coeff));
coordIndices.push_back(findex);
}
else if(i==3 || i ==4){
std::vector<int> findex(1,0);
if(i==3)
functions.push_back(tx.createSimTKFunction());
else
functions.push_back(ty.createSimTKFunction());
coordIndices.push_back(findex);
}
else{
std::vector<int> findex(0);
functions.push_back(new SimTK::Function::Constant(0, 0));
coordIndices.push_back(findex);
}
}
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
//system.updDefaultSubsystem().addEventReporter(new VTKEventReporter(system, 0.01));
// Thigh connected by hip
MobilizedBody::Pin thigh(matter.Ground(), Transform(hipInGround),
SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur));
//Function-based knee connects shank
MobilizedBody::FunctionBased shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia), nm, functions, coordIndices);
//MobilizedBody::Pin shank(thigh, SimTK::Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), SimTK::Transform(kneeInTibia));
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//==========================================================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM);
// create hip as a pin joint
PinJoint hip("hip",ground, hipInGround, Vec3(0), osim_thigh, hipInFemur, Vec3(0));
// Rename hip coordinates for a pin joint
hip.getCoordinateSet()[0].setName("hip_flex");
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_thigh);
osimModel->addJoint(&hip);
// Add another body via a knee joint
OpenSim::Body osim_shank("shank", tibiaMass.getMass(),
tibiaMass.getMassCenter(), tibiaMass.getInertia());
// Define knee coordinates and axes for custom joint spatial transform
SpatialTransform kneeTransform;
string knee_q_name = "knee_q";
string tx_name = "knee_tx";
string ty_name = "knee_ty";
Array<string> indepCoords(knee_q_name, 1, 1);
// knee flexion/extension
kneeTransform[2].setCoordinateNames(indepCoords);
kneeTransform[2].setFunction(new LinearFunction());
// translation X
kneeTransform[3].setCoordinateNames(OpenSim::Array<std::string>(tx_name, 1, 1));
kneeTransform[3].setFunction(new LinearFunction());
// translation Y
kneeTransform[4].setCoordinateNames(OpenSim::Array<std::string>(ty_name, 1, 1));
kneeTransform[4].setFunction(new LinearFunction());
// create custom knee joint
CustomJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), osim_shank, kneeInTibia, Vec3(0), kneeTransform);
// Add the shank body which now also contains the knee joint to the model
osimModel->addBody(&osim_shank);
osimModel->addJoint(&knee);
// Constrain the knee translations to follow the desired manifold
CoordinateCouplerConstraint knee_tx_constraint;
CoordinateCouplerConstraint knee_ty_constraint;
knee_tx_constraint.setName("knee_tx_coupler");
knee_ty_constraint.setName("knee_ty_coupler");
knee_tx_constraint.setIndependentCoordinateNames(indepCoords);
knee_ty_constraint.setIndependentCoordinateNames(indepCoords);
knee_tx_constraint.setDependentCoordinateName(tx_name);
knee_ty_constraint.setDependentCoordinateName(ty_name);
knee_tx_constraint.setFunction(tx);
knee_ty_constraint.setFunction(ty);
// Add the constraints
osimModel->addConstraint(&knee_tx_constraint);
osimModel->addConstraint(&knee_ty_constraint);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
// OpenSim model must realize the topology to get valid osim_state
osimModel->setGravity(gravity_vec);
PointKinematics *pointKin = new PointKinematics(osimModel);
// Get the point location of the shank origin in space
pointKin->setBodyPoint("shank", Vec3(0));
osimModel->addAnalysis(pointKin);
// Model cannot own model components created on the stack in this test program
osimModel->disownAllComponents();
// write out the model to file
osimModel->print("testCouplerConstraint.osim");
//wipe-out the model just constructed
delete osimModel;
// reconstruct from the model file
osimModel = new Model("testCouplerConstraint.osim");
ForceReporter *forceReport = new ForceReporter(osimModel);
forceReport->includeConstraintForces(true);
osimModel->addAnalysis(forceReport);
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//==========================================================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state, "testCoordinateCouplerConstraint FAILED\n");
// Forces were held in storage during simulation, now write to file
forceReport->printResults("CouplerModelForces");
}
void testPointOnLineConstraint()
{
using namespace SimTK;
cout << endl;
cout << "==================================================================" << endl;
cout << "OpenSim PointOnLineConstraint vs. Simbody Constraint::PointOnLine " << endl;
cout << "==================================================================" << endl;
Random::Uniform randomDirection(-1, 1);
Vec3 lineDirection(randomDirection.getValue(), randomDirection.getValue(),
randomDirection.getValue());
UnitVec3 normLineDirection(lineDirection.normalize());
Vec3 pointOnLine(0,0,0);
Vec3 pointOnFollower(0,0,0);
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
// Create a free joint between the foot and ground
MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)),
SimTK::Body::Rigid(footMass), Transform(Vec3(0)));
// Constrain foot to line on ground
SimTK::Constraint::PointOnLine simtkPointOnLine(matter.Ground(),
normLineDirection, pointOnLine, foot, pointOnFollower);
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//=========================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
//OpenSim foot
OpenSim::Body osim_foot("foot", footMass.getMass(),
footMass.getMassCenter(), footMass.getInertia());
// create foot as a free joint
FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0),
osim_foot, Vec3(0), Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_foot);
osimModel->addJoint(&footJoint);
// add a point on line constraint
PointOnLineConstraint lineConstraint(ground, normLineDirection, pointOnLine,
osim_foot, pointOnFollower);
osimModel->addConstraint(&lineConstraint);
// BAD: have to set memoryOwner to false or program will crash when this test is complete.
osimModel->disownAllComponents();
osimModel->setGravity(gravity_vec);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//==========================================================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state, "testPointOnLineConstraint FAILED\n");
} // end testPointOnLineConstraint
void testWeldConstraint()
{
using namespace SimTK;
cout << endl;
cout << "==================================================================" << endl;
cout << " OpenSim WeldConstraint vs. Simbody Constraint::Weld " << endl;
cout << "==================================================================" << endl;
Random::Uniform randomValue(-0.05, 0.1);
Vec3 weldInGround(randomValue.getValue(), randomValue.getValue(), 0);
Vec3 weldInFoot(0.1*randomValue.getValue(), 0.1*randomValue.getValue(), 0);
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
// Thigh connected by hip
MobilizedBody::Pin thigh(matter.Ground(), SimTK::Transform(hipInGround),
SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur));
// Pin knee connects shank
MobilizedBody::Pin shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia));
// Pin ankle connects foot
MobilizedBody::Pin foot(shank, Transform(ankleInTibia), SimTK::Body::Rigid(footMass), Transform(ankleInFoot));
SimTK::Constraint::Weld weld(matter.Ground(), Transform(weldInGround), foot, Transform(weldInFoot));
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//==========================================================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
//OpenSim thigh
OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM);
// create Pin hip joint
PinJoint hip("hip", ground, hipInGround, Vec3(0),
osim_thigh, hipInFemur, Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_thigh);
osimModel->addJoint(&hip);
//OpenSim shank
OpenSim::Body osim_shank("shank", tibiaMass.getMass(),
tibiaMass.getMassCenter(), tibiaMass.getInertia());
// create Pin knee joint
PinJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0),
osim_shank, kneeInTibia, Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_shank);
osimModel->addJoint(&knee);
//OpenSim foot
OpenSim::Body osim_foot("foot", footMass.getMass(),
footMass.getMassCenter(), footMass.getInertia());
// create Pin ankle joint
PinJoint ankle("ankle", osim_shank, ankleInTibia, Vec3(0),
osim_foot, ankleInFoot, Vec3(0));
// Add the foot body which now also contains the hip joint to the model
osimModel->addBody(&osim_foot);
osimModel->addJoint(&ankle);
// add a point on line constraint
WeldConstraint footConstraint( "footConstraint",
ground, SimTK::Transform(weldInGround),
osim_foot, SimTK::Transform(weldInFoot) );
osimModel->addConstraint(&footConstraint);
// BAD: but if model maintains ownership, it will attempt to delete stack allocated objects
osimModel->disownAllComponents();
osimModel->setGravity(gravity_vec);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
osimModel->setup();
osimModel->print("testWeldConstraint.osim");
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//=========================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state,
"testWeldConstraint FAILED\n");
}
void testConstantDistanceConstraint()
{
using namespace SimTK;
cout << endl;
cout << "==================================================================" << endl;
cout << " OpenSim ConstantDistanceConstraint vs. Simbody Constraint::Rod " << endl;
cout << "==================================================================" << endl;
Random::Uniform randomLocation(-1, 1);
Vec3 pointOnFoot(randomLocation.getValue(), randomLocation.getValue(), randomLocation.getValue());
Vec3 pointOnGround(0,0,0);
/** for some reason, adding another Random::Uniform causes testWeldConstraint to fail.
Why doesn't it cause this test to fail???? */
//Random::Uniform randomLength(0.01, 0.2);
//randomLength.setSeed(1024);
//double rodLength = randomLength.getValue();
double rodLength = 0.05;
//std::cout << "Random Length = " << rodLength2 << ", used length = " << rodLength << std::endl;
// Define the Simbody system
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
// Create a free joint between the foot and ground
MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)),
SimTK::Body::Rigid(footMass), Transform(Vec3(0)));
// Constrain foot to point on ground
SimTK::Constraint::Rod simtkRod(matter.Ground(), pointOnGround, foot, pointOnFoot, rodLength);
// Simbody model state setup
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
//==========================================================================================================
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
const Ground& ground = osimModel->getGround();;
//OpenSim foot
OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia());
// create foot as a free joint
FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0), osim_foot, Vec3(0), Vec3(0));
// Add the thigh body which now also contains the hip joint to the model
osimModel->addBody(&osim_foot);
osimModel->addJoint(&footJoint);
// add a constant distance constraint
ConstantDistanceConstraint rodConstraint(ground, pointOnGround, osim_foot, pointOnFoot,rodLength);
osimModel->addConstraint(&rodConstraint);
// BAD: have to set memoryOwner to false or program will crash when this test is complete.
osimModel->disownAllComponents();
osimModel->setGravity(gravity_vec);
//Add analyses before setting up the model for simulation
Kinematics *kinAnalysis = new Kinematics(osimModel);
kinAnalysis->setInDegrees(false);
osimModel->addAnalysis(kinAnalysis);
// Need to setup model before adding an analysis since it creates the AnalysisSet
// for the model if it does not exist.
State& osim_state = osimModel->initSystem();
//==========================================================================================================
// Compare Simbody system and OpenSim model simulations
compareSimulations(system, state, osimModel, osim_state, "testConstantDistanceConstraint FAILED\n");
}
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
// CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
MultibodySystem mbs; mbs.setUseUniformBackground(true);
SimbodyMatterSubsystem crankRocker(mbs);
GeneralForceSubsystem forces(mbs);
DecorationSubsystem viz(mbs);
Force::UniformGravity gravity(forces, crankRocker, Vec3(0, -g, 0));
// ADD BODIES AND THEIR MOBILIZERS
Body::Rigid crankBody = Body::Rigid(MassProperties(.1, Vec3(0), 0.1*UnitInertia::brick(1,3,.5)));
crankBody.addDecoration(DecorativeEllipsoid(0.1*Vec3(1,3,.4))
.setResolution(10)
.setOpacity(.2));
Body::Rigid sliderBody = Body::Rigid(MassProperties(.2, Vec3(0), 0.2*UnitInertia::brick(1,5,.5)));
sliderBody.addDecoration(DecorativeEllipsoid(0.2*Vec3(1,5,.4))
.setColor(Blue)
.setResolution(10)
.setOpacity(.2));
Body::Rigid longBar = Body::Rigid(MassProperties(0.01, Vec3(0), 0.01*UnitInertia::cylinderAlongX(.1, 5)));
longBar.addDecoration(Rotation(Pi/2,ZAxis), DecorativeCylinder(.01, 1));
MobilizedBody::Pin
crank(crankRocker.Ground(), Transform(), crankBody, Transform(Vec3(0, .15, 0)));
MobilizedBody::Pin
slider(crankRocker.Ground(), Transform(Vec3(2.5,0,0)), sliderBody, Transform(Vec3(0, 1, 0)));
MobilizedBody::Universal
rightConn(crank, Transform(Rotation(-Pi/2,YAxis),Vec3(0,-.3,0)),
longBar, Transform(Rotation(-Pi/2,YAxis),Vec3(-1,0,0)));
Constraint::Ball ball(slider, Vec3(0,-1, 0), rightConn, Vec3(1,0,0));
ball.setDefaultRadius(0.01);
//Constraint::Rod rodl(leftConn, Vec3(0), rightConn, Vec3(-2.5,0,0), 2.5);
//Constraint::Rod rodr(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-1.5,0,0), std::sqrt(2.));
//Constraint::Rod rodo(leftConn, Vec3(1.5,0,0), rightConn, Vec3(-2.5,1,0), std::sqrt(2.));
//Constraint::Rod rodl(leftConn, Vec3(-2.5,0,0), rightConn, Vec3(-2.5,0,0), 5);
//Constraint::Rod rodr(leftConn, Vec3(2.5,0,0), rightConn, Vec3(2.5,0,0), 5);
//Constraint::Rod rodo(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-2.5,1,0), 2);
// Add visualization line (orange=spring, black=constraint)
//viz.addRubberBandLine(crank, Vec3(0,-3,0),
// slider, Vec3(0,-5,0),
// DecorativeLine().setColor(Black).setLineThickness(4));
//forces.addMobilityConstantForce(leftPendulum, 0, 20);
//forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
//forces.addGlobalEnergyDrain(1);
Force::MobilityConstantForce(forces, crank, 0, 1);
Force::MobilityLinearDamper(forces, crank, 0, 1.0);
//Motion::Linear(crank, Vec3(a,b,c)); // crank(t)=at^2+bt+c
//Motion::Linear lmot(rightConn, Vec3(a,b,c)); // both axes follow
//lmot.setAxis(1, Vec3(d,e,f));
//Motion::Orientation(someBall, orientFuncOfT);
//someBall.prescribeOrientation(orientFunc);
//Motion::Relax(crank); // acc=vel=0, pos determined by some default relaxation solver
// Like this, or should this be an Instance-stage mode of every mobilizer?
//Motion::Lock(crank); // acc=vel=0; pos is discrete or fast
//Motion::Steady(crank, Vec3(1,2,3)); // acc=0; vel=constant; pos integrated
//Motion::Steady crankRate(crank, 1); // acc=0; vel=constant, same for each axis; pos integrated
// or ...
//crank.lock(state);
//crank.setMotionType(state, Regular|Discrete|Fast|Prescribed, stage);
// need a way to register a mobilizer with a particular relaxation solver,
// switch between dynamic, continuous relaxed, end-of-step relaxed, discrete.
// what about a "local" (explicit) relaxation, like q=(q1+q2)/2 ?
State s = mbs.realizeTopology(); // returns a reference to the the default state
mbs.realizeModel(s); // define appropriate states for this System
//crankRate.setRate(s, 3);
crank.setAngle(s, 5); //q
crank.setRate(s, 3); //u
Visualizer display(mbs);
mbs.realize(s, Stage::Position);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "qErr=" << s.getQErr() << endl;
crank.setAngle(s, -30*Deg2Rad);
//crank.setQToFitRotation (s, Rotation::aboutZ(-.9*Pi/2));
//TODO
//rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));
//crank.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
mbs.realize(s, Stage::Velocity);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "qErr=" << s.getQErr() << endl;
// These are the SimTK Simmath integrators:
RungeKuttaMersonIntegrator myStudy(mbs);
//CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::Newton);
//myStudy.setMaximumStepSize(0.001);
myStudy.setAccuracy(1e-3);
//myStudy.setProjectEveryStep(true);
//myStudy.setAllowInterpolation(false);
//myStudy.setMaximumStepSize(.1);
const Real dt = 1./30; // output intervals
const Real finalTime = 10;
myStudy.setFinalTime(finalTime);
cout << "Hit ENTER in console to continue ...\n";
getchar();
display.report(s);
cout << "Hit ENTER in console to continue ...\n";
getchar();
// Peforms assembly if constraints are violated.
myStudy.initialize(s);
myStudy.setProjectEveryStep(false);
myStudy.setConstraintTolerance(.001);
myStudy.initialize(myStudy.getState());
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 << "qErr=" << s.getQErr() << endl;
}
Integrator::SuccessfulStepStatus status;
int nextReport = 0;
while ((status=myStudy.stepTo(nextReport*dt, Infinity))
!= Integrator::EndOfSimulation)
{
const State& s = myStudy.getState();
mbs.realize(s);
const Real crankAngle = crank.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg;
printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
crankAngle,
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",
crankRocker.getQErr(s).normRMS(),
crankRocker.getUErr(s).normRMS(),
crankRocker.getUDotErr(s).normRMS());
display.report(s);
if (status == Integrator::ReachedReportTime)
++nextReport;
}
for (int i=0; i<100; ++i)
display.report(myStudy.getState());
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 exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
catch (...) {
printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
}
void main_simulation()
#endif
{
// inputs
double fitness;
#ifdef OPTI
double *optiParams;
#endif
Loop_inputs *loop_inputs;
// initialization
loop_inputs = init_simulation();
// optimization init
#ifdef OPTI
optiParams = (double*) malloc(NB_PARAMS_TO_OPTIMIZE*sizeof(double));
get_real_params_to_opt(optiNorms, optiParams);
erase_for_opti(optiParams, loop_inputs->MBSdata);
free(optiParams);
#endif
// -- Simbody -- //
#ifdef SIMBODY
// / Create the system. Define all "system" objects - system, matterm forces, tracker, contactForces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
system.setUpDirection(+ZAxis); // that is for visualization only. The default direction is +X
SimbodyVariables simbodyVariables;
// set all the mechanical parameters of the contact
simbodyVariables.p_system = &system;
simbodyVariables.p_matter = &matter;
simbodyVariables.p_tracker = &tracker;
simbodyVariables.p_contactForces = &contactForces;
// cout<<"BoxInd in Main = "<<BoxInd<<" -- should be default \n";
//init_Simbody(&simbodyVariables);
init_Simbody(&simbodyVariables, loop_inputs->MBSdata->user_IO->simbodyBodies);
//it is "system" commands. We cannot avoid them.
system.realizeTopology();
State state = system.getDefaultState();
simbodyVariables.p_state = &state;
//it is "system" command. We cannot avoid them.
system.realizeModel(state);
p_simbodyVariables = &simbodyVariables;
#endif
// loop
fitness = loop_simulation(loop_inputs);
// end of the simulation
finish_simulation(loop_inputs);
#ifdef OPTI
return fitness;
#else
#if defined(PRINT_REPORT)
printf("fitness: %f\n", fitness);
#endif
#endif
}
int main() {
try
{ // Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, UnitVec3(-1,0,0), 9.81);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
contactForces.setTransitionVelocity(1e-2); // m/s
// Ground's normal is +x for this model
system.setUpDirection(+XAxis);
// Uncomment this if you want a more elegant movie.
//matter.setShowDefaultGeometry(false);
const Real ud = .3; // dynamic
const Real us = .6; // static
const Real uv = 0; // viscous (force/velocity)
const Real k = 1e8; // pascals
const Real c = 0.01; // dissipation (1/v)
// Halfspace default is +x, this one occupies -x instead, so flip.
const Rotation R_xdown(Pi,ZAxis);
matter.Ground().updBody().addContactSurface(
Transform(R_xdown, Vec3(0,0,0)),
ContactSurface(ContactGeometry::HalfSpace(),
ContactMaterial(k,c,us,ud,uv)));
const Real ellipsoidMass = 1; // kg
const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
const Vec3 comLoc(-1*Cm2m, 0, 0);
const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));
ellipsoidBody.addDecoration(Transform(),
DecorativeEllipsoid(halfDims).setColor(Cyan)
//.setOpacity(.5)
.setResolution(3));
ellipsoidBody.addContactSurface(Transform(),
ContactSurface(ContactGeometry::Ellipsoid(halfDims),
ContactMaterial(k,c,us,ud,uv))
);
MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
ellipsoidBody, Transform(Vec3(0)));
Visualizer viz(system);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
viz.setMode(Visualizer::RealTime);
viz.setDesiredFrameRate(FrameRate);
viz.setCameraClippingPlanes(0.1, 10);
Visualizer::InputSilo* silo = new Visualizer::InputSilo();
viz.addInputListener(silo);
Array_<std::pair<String,int> > runMenuItems;
runMenuItems.push_back(std::make_pair("Go", GoItem));
runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
runMenuItems.push_back(std::make_pair("Quit", QuitItem));
viz.addMenu("Run", RunMenuId, runMenuItems);
Array_<std::pair<String,int> > helpMenuItems;
helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
viz.addMenu("Help", HelpMenuId, helpMenuItems);
system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
// Check for a Run->Quit menu pick every 1/4 second.
system.addEventHandler(new UserInputHandler(*silo, .25));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
matter.setUseEulerAngles(state, true);
system.realizeModel(state);
ellipsoid.setQToFitTransform(state, Transform(
Rotation(BodyRotationSequence, 0 *Deg2Rad, XAxis,
0.5*Deg2Rad, YAxis,
-0.5*Deg2Rad, ZAxis),
Vec3(2.1*Cm2m, 0, 0)));
ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s
viz.report(state);
printf("Default state\n");
cout << "\nChoose 'Go' from Run menu to simulate:\n";
int menuId, item;
do { silo->waitForMenuPick(menuId, item);
if (menuId != RunMenuId || item != GoItem)
cout << "\aDude ... follow instructions!\n";
} while (menuId != RunMenuId || item != GoItem);
// Simulate it.
//ExplicitEulerIntegrator integ(system);
//CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
//RungeKuttaFeldbergIntegrator integ(system);
RungeKuttaMersonIntegrator integ(system);
//RungeKutta3Integrator integ(system);
//VerletIntegrator integ(system);
//integ.setMaximumStepSize(1e-0001);
integ.setAccuracy(1e-4); // minimum for CPodes
//integ.setAccuracy(.01);
TimeStepper ts(system, integ);
ts.initialize(state);
double cpuStart = cpuTime();
double realStart = realTime();
ts.stepTo(10.0);
const double timeInSec = realTime() - realStart;
const int evals = integ.getNumRealizations();
cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
<< (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
<< (1000*ts.getTime())/evals << "ms/eval\n";
cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
printf("Using Integrator %s at accuracy %g:\n",
integ.getMethodName(), integ.getAccuracyInUse());
printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
viz.dumpStats(std::cout);
// Add as slider to control playback speed.
viz.addSlider("Speed", 1, 0, 4, 1);
viz.setMode(Visualizer::PassThrough);
silo->clear(); // forget earlier input
double speed = 1; // will change if slider moves
while(true) {
cout << "Choose Run/Replay to see that again ...\n";
int menuId, item;
silo->waitForMenuPick(menuId, item);
if (menuId != RunMenuId) {
cout << "\aUse the Run menu!\n";
continue;
}
if (item == QuitItem)
break;
if (item != ReplayItem) {
cout << "\aHuh? Try again.\n";
continue;
}
for (double i=0; i < (int)saveEm.size(); i += speed ) {
int slider; Real newValue;
if (silo->takeSliderMove(slider,newValue)) {
speed = newValue;
}
viz.report(saveEm[(int)i]);
}
}
} catch (const std::exception& e) {
std::printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
} catch (...) {
std::printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
return 0;
}
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);
}
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);
}
}
Real ObservedPointFitter::findBestFit
(const MultibodySystem& system, State& state,
const Array_<MobilizedBodyIndex>& bodyIxs,
const Array_<Array_<Vec3> >& stations,
const Array_<Array_<Vec3> >& targetLocations,
const Array_<Array_<Real> >& weights,
Real tolerance)
{
// Verify the inputs.
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
SimTK_APIARGCHECK(bodyIxs.size() == stations.size() && stations.size() == targetLocations.size(), "ObservedPointFitter", "findBestFit", "bodyIxs, stations, and targetLocations must all be the same length");
int numBodies = matter.getNumBodies();
for (int i = 0; i < (int)stations.size(); ++i) {
SimTK_APIARGCHECK(bodyIxs[i] >= 0 && bodyIxs[i] < numBodies, "ObservedPointFitter", "findBestFit", "Illegal body ID");
SimTK_APIARGCHECK(stations[i].size() == targetLocations[i].size(), "ObservedPointFitter", "findBestFit", "Different number of stations and target locations for body");
}
// Build a list of children for each body.
Array_<Array_<MobilizedBodyIndex> > children(matter.getNumBodies());
for (int i = 0; i < matter.getNumBodies(); ++i) {
const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
if (!body.isGround())
children[body.getParentMobilizedBody().getMobilizedBodyIndex()].push_back(body.getMobilizedBodyIndex());
}
// Build a mapping of body IDs to indices.
Array_<int> bodyIndex(matter.getNumBodies());
for (int i = 0; i < (int) bodyIndex.size(); ++i)
bodyIndex[i] = -1;
for (int i = 0; i < (int)bodyIxs.size(); ++i)
bodyIndex[bodyIxs[i]] = i;
// Find the number of stations on each body with a nonzero weight.
Array_<int> numStations(matter.getNumBodies());
for (int i = 0; i < (int) numStations.size(); ++i)
numStations[i] = 0;
for (int i = 0; i < (int)weights.size(); ++i) {
for (int j = 0; j < (int)weights[i].size(); ++j)
if (weights[i][j] != 0)
numStations[bodyIxs[i]]++;
}
// Perform the initial estimation of Q for each mobilizer.
// Our first guess is the passed-in q's, with quaternions converted
// to Euler angles if necessary. As we solve a subproblem for each
// of the bodies in ascending order, we'll update tempState's q's
// for that body to their solved values.
State tempState;
if (!matter.getUseEulerAngles(state))
matter.convertToEulerAngles(state, tempState);
else tempState = state;
system.realizeModel(tempState);
system.realize(tempState, Stage::Position);
// This will accumulate best-guess spatial poses for the bodies as
// they are processed. This is useful for when a body is used as
// an artificial base body; our first guess will to be to place it
// wherever it was the last time it was used in a subproblem.
Array_<Transform> guessX_GB(matter.getNumBodies());
for (MobilizedBodyIndex mbx(1); mbx < guessX_GB.size(); ++mbx)
guessX_GB[mbx] = matter.getMobilizedBody(mbx).getBodyTransform(tempState);
for (int i = 0; i < matter.getNumBodies(); ++i) {
MobilizedBodyIndex id(i);
const MobilizedBody& body = matter.getMobilizedBody(id);
if (body.getNumQ(tempState) == 0)
continue; // No degrees of freedom to determine.
if (children[id].size() == 0 && numStations[id] == 0)
continue; // There are no stations whose positions are affected by this.
Array_<MobilizedBodyIndex> originalBodyIxs;
int currentBodyIndex = findBodiesForClonedSystem(body.getMobilizedBodyIndex(), numStations, matter, children, originalBodyIxs);
if (currentBodyIndex == (int)originalBodyIxs.size()-1
&& (bodyIndex[id] == -1 || stations[bodyIndex[id]].size() == 0))
continue; // There are no stations whose positions are affected by this.
MultibodySystem copy;
Array_<MobilizedBodyIndex> copyBodyIxs;
bool hasArtificialBaseBody;
createClonedSystem(system, copy, originalBodyIxs, copyBodyIxs, hasArtificialBaseBody);
const SimbodyMatterSubsystem& copyMatter = copy.getMatterSubsystem();
// Construct an initial state.
State copyState = copy.getDefaultState();
assert(copyBodyIxs.size() == originalBodyIxs.size());
for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
if (ob==0 && hasArtificialBaseBody)
copyMobod.setQToFitTransform(copyState, guessX_GB[origMobod.getMobilizedBodyIndex()]);
else
copyMobod.setQFromVector(copyState, origMobod.getQAsVector(tempState));
}
Array_<Array_<Vec3> > copyStations(copyMatter.getNumBodies());
Array_<Array_<Vec3> > copyTargetLocations(copyMatter.getNumBodies());
Array_<Array_<Real> > copyWeights(copyMatter.getNumBodies());
for (int j = 0; j < (int)originalBodyIxs.size(); ++j) {
int index = bodyIndex[originalBodyIxs[j]];
if (index != -1) {
copyStations[copyBodyIxs[j]] = stations[index];
copyTargetLocations[copyBodyIxs[j]] = targetLocations[index];
copyWeights[copyBodyIxs[j]] = weights[index];
}
}
try {
OptimizerFunction optimizer(copy, copyState, copyBodyIxs, copyStations, copyTargetLocations, copyWeights);
Vector q(copyState.getQ());
//std::cout << "BODY " << i << " q0=" << q << std::endl;
optimizer.optimize(q, tolerance);
//std::cout << " qf=" << q << std::endl;
copyState.updQ() = q;
copy.realize(copyState, Stage::Position);
// Transfer updated state back to tempState as improved initial guesses.
// However, all but the currentBody will get overwritten later.
for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
guessX_GB[originalBodyIxs[ob]] = copyMobod.getBodyTransform(copyState);
if (ob==0 && hasArtificialBaseBody) continue; // leave default state
const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
origMobod.setQFromVector(tempState, copyMobod.getQAsVector(copyState));
}
//body.setQFromVector(tempState, copyMatter.getMobilizedBody(copyBodyIxs[currentBodyIndex]).getQAsVector(copyState));
}
catch (Exception::OptimizerFailed ex) {
std::cout << "Optimization failure for body "<<i<<": "<<ex.getMessage() << std::endl;
// Just leave this body's state variables set to 0, and rely on the final optimization to fix them.
}
}
// Now do the final optimization of the whole system.
OptimizerFunction optimizer(system, tempState, bodyIxs, stations, targetLocations, weights);
Vector q = tempState.getQ();
optimizer.optimize(q, tolerance);
if (matter.getUseEulerAngles(state))
state.updQ() = q;
else {
tempState.updQ() = q;
matter.convertToQuaternions(tempState, state);
}
// Return the RMS error in the optimized system.
Real error;
optimizer.objectiveFunc(q, true, error);
if (UseWeighted)
return std::sqrt(error - MinimumShift); // already weighted; this makes WRMS
Real totalWeight = 0;
for (int i = 0; i < (int)weights.size(); ++i)
for (int j = 0; j < (int)weights[i].size(); ++j)
totalWeight += weights[i][j];
return std::sqrt((error-MinimumShift)/totalWeight);
}
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
// 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);
}
}
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) {
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);
}
}
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;
}
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);
}
}
//------------------------------------------------------------------------------
// main program
//------------------------------------------------------------------------------
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
int i = 0;
//--------------------------------------------------------------------------
// Experimental data points (x,y) of tibia origin (tibial plateau) measured
// w.r.t. to origin of the femur (hip joint center) in the femur frame as a
// function of knee joint angle. From Yamaguchi and Zajac, 1989.
//--------------------------------------------------------------------------
// Tibia X:
int npx = 12;
Real angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197,
-0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443,
0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393};
Real kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000,
0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000,
-0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000};
// Tibia Y; note that Y data is offset by -0.4 due to body frame placement.
int npy = 7;
Real angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399,
-0.174532925199, 0.159148563428, 2.094395102393};
Real kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000,
-0.396600000000, -0.395264000000, -0.396000000000 };
// Create SimTK Vectors to hold data points, and initialize from above arrays.
Vector ka_x (npx, angX); // measured knee angles when X data was collected
Vector ka_y (npy, angY); // measured knee angles when Y data was collected
Vector tib_x(npx, kneeX);
Vector tib_y(npy, kneeY);
#ifdef SHOULD_EXAGGERATE
// See above.
tib_x *= ExaggerateX;
tib_y = (tib_y+0.4)*ExaggerateY - 0.4; // exaggerate deviation only, not offset
#endif
// Generate splines from vectors of data points.
const int Degree = 3; // use cubics
SplineFitter<Real> fitterX = SplineFitter<Real>::fitFromGCV(Degree, ka_x, tib_x);
SplineFitter<Real> fitterY = SplineFitter<Real>::fitFromGCV(Degree, ka_y, tib_y);
Spline fx = fitterX.getSpline();
Spline fy = fitterY.getSpline();
//--------------------------------------------------------------------------
// Define the 6-spatial functions that specify the motion of the tibia as a
// a FunctionBased MobilizedBody (shank) w.r.t. to its parent (the thigh).
//--------------------------------------------------------------------------
// Each function has to return 1 Real value
std::vector<const Function*> functions(6);
// as a function of coordIndices (more than one per function)
std::vector< std::vector<int> > coordIndices(6);
// about a body-fixed axis for rotations or in parent translations
std::vector<Vec3> axes(6);
// Set the 1st and 2nd spatial rotation about the orthogonal (X then Y) axes as
// constant values. That is they don't contribute to motion nor do they have
// any coordinates in the equations of motion.
// |--------------------------------|
// | 1st function: rotation about X |
// |--------------------------------|
functions[0] = (new Function::Constant(0, 0));
std::vector<int> noIndex(0);
coordIndices[0] =(noIndex);
// |--------------------------------|
// | 2nd function: rotation about Y |
// |--------------------------------|
functions[1] = (new Function::Constant(0, 0));
coordIndices[1] = (noIndex);
// Set the spatial rotation about third axis to be the knee-extension
// angle (the one q) about the Z-axis of the tibia at the femoral condyles
// Define the coefficients of the linear function of the knee-angle with the
// spatial rotation about Z.
Vector coeff(2);
// Linear function x3 = coeff[0]*q + coeff[1]
coeff[0] = 1; coeff[1] = 0;
// |--------------------------------|
// | 3rd function: rotation about Z |
// |--------------------------------|
functions[2] = new Function::Linear(coeff);
// function of coordinate 0 (knee extension angle)
std::vector<int> qIndex(1,0);
coordIndices[2] = qIndex;
// Set the spatial translations as a function (splines) along the parent X and Y axes
// |-----------------------------------|
// | 4th function: translation about X |
// |-----------------------------------|
functions[3] = new Spline(fx); // Give the mobilizer a copy it can own.
coordIndices[3] =(qIndex);
// |-----------------------------------|
// | 5th function: translation about Y |
// |-----------------------------------|
functions[4] = new Spline(fy); // Give the mobilizer a copy it can own.
coordIndices[4] =(qIndex);
// |-----------------------------------|
// | 6th function: translation about Z |
// |-----------------------------------|
functions[5] = (new Function::Constant(0, 0));
coordIndices[5] = (noIndex);
// Construct the multibody system
const Real grav = 9.80665;
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, grav);
matter.setShowDefaultGeometry(true);
//--------------------------------------------------------------------------
// Define the model's physical (body) properties
//--------------------------------------------------------------------------
//Thigh
Body::Rigid femur(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337))));
femur.addDecoration(Transform(Vec3(0, -0.21+0.1715, 0)),
DecorativeCylinder(0.04, 0.21).setColor(Orange).setOpacity(.5));
//Shank
Body::Rigid tibia(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484))));
tibia.addDecoration(Transform(Vec3(0, -0.235+0.1862, 0)),
DecorativeCylinder(0.035, 0.235).setColor(Red));
//--------------------------------------------------------------------------
// Build the multibody system by adding mobilized bodies to the matter subsystem
//--------------------------------------------------------------------------
// Add the thigh via hip joint
MobilizedBody::Pin thigh(matter.Ground(), Transform(Vec3(0)), femur, Transform(Vec3(0.0020, 0.1715, 0)));
// This is how you might model the knee if you could only use a pin joint.
//MobilizedBody::Pin shank(thigh, Transform(Vec3(0.0033, -0.2294, 0)),
// tibia, Transform(Vec3(0.0, 0.1862, 0.0)));
// NOTE: function of Y-translation data was defined int the femur frame
// according to Yamaguchi and Zajac, which had the orgin at the hip joint
// center and the Y along the long-axis of the femur and Z out of the page.
MobilizedBody::FunctionBased shank(thigh, Transform(Vec3(0.0020, 0.1715, 0)),
tibia, Transform(Vec3(0.0, 0.1862, 0.0)),
1, // # of mobilities (dofs) for this joint
functions, coordIndices);
// Add some stop springs so the knee angle won't get outside the range of spline
// data we have. This custom force element is defined above.
Force::Custom(forces, new MyStop(shank, -Pi/2, 0*Pi, 100));
//--------------------------------------------------------------------------
// Setup reporters so we can get some output.
//--------------------------------------------------------------------------
// Vizualizer Animation
Visualizer viz(system);
system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
// Energy -- reporter defined above.
system.addEventReporter(new MyEnergyReporter(system, 0.01));
//--------------------------------------------------------------------------
// Complete the construction of the "const" part of the System and
// allocate the default state.
//--------------------------------------------------------------------------
system.realizeTopology();
// Get a copy of the default state to work with.
State state = system.getDefaultState();
//--------------------------------------------------------------------------
// Set modeling options if any (this one is not actually needed here).
//--------------------------------------------------------------------------
matter.setUseEulerAngles(state, true);
// Complete construction of the model, allocating additional state variables
// if necessary.
system.realizeModel(state);
//--------------------------------------------------------------------------
// Set initial conditions.
//--------------------------------------------------------------------------
// Hip and knee coordinates and speeds similar to early swing
double hip_angle = -45*Pi/180;
double knee_angle = 0*Pi/180;
double hip_vel = 1;
double knee_vel = -5.0;
// Set initial states (Q's and U's)
// Position
thigh.setOneQ(state, 0, hip_angle);
shank.setOneQ(state, 0, knee_angle);
// Speed
thigh.setOneU(state, 0, hip_vel);
shank.setOneU(state, 0, knee_vel);
//--------------------------------------------------------------------------
// Run simulation.
//--------------------------------------------------------------------------
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(Accuracy);
TimeStepper ts(system, integ);
ts.initialize(state); // set IC's
ts.stepTo(5.0);
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
return 0;
}