void StaticOptimizationTarget:: computeActuatorAreas(const SimTK::State& s ) { // COMPUTE ACTUATOR AREAS ForceSet& forceSet = _model->updForceSet(); for(int i=0, j=0;i<forceSet.getSize();i++) { ScalarActuator *act = dynamic_cast<ScalarActuator*>(&forceSet.get(i)); if( act ) { act->setActuation(s, 1.0); _recipAreaSquared[j] = act->getStress(s); _recipAreaSquared[j] *= _recipAreaSquared[j]; j++; } } }
void ActuatorForceTarget:: computePerformanceVectors(SimTK::State& s, const Vector &aF, Vector &rAccelPerformanceVector, Vector &rForcePerformanceVector) { const Set<Actuator> &fSet = _controller->getActuatorSet(); for(int i=0;i<fSet.getSize();i++) { ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]); act->setOverrideActuation(s, aF[i]); act->overrideActuation(s, true); } _controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration ); CMC_TaskSet& taskSet = _controller->updTaskSet(); taskSet.computeAccelerations(s); Array<double> &w = taskSet.getWeights(); Array<double> &aDes = taskSet.getDesiredAccelerations(); Array<double> &a = taskSet.getAccelerations(); // PERFORMANCE double sqrtStressTermWeight = sqrt(_stressTermWeight); for(int i=0;i<fSet.getSize();i++) { ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]); rForcePerformanceVector[i] = sqrtStressTermWeight * act->getStress(s); } int nacc = aDes.getSize(); for(int i=0;i<nacc;i++) rAccelPerformanceVector[i] = sqrt(w[i]) * (a[i] - aDes[i]); // reset the actuator control for(int i=0;i<fSet.getSize();i++) { ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]); act->overrideActuation(s, false); } }
/** * 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); }