/**
* Compute the controls for a simulation.
*
* This method alters the control set in order to control the simulation.
*/
void CMC::
computeControls(SimTK::State& s, ControlSet &controlSet)
{
// CONTROLS SHOULD BE RECOMPUTED- NEED A NEW TARGET TIME
_tf = s.getTime() + _targetDT;
int i,j;
// TURN ANALYSES OFF
_model->updAnalysisSet().setOn(false);
// TIME STUFF
double tiReal = s.getTime();
double tfReal = _tf;
cout<<"CMC.computeControls: t = "<<s.getTime()<<endl;
if(_verbose) {
cout<<"\n\n----------------------------------\n";
cout<<"integration step size = "<<_targetDT<<", target time = "<<_tf<<endl;
}
// SET CORRECTIONS
int nq = _model->getNumCoordinates();
int nu = _model->getNumSpeeds();
FunctionSet *qSet = _predictor->getCMCActSubsys()->getCoordinateTrajectories();
FunctionSet *uSet = _predictor->getCMCActSubsys()->getSpeedTrajectories();
Array<double> qDesired(0.0,nq),uDesired(0.0,nu);
qSet->evaluate(qDesired,0,tiReal);
if(uSet!=NULL) {
uSet->evaluate(uDesired,0,tiReal);
} else {
qSet->evaluate(uDesired,1,tiReal);
}
Array<double> qCorrection(0.0,nq),uCorrection(0.0,nu);
const Vector& q = s.getQ();
const Vector& u = s.getU();
for(i=0;i<nq;i++) qCorrection[i] = q[i] - qDesired[i];
for(i=0;i<nu;i++) uCorrection[i] = u[i] - uDesired[i];
_predictor->getCMCActSubsys()->setCoordinateCorrections(&qCorrection[0]);
_predictor->getCMCActSubsys()->setSpeedCorrections(&uCorrection[0]);
if( _verbose ) {
cout << "\n=============================" << endl;
cout << "\nCMC:computeControls" << endl;
cout << "\nq's = " << s.getQ() << endl;
cout << "\nu's = " << s.getU() << endl;
cout << "\nz's = " << s.getZ() << endl;
cout<<"\nqDesired:"<<qDesired << endl;
cout<<"\nuDesired:"<<uDesired << endl;
cout<<"\nQCorrections:"<<qCorrection << endl;
cout<<"\nUCorrections:"<<uCorrection << endl;
}
// realize to Velocity because some tasks (eg. CMC_Point) need to be
// at velocity to compute errors
_model->getMultibodySystem().realize(s, Stage::Velocity );
// ERRORS
_taskSet->computeErrors(s, tiReal);
_taskSet->recordErrorsAsLastErrors();
Array<double> &pErr = _taskSet->getPositionErrors();
Array<double> &vErr = _taskSet->getVelocityErrors();
if(_verbose) cout<<"\nErrors at time "<<s.getTime()<<":"<<endl;
int e=0;
for(i=0;i<_taskSet->getSize();i++) {
TrackingTask& task = _taskSet->get(i);
if(_verbose) {
for(j=0;j<task.getNumTaskFunctions();j++) {
cout<<task.getName()<<": ";
cout<<"pErr="<<pErr[e]<<" vErr="<<vErr[e]<<endl;
e++;
}
}
}
double *err = new double[pErr.getSize()];
for(i=0;i<pErr.getSize();i++) err[i] = pErr[i];
_pErrStore->append(tiReal,pErr.getSize(),err);
for(i=0;i<vErr.getSize();i++) err[i] = vErr[i];
_vErrStore->append(tiReal,vErr.getSize(),err);
// COMPUTE DESIRED ACCELERATIONS
_taskSet->computeDesiredAccelerations(s, tiReal,tfReal);
// Set the weight of the stress term in the optimization target based on this sigmoid-function-blending
// Note that if no task limits are set then by default the weight will be 1.
// TODO: clean this up -- currently using dynamic_casts to make sure we're not using fast target, etc.
if(dynamic_cast<ActuatorForceTarget*>(_target)) {
double relativeTau = 0.1;
ActuatorForceTarget *realTarget = dynamic_cast<ActuatorForceTarget*>(_target);
Array<double> &pErr = _taskSet->getPositionErrors();
double stressTermWeight = 1;
for(i=0;i<_taskSet->getSize();i++) {
if(dynamic_cast<CMC_Joint*>(&_taskSet->get(i))) {
CMC_Joint& jointTask = dynamic_cast<CMC_Joint&>(_taskSet->get(i));
if(jointTask.getLimit()) {
double w = ForwardTool::SigmaDn(jointTask.getLimit() * relativeTau, jointTask.getLimit(), fabs(pErr[i]));
if(_verbose) cout << "Task " << i << ": err=" << pErr[i] << ", limit=" << jointTask.getLimit() << ", sigmoid=" << w << endl;
stressTermWeight = min(stressTermWeight, w);
}
}
}
if(_verbose) cout << "Setting stress term weight to " << stressTermWeight << " (relativeTau was " << relativeTau << ")" << std::endl;
realTarget->setStressTermWeight(stressTermWeight);
for(i=0;i<vErr.getSize();i++) err[i] = vErr[i];
_stressTermWeightStore->append(tiReal,1,&stressTermWeight);
}
// SET BOUNDS ON CONTROLS
int N = _predictor->getNX();
Array<double> xmin(0.0,N),xmax(1.0,N);
for(i=0;i<N;i++) {
Control& x = controlSet.get(i);
xmin[i] = x.getControlValueMin(tiReal);
xmax[i] = x.getControlValueMax(tiReal);
}
if(_verbose) {
cout<<"\nxmin:\n"<<xmin<<endl;
cout<<"\nxmax:\n"<<xmax<<endl;
}
// COMPUTE BOUNDS ON MUSCLE FORCES
Array<double> zero(0.0,N);
Array<double> fmin(0.0,N),fmax(0.0,N);
_predictor->setInitialTime(tiReal);
_predictor->setFinalTime(tfReal);
_predictor->setTargetForces(&zero[0]);
_predictor->evaluate(s, &xmin[0], &fmin[0]);
_predictor->evaluate(s, &xmax[0], &fmax[0]);
SimTK::State newState = _predictor->getCMCActSubsys()->getCompleteState();
if(_verbose) {
cout<<endl<<endl;
cout<<"\ntiReal = "<<tiReal<<" tfReal = "<<tfReal<<endl;
cout<<"Min forces:\n";
cout<<fmin<<endl;
cout<<"Max forces:\n";
cout<<fmax<<endl;
}
// Print actuator force range if range is small
double range;
for(i=0;i<N;i++) {
range = fmax[i] - fmin[i];
if(range<1.0) {
cout << "CMC::computeControls WARNING- small force range for "
<< getActuatorSet()[i].getName()
<< " ("<<fmin[i]<<" to "<<fmax[i]<<")\n" << endl;
// if the force range is so small it means the control value, x,
// is inconsequential and we might as well choose the smallest control
// value possible, or else the RootSolver will choose the last value
// it used to evaluate the force, which will be the maximum control
// value. In other words, if the fiber length is so short that no level
// of activation can produce force, the RootSolver gets the same answer
// for force if it uses xmin or:: xmax, but since it uses xmax last
// it returns xmax as the control value. Make xmax = xmin to avoid that.
xmax[i] = xmin[i];
}
}
// SOLVE STATIC OPTIMIZATION FOR DESIRED ACTUATOR FORCES
SimTK::Vector lowerBounds(N), upperBounds(N);
for(i=0;i<N;i++) {
if(fmin[i]<fmax[i]) {
lowerBounds[i] = fmin[i];
upperBounds[i] = fmax[i];
} else {
lowerBounds[i] = fmax[i];
upperBounds[i] = fmin[i];
}
}
_target->setParameterLimits(lowerBounds, upperBounds);
// OPTIMIZER ERROR TRAP
_f.setSize(N);
if(!_target->prepareToOptimize(newState, &_f[0])) {
// No direct solution, need to run optimizer
Vector fVector(N,&_f[0],true);
try {
_optimizer->optimize(fVector);
}
catch (const SimTK::Exception::Base& ex) {
cout << ex.getMessage() << endl;
cout << "OPTIMIZATION FAILED..." << endl;
cout<<endl;
ostringstream msg;
msg << "CMC.computeControls: ERROR- Optimizer could not find a solution." << endl;
msg << "Unable to find a feasible solution at time = " << s.getTime() << "." << endl;
msg << "Model cannot generate the forces necessary to achieve the target acceleration." << endl;
msg << "Possible issues: 1. not all model degrees-of-freedom are actuated, " << endl;
msg << "2. there are tracking tasks for locked coordinates, and/or" << endl;
msg << "3. there are unnecessary control constraints on reserve/residual actuators." << endl;
cout<<"\n"<<msg.str()<<endl<<endl;
throw(new OpenSim::Exception(msg.str(), __FILE__,__LINE__));
}
} else {
// Got a direct solution, don't need to run optimizer
}
if(_verbose) _target->printPerformance(&_f[0]);
if(_verbose) {
cout<<"\nDesired actuator forces:\n";
cout<<_f<<endl;
}
// ROOT SOLVE FOR EXCITATIONS
_predictor->setTargetForces(&_f[0]);
RootSolver rootSolver(_predictor);
Array<double> tol(4.0e-3,N);
Array<double> fErrors(0.0,N);
Array<double> controls(0.0,N);
controls = rootSolver.solve(s, xmin,xmax,tol);
if(_verbose) {
cout<<"\n\nXXX t=" << _tf << " Controls:" <<controls<<endl;
}
// FILTER OSCILLATIONS IN CONTROL VALUES
if(_useCurvatureFilter) FilterControls(s, controlSet,_targetDT,controls,_verbose);
// SET EXCITATIONS
controlSet.setControlValues(_tf,&controls[0]);
_model->updAnalysisSet().setOn(true);
}
void testCopyModel(string fileName)
{
Model *model = new Model(fileName);
SimTK::State defaultState = model->initSystem();
Model *modelCopy = new Model(*model);
// At this point properties should all match. assert that
ASSERT(*model==*modelCopy);
delete model;
SimTK::State& defaultStateOfCopy = modelCopy->initSystem();
// Compare state
defaultState.getY().dump("defaultState:Y");
ASSERT ((defaultState.getY()-defaultStateOfCopy.getY()).norm() < 1e-7);
defaultState.getZ().dump("defaultState:Z");
ASSERT ((defaultState.getZ()-defaultStateOfCopy.getZ()).norm() < 1e-7);
// Now delete original model and make sure copy can stand
Model *newModel = modelCopy->clone();
// Compare state again
delete modelCopy;
SimTK::State& defaultStateOfCopy2 = newModel->initSystem();
// Compare state
ASSERT ((defaultState.getY()-defaultStateOfCopy2.getY()).norm() < 1e-7);
ASSERT ((defaultState.getZ()-defaultStateOfCopy2.getZ()).norm() < 1e-7);
delete newModel;
}
/**
* This method is called at the beginning of an analysis so that any
* necessary initializations may be performed.
*
* This method is meant to be called at the beginning of an integration
*
* @param s Current state .
*
* @return -1 on error, 0 otherwise.
*/
int StaticOptimization::
begin(SimTK::State& s )
{
if(!proceed()) return(0);
// Make a working copy of the model
delete _modelWorkingCopy;
_modelWorkingCopy = _model->clone();
_modelWorkingCopy->initSystem();
// Replace model force set with only generalized forces
if(_model) {
SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState();
// Update the _forceSet we'll be computing inverse dynamics for
if(_ownsForceSet) delete _forceSet;
if(_useModelForceSet) {
// Set pointer to model's internal force set
_forceSet = &_modelWorkingCopy->updForceSet();
_ownsForceSet = false;
} else {
ForceSet& as = _modelWorkingCopy->updForceSet();
// Keep a copy of forces that are not muscles to restore them back.
ForceSet* saveForces = as.clone();
// Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
_forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
_ownsForceSet = false;
_modelWorkingCopy->setAllControllersEnabled(false);
_numCoordinateActuators = _forceSet->getSize();
// Copy whatever forces that are not muscles back into the model
for(int i=0; i<saveForces->getSize(); i++){
const Force& f=saveForces->get(i);
if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
as.append(saveForces->get(i).clone());
}
}
SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
// Set modeling options for Actuators to be overriden
for(int i=0; i<_forceSet->getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
if( act ) {
act->overrideActuation(sWorkingCopy, true);
}
}
sWorkingCopy.setTime(s.getTime());
sWorkingCopy.setQ(s.getQ());
sWorkingCopy.setU(s.getU());
sWorkingCopy.setZ(s.getZ());
_modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity);
_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
// Gather indices into speed set corresponding to the unconstrained degrees of freedom
// (for which we will set acceleration constraints)
_accelerationIndices.setSize(0);
const CoordinateSet& coordSet = _model->getCoordinateSet();
for(int i=0; i<coordSet.getSize(); i++) {
const Coordinate& coord = coordSet.get(i);
if(!coord.isConstrained(sWorkingCopy)) {
Array<int> inds = _statesStore->
getColumnIndicesForIdentifier(coord.getName()) ;
_accelerationIndices.append(inds[0]);
}
}
int na = _forceSet->getSize();
int nacc = _accelerationIndices.getSize();
if(na < nacc)
throw(Exception("StaticOptimization: ERROR- over-constrained "
"system -- need at least as many forces as there are degrees of freedom.\n") );
_forceReporter.reset(new ForceReporter(_modelWorkingCopy));
_forceReporter->begin(sWorkingCopy);
_forceReporter->updForceStorage().reset();
_parameters.resize(_modelWorkingCopy->getNumControls());
_parameters = 0;
}
_statesSplineSet=GCVSplineSet(5,_statesStore);
// DESCRIPTION AND LABELS
constructDescription();
constructColumnLabels();
deleteStorage();
allocateStorage();
// RESET STORAGE
_activationStorage->reset(s.getTime());
_forceReporter->updForceStorage().reset(s.getTime());
// RECORD
int status = 0;
if(_activationStorage->getSize()<=0) {
status = record(s);
const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
for(int k=0;k<fs.getSize();k++) {
ScalarActuator* act = dynamic_cast<ScalarActuator *>(&fs[k]);
if (act){
cout << "Bounds for " << act->getName() << ": "
<< act->getMinControl() << " to "
<< act->getMaxControl() << endl;
}
else{
std::string msg = getConcreteClassName();
msg += "::can only process scalar Actuator types.";
throw Exception(msg);
}
}
}
return(status);
}
/**
* Compute the initial states for a simulation.
*
* The caller should send in an initial guess. The Qs and Us are set
* based on the desired trajectories. The actuator states are set by
* solving for a desired set of actuator forces, and then letting the states
* come to equilibrium for those forces.
*
* @param rTI Initial time in normalized time. Note this is changed to
* the time corresponding to the new initial states on return.
* @param s Initial states.
*/
void CMC::
computeInitialStates(SimTK::State& s, double &rTI)
{
int i,j;
int N = _predictor->getNX();
SimTK::State initialState = s;
Array<double> xmin(0.01,N),forces(0.0,N);
double tiReal = rTI;
if( _verbose ) {
cout<<"\n\n=============================================\n";
cout<<"enter CMC.computeInitialStates: ti="<< rTI << " q's=" << s.getQ() <<endl;
cout<<"\nenter CMC.computeInitialStates: ti="<< rTI << " u's=" << s.getU() <<endl;
cout<<"\nenter CMC.computeInitialStates: ti="<< rTI << " z's=" << s.getZ() <<endl;
cout<<"=============================================\n";
}
// TURN ANALYSES OFF
_model->updAnalysisSet().setOn(false);
// CONSTRUCT CONTROL SET
ControlSet xiSet;
for(i=0;i< getNumControls();i++) {
ControlConstant *x = new ControlConstant();
x->setName(_controlSet.get(i).getName());
x->setIsModelControl(true);
// This is not a very good way to set the bounds on the controls because ConrtolConstant only supports constant
// min/max bounds but we may have time-dependent min/max curves specified in the controls constraints file
//
Control& xPredictor = _controlSet.get(i);
x->setDefaultParameterMin(xPredictor.getDefaultParameterMin());
x->setDefaultParameterMax(xPredictor.getDefaultParameterMax());
double xmin = xPredictor.getControlValueMin(tiReal);
if(!SimTK::isNaN(xmin)) x->setControlValueMin(tiReal,xmin);
double xmax = xPredictor.getControlValueMax(tiReal);
if(!SimTK::isNaN(xmax)) x->setControlValueMax(tiReal,xmax);
xiSet.adoptAndAppend(x);
}
// ACTUATOR EQUILIBRIUM
// 1
//
// perform integration but reset the coords and speeds so only actuator
// states at changed
obtainActuatorEquilibrium(s,tiReal,0.200,xmin,true);
if( _verbose ) {
cout<<"\n\n=============================================\n";
cout<<"#1 act Equ. CMC.computeInitialStates: ti="<< rTI << " q's=" << s.getQ() <<endl;
cout<<"\n#1 act Equ. CMC.computeInitialStates: ti="<< rTI << " u's=" << s.getU() <<endl;
cout<<"\n#1 act Equ. CMC.computeInitialStates: ti="<< rTI << " z's=" << s.getZ() <<endl;
cout<<"=============================================\n";
}
restoreConfiguration( s, initialState ); // set internal coord,speeds to initial vals.
// 2
obtainActuatorEquilibrium(s,tiReal,0.200,xmin,true);
if( _verbose ) {
cout<<"\n\n=============================================\n";
cout<<"#2 act Equ. CMC.computeInitialStates: ti="<< rTI << " q's=" << s.getQ() <<endl;
cout<<"\n#2 act Equ. CMC.computeInitialStates: ti="<< rTI << " u's=" << s.getU() <<endl;
cout<<"\n#2 act Equ. CMC.computeInitialStates: ti="<< rTI << " z's=" << s.getZ() <<endl;
cout<<"=============================================\n";
}
restoreConfiguration( s, initialState );
// CHANGE THE TARGET DT ON THE CONTROLLER TEMPORARILY
double oldTargetDT = getTargetDT();
double newTargetDT = 0.030;
setTargetDT(newTargetDT);
// REPEATEDLY CONTROL OVER THE FIRST TIME STEP
Array<double> xi(0.0, getNumControls());
for(i=0;i<2;i++) {
// CLEAR ANY PREVIOUS CONTROL NODES
for(j=0;j<_controlSet.getSize();j++) {
ControlLinear& control = (ControlLinear&)_controlSet.get(j);
control.clearControlNodes();
}
// COMPUTE CONTROLS
s.updTime() = rTI;
computeControls( s, xiSet);
_model->updAnalysisSet().setOn(false);
// GET CONTROLS
xiSet.getControlValues(rTI,xi);
// OBTAIN EQUILIBRIUM
if(i<1) {
obtainActuatorEquilibrium(s,tiReal,0.200,xi,true);
restoreConfiguration(s, initialState );
}
}
// GET NEW STATES
_predictor->evaluate(s, &xi[0], &forces[0]);
rTI += newTargetDT;
// CLEANUP
setTargetDT(oldTargetDT);
_model->updAnalysisSet().setOn(true);
if( _verbose ) {
cout<<"\n\n=============================================\n";
cout<<"finish CMC.computeInitialStates: ti="<< rTI << " q's=" << s.getQ() <<endl;
cout<<"\nfinish CMC.computeInitialStates: ti="<< rTI << " u's=" << s.getU() <<endl;
cout<<"\nfinish CMC.computeInitialStates: ti="<< rTI << " z's=" << s.getZ() <<endl;
cout<<"=============================================\n";
}
}
/**
* 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);
}