//============================================================================== // CONSTRUCTION AND DESTRUCTION //============================================================================== bool ActuatorForceTargetFast:: prepareToOptimize(SimTK::State& s, double *x) { // Keep around a "copy" of the state so we can use it in objective function // in cases where we're tracking states _saveState = s; #ifdef USE_LINEAR_CONSTRAINT_MATRIX int nf = _controller->getActuatorSet().getSize(); int nc = getNumConstraints(); _constraintMatrix.resize(nc,nf); _constraintVector.resize(nc); Vector f(nf), c(nc); // Build linear constraint matrix and constant constraint vector f = 0; computeConstraintVector(s, f, _constraintVector); for(int j=0; j<nf; j++) { f[j] = 1; computeConstraintVector(s, f, c); _constraintMatrix(j) = (c - _constraintVector); f[j] = 0; } #endif // use temporary copy of state because computeIsokineticForceAssumingInfinitelyStiffTendon // will change the muscle states. This is necessary ONLY in the case of deprecated muscles SimTK::State tempState = s; double activation = 1.0; getController()->getModel().getMultibodySystem().realize( tempState, SimTK::Stage::Dynamics ); // COMPUTE MAX ISOMETRIC FORCE const Set<Actuator>& fSet = _controller->getActuatorSet(); double fOpt = SimTK::NaN; getController()->getModel().getMultibodySystem().realize(tempState, SimTK::Stage::Dynamics ); for(int i=0 ; i<fSet.getSize(); ++i) { ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]); Muscle* mus = dynamic_cast<Muscle*>(act); if(mus==NULL) { fOpt = act->getOptimalForce(); } else{ fOpt = mus->calcInextensibleTendonActiveFiberForce(tempState, activation); } if( std::fabs(fOpt) < SimTK::TinyReal ) fOpt = SimTK::TinyReal; _recipOptForceSquared[i] = 1.0 / (fOpt*fOpt); } // return false to indicate that we still need to proceed with optimization (did not do a lapack direct solve) return false; }
//============================================================================== // CONSTRUCTION //============================================================================== bool StaticOptimizationTarget:: prepareToOptimize(SimTK::State& s, double *x) { // COMPUTE MAX ISOMETRIC FORCE const ForceSet& fSet = _model->getForceSet(); for(int i=0, j=0;i<fSet.getSize();i++) { ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet.get(i)); if( act ) { double fOpt; Muscle *mus = dynamic_cast<Muscle*>(&fSet.get(i)); if( mus ) { //ActivationFiberLengthMuscle *aflmus = dynamic_cast<ActivationFiberLengthMuscle*>(mus); if(mus && _useMusclePhysiology) { _model->setAllControllersEnabled(true); fOpt = mus->calcInextensibleTendonActiveFiberForce(s, 1.0); _model->setAllControllersEnabled(false); } else { fOpt = mus->getMaxIsometricForce(); } } else { fOpt = act->getOptimalForce(); } _optimalForce[j++] = fOpt; } } #ifdef USE_LINEAR_CONSTRAINT_MATRIX //cout<<"Computing linear constraint matrix..."<<endl; int np = getNumParameters(); int nc = getNumConstraints(); _constraintMatrix.resize(nc,np); _constraintVector.resize(nc); Vector pVector(np), cVector(nc); // Build linear constraint matrix and constant constraint vector pVector = 0; computeConstraintVector(s, pVector,_constraintVector); for(int p=0; p<np; p++) { pVector[p] = 1; computeConstraintVector(s, pVector, cVector); for(int c=0; c<nc; c++) _constraintMatrix(c,p) = (cVector[c] - _constraintVector[c]); pVector[p] = 0; } #endif // return false to indicate that we still need to proceed with optimization return false; }
/** * Constructor. * * @param aNX Number of controls. * @param aController Parent controller. */ ActuatorForceTargetFast:: ActuatorForceTargetFast(SimTK::State& s, int aNX,CMC *aController): OptimizationTarget(aNX), _controller(aController) { // NUMBER OF CONTROLS if(getNumParameters()<=0) { throw(Exception("ActuatorForceTargetFast: ERROR- no controls.\n")); } // ALLOCATE STATE ARRAYS int ny = _controller->getModel().getNumStateVariables(); int nq = _controller->getModel().getNumCoordinates(); int nu = _controller->getModel().getNumSpeeds(); int na = _controller->getActuatorSet().getSize(); _y.setSize(ny); _dydt.setSize(ny); _dqdt.setSize(nq); _dudt.setSize(nu); _recipAreaSquared.setSize(na); _recipOptForceSquared.setSize(na); _recipAvgActForceRangeSquared.setSize(na); int nConstraints = _controller->getTaskSet().getNumActiveTaskFunctions(); // NUMBERS OF CONSTRAINTS // There are only linear equality constraints. setNumEqualityConstraints(nConstraints); setNumLinearEqualityConstraints(nConstraints); // DERIVATIVE PERTURBATION SIZES; setDX(1.0e-6); // COMPUTE ACTUATOR AREAS Array<double> f(1.0,na); const Set<Actuator>& fSet = _controller->getActuatorSet(); for(int i=0,j=0;i<fSet.getSize();i++) { ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]); Muscle* musc = dynamic_cast<Muscle *>(act); if(musc) _recipAreaSquared[j] = f[j]/musc->getMaxIsometricForce(); else _recipAreaSquared[j] = f[j]/act->getOptimalForce(); _recipAreaSquared[j] *= _recipAreaSquared[j]; j++; } }