/**
* Compute and record the results.
*
* This method, for the purpose of example, records the position and
* orientation of each body in the model. You will need to customize it
* to perform your analysis.
*
* @param aT Current time in the simulation.
* @param aX Current values of the controls.
* @param aY Current values of the states: includes generalized coords and speeds
*/
int InducedAccelerations::record(const SimTK::State& s)
{
int nu = _model->getNumSpeeds();
double aT = s.getTime();
cout << "time = " << aT << endl;
SimTK::Vector Q = s.getQ();
// Reset Accelerations for coordinates at this time step
for(int i=0;i<_coordSet.getSize();i++) {
_coordIndAccs[i]->setSize(0);
}
// Reset Accelerations for bodies at this time step
for(int i=0;i<_bodySet.getSize();i++) {
_bodyIndAccs[i]->setSize(0);
}
// Reset Accelerations for system center of mass at this time step
_comIndAccs.setSize(0);
_constraintReactions.setSize(0);
SimTK::State s_analysis = _model->getWorkingState();
_model->initStateWithoutRecreatingSystem(s_analysis);
// Just need to set current time and position to determine state of constraints
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// Check the external forces and determine if contact constraints should be applied at this time
// and turn constraint on if it should be.
Array<bool> constraintOn = applyContactConstraintAccordingToExternalForces(s_analysis);
// Hang on to a state that has the right flags for contact constraints turned on/off
_model->setPropertiesFromState(s_analysis);
// Use this state for the remainder of this step (record)
s_analysis = _model->getMultibodySystem().realizeTopology();
// DO NOT recreate the system, will lose location of constraint
_model->initStateWithoutRecreatingSystem(s_analysis);
// Cycle through the force contributors to the system acceleration
for(int c=0; c< _contributors.getSize(); c++){
//cout << "Solving for contributor: " << _contributors[c] << endl;
// Need to be at the dynamics stage to disable a force
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Dynamics);
if(_contributors[c] == "total"){
// Set gravity ON
_model->getGravityForce().enable(s_analysis);
//Use same conditions on constraints
s_analysis.setTime(aT);
// Set the configuration (gen. coords and speeds) of the model.
s_analysis.setQ(Q);
s_analysis.setU(s.getU());
s_analysis.setZ(s.getZ());
//Make sure all the actuators are on!
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, false);
}
// Get to the point where we can evaluate unilateral constraint conditions
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);
/* *********************************** ERROR CHECKING *******************************
SimTK::Vec3 pcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(s_analysis);
SimTK::Vec3 vcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(s_analysis);
SimTK::Vec3 acom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_analysis);
SimTK::Matrix M;
_model->getMultibodySystem().getMatterSubsystem().calcM(s_analysis, M);
cout << "mass matrix: " << M << endl;
SimTK::Inertia sysInertia = _model->getMultibodySystem().getMatterSubsystem().calcSystemCentralInertiaInGround(s_analysis);
cout << "system inertia: " << sysInertia << endl;
SimTK::SpatialVec sysMomentum =_model->getMultibodySystem().getMatterSubsystem().calcSystemMomentumAboutGroundOrigin(s_analysis);
cout << "system momentum: " << sysMomentum << endl;
const SimTK::Vector &appliedMobilityForces = _model->getMultibodySystem().getMobilityForces(s_analysis, SimTK::Stage::Dynamics);
appliedMobilityForces.dump("All Applied Mobility Forces");
// Get all applied body forces like those from conact
const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces = _model->getMultibodySystem().getRigidBodyForces(s_analysis, SimTK::Stage::Dynamics);
appliedBodyForces.dump("All Applied Body Forces");
SimTK::Vector ucUdot;
SimTK::Vector_<SimTK::SpatialVec> ucA_GB;
_model->getMultibodySystem().getMatterSubsystem().calcAccelerationIgnoringConstraints(s_analysis, appliedMobilityForces, appliedBodyForces, ucUdot, ucA_GB) ;
ucUdot.dump("Udots Ignoring Constraints");
ucA_GB.dump("Body Accelerations");
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize(), SimTK::SpatialVec(SimTK::Vec3(0)));
SimTK::Vector constraintMobilityForces(0);
int nc = _model->getMultibodySystem().getMatterSubsystem().getNumConstraints();
for (SimTK::ConstraintIndex cx(0); cx < nc; ++cx) {
if (!_model->getMultibodySystem().getMatterSubsystem().isConstraintDisabled(s_analysis, cx)){
cout << "Constraint " << cx << " enabled!" << endl;
}
}
//int nMults = _model->getMultibodySystem().getMatterSubsystem().getTotalMultAlloc();
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet[i].calcConstraintForces(s_analysis, constraintBodyForces, constraintMobilityForces);
}
constraintBodyForces.dump("Constraint Body Forces");
constraintMobilityForces.dump("Constraint Mobility Forces");
// ******************************* end ERROR CHECKING *******************************/
for(int i=0; i<constraintOn.getSize(); i++) {
_constraintSet.get(i).setDisabled(s_analysis, !constraintOn[i]);
// Make sure we stay at Dynamics so each constraint can evaluate its conditions
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);
}
// This should also push changes to defaults for unilateral conditions
_model->setPropertiesFromState(s_analysis);
}
else if(_contributors[c] == "gravity"){
// Set gravity ON
_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), false);
//s_analysis = _model->initSystem();
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// zero velocity
s_analysis.setU(SimTK::Vector(nu,0.0));
s_analysis.setZ(s.getZ());
// disable actuator forces
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, true);
}
}
else if(_contributors[c] == "velocity"){
// Set gravity off
_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), true);
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// non-zero velocity
s_analysis.setU(s.getU());
s_analysis.setZ(s.getZ());
// zero actuator forces
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, true);
}
// Set the configuration (gen. coords and speeds) of the model.
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Velocity);
}
else{ //The rest are actuators
// Set gravity OFF
_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), true);
// zero actuator forces
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, true);
}
//s_analysis = _model->initSystem();
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// zero velocity
SimTK::Vector U(nu,0.0);
s_analysis.setU(U);
s_analysis.setZ(s.getZ());
// light up the one actuator who's contribution we are looking for
int ai = _model->getActuators().getIndex(_contributors[c]);
if(ai<0)
throw Exception("InducedAcceleration: ERR- Could not find actuator '"+_contributors[c],__FILE__,__LINE__);
Actuator &actuator = _model->getActuators().get(ai);
actuator.setDisabled(s_analysis, false);
actuator.overrideForce(s_analysis, false);
Muscle *muscle = dynamic_cast<Muscle *>(&actuator);
if(muscle){
if(_computePotentialsOnly){
muscle->overrideForce(s_analysis, true);
muscle->setOverrideForce(s_analysis, 1.0);
}
}
// Set the configuration (gen. coords and speeds) of the model.
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Model);
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Velocity);
}// End of if to select contributor
// cout << "Constraint 0 is of "<< _constraintSet[0].getConcreteClassName() << " and should be " << constraintOn[0] << " and is actually " << (_constraintSet[0].isDisabled(s_analysis) ? "off" : "on") << endl;
// cout << "Constraint 1 is of "<< _constraintSet[1].getConcreteClassName() << " and should be " << constraintOn[1] << " and is actually " << (_constraintSet[1].isDisabled(s_analysis) ? "off" : "on") << endl;
// After setting the state of the model and applying forces
// Compute the derivative of the multibody system (speeds and accelerations)
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);
// Sanity check that constraints hasn't totally changed the configuration of the model
double error = (Q-s_analysis.getQ()).norm();
// Report reaction forces for debugging
/*
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize());
SimTK::Vector mobilityForces(0);
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet.get(i).calcConstraintForces(s_analysis, constraintBodyForces, mobilityForces);
}*/
// VARIABLES
SimTK::Vec3 vec,angVec;
// Get Accelerations for kinematics of bodies
for(int i=0;i<_coordSet.getSize();i++) {
double acc = _coordSet.get(i).getAccelerationValue(s_analysis);
if(getInDegrees())
acc *= SimTK_RADIAN_TO_DEGREE;
_coordIndAccs[i]->append(1, &acc);
}
// cout << "Input Body Names: "<< _bodyNames << endl;
// Get Accelerations for kinematics of bodies
for(int i=0;i<_bodySet.getSize();i++) {
Body &body = _bodySet.get(i);
// cout << "Body Name: "<< body->getName() << endl;
const SimTK::Vec3& com = body.get_mass_center();
// Get the body acceleration
_model->getSimbodyEngine().getAcceleration(s_analysis, body, com, vec);
_model->getSimbodyEngine().getAngularAcceleration(s_analysis, body, angVec);
// CONVERT TO DEGREES?
if(getInDegrees())
angVec *= SimTK_RADIAN_TO_DEGREE;
// FILL KINEMATICS ARRAY
_bodyIndAccs[i]->append(3, &vec[0]);
_bodyIndAccs[i]->append(3, &angVec[0]);
}
// Get Accelerations for kinematics of COM
if(_includeCOM){
// Get the body acceleration in ground
vec = _model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_analysis);
// FILL KINEMATICS ARRAY
_comIndAccs.append(3, &vec[0]);
}
// Get induced constraint reactions for contributor
if(_reportConstraintReactions){
for(int j=0; j<_constraintSet.getSize(); j++){
_constraintReactions.append(_constraintSet[j].getRecordValues(s_analysis));
}
}
} // End cycling through contributors at this time step
// Set the accelerations of coordinates into their storages
int nc = _coordSet.getSize();
for(int i=0; i<nc; i++) {
_storeInducedAccelerations[i]->append(aT, _coordIndAccs[i]->getSize(),&(_coordIndAccs[i]->get(0)));
}
// Set the accelerations of bodies into their storages
int nb = _bodySet.getSize();
for(int i=0; i<nb; i++) {
_storeInducedAccelerations[nc+i]->append(aT, _bodyIndAccs[i]->getSize(),&(_bodyIndAccs[i]->get(0)));
}
// Set the accelerations of system center of mass into a storage
if(_includeCOM){
_storeInducedAccelerations[nc+nb]->append(aT, _comIndAccs.getSize(), &_comIndAccs[0]);
}
if(_reportConstraintReactions){
_storeConstraintReactions->append(aT, _constraintReactions.getSize(), &_constraintReactions[0]);
}
return(0);
}
