/**
* Append actuators from an actuator set to this set. Copies of the actuators are not made.
*
* This method overrides the method in Set<Force> so that several
* internal variables of the actuator set can be updated.
*
* @param aForceSet The set of actuators to be appended.
* @param aAllowDuplicateNames If true, all actuators will be appended; If false, don't append actuators whose
* name already exists in this model's actuator set.
* @return True if successful; false otherwise.
*/
bool ForceSet::append(ForceSet &aForceSet, bool aAllowDuplicateNames)
{
bool success = true;
for(int i=0;i<aForceSet.getSize() && success;i++) {
bool nameExists = false;
if(!aAllowDuplicateNames) {
std::string name = aForceSet.get(i).getName();
for(int j=0;j<getSize();j++) {
if(get(j).getName() == name) {
nameExists = true;
break;
}
}
}
if(!nameExists) {
if(!ModelComponentSet<Force>::adoptAndAppend(&aForceSet.get(i)))
success = false;
}
}
if(success) {
updateActuators();
updateMuscles();
}
return(success);
}
/**
* This method is called at the beginning of an analysis so that any
* necessary initializations may be performed.
*
* This method should be overridden in the child class. It is
* included here so that the child class will not have to implement it if it
* is not necessary.
*
* @param s state of system
*
* @return -1 on error, 0 otherwise.
*/
int InverseDynamics::begin(const SimTK::State& s )
{
if(!proceed()) return(0);
SimTK::Matrix massMatrix;
_model->getMatterSubsystem().calcM(s, massMatrix);
//massMatrix.dump("mass matrix is:");
// Check that m is full rank
try {
SimTK::FactorLU lu(massMatrix);
} catch (const SimTK::Exception::Base&) {
throw Exception("InverseDynamics: ERROR- mass matrix is singular ",__FILE__,__LINE__);
}
// enable the line below when simmath is fixed
//bool singularMassMatrix = lu.isSingular();
//cout << "Mass matrix is " << (singularMassMatrix?"singular":"Non-singular");
// Make a working copy of the model
delete _modelWorkingCopy;
_modelWorkingCopy = _model->clone();
_modelWorkingCopy->updAnalysisSet().setSize(0);
// Replace model force set with only generalized forces
if(_model) {
SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->initSystem();
// 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();
_numCoordinateActuators = _modelWorkingCopy->getActuators().getSize();
} 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);
_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());
}
}
_modelWorkingCopy->setAllControllersEnabled(false);
_ownsForceSet = false;
SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
// Gather indices into speed set corresponding to the unconstrained degrees of freedom (for which we will set acceleration constraints)
_accelerationIndices.setSize(0);
auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
for(size_t i=0u; i<coordinates.size(); ++i) {
const Coordinate& coord = *coordinates[i];
if(!coord.isConstrained(sWorkingCopy)) {
_accelerationIndices.append(static_cast<int>(i));
}
}
_dydt.setSize(_modelWorkingCopy->getNumCoordinates() + _modelWorkingCopy->getNumSpeeds());
int nf = _numCoordinateActuators;
int nacc = _accelerationIndices.getSize();
if(nf < nacc)
throw(Exception("InverseDynamics: ERROR- over-constrained system -- need at least as many forces as there are degrees of freedom.\n"));
// Realize to velocity in case there are any velocity dependent forces
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
_constraintMatrix.resize(nacc,nf);
_constraintVector.resize(nacc);
_performanceMatrix.resize(nf,nf);
_performanceMatrix = 0;
for(int i=0,j=0; i<nf; i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
if( act ) {
act->setActuation(sWorkingCopy, 1);
_performanceMatrix(j,j) = act->getStress(sWorkingCopy);
j++;
}
}
_performanceVector.resize(nf);
_performanceVector = 0;
int lwork = nf + nf + nacc;
_lapackWork.resize(lwork);
}
_statesSplineSet=GCVSplineSet(5,_statesStore);
// DESCRIPTION AND LABELS
constructDescription();
constructColumnLabels();
deleteStorage();
allocateStorage();
// RESET STORAGE
_storage->reset(s.getTime());
// RECORD
int status = 0;
if(_storage->getSize()<=0) {
status = record(s);
}
return(status);
}
/**
* 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);
}
