//============================================================================== // 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++; } }
/** * This function is called at every time step for every actuator. * * @param s Current state of the system * @param controls Controls being calculated */ void computeControls(const SimTK::State& s, SimTK::Vector &controls) const { // Get the current time in the simulation. double t = s.getTime(); // Read the mass of the block. double blockMass = getModel().getBodySet().get( "block" ).getMass(); // Get pointers to each of the muscles in the model. Muscle* leftMuscle = dynamic_cast<Muscle*> ( &getActuatorSet().get(0) ); Muscle* rightMuscle = dynamic_cast<Muscle*> ( &getActuatorSet().get(1) ); // Compute the desired position of the block in the tug-of-war // model. double zdes = desiredModelZPosition(t); // Compute the desired velocity of the block in the tug-of-war // model. double zdesv = desiredModelZVelocity(t); // Compute the desired acceleration of the block in the tug- // of-war model. double zdesa = desiredModelZAcceleration(t); // Get the z translation coordinate in the model. const Coordinate& zCoord = _model->getCoordinateSet(). get( "blockToGround_zTranslation" ); // Get the current position of the block in the tug-of-war // model. double z = zCoord.getValue(s); // Get the current velocity of the block in the tug-of-war // model. double zv = zCoord.getSpeedValue(s); // Compute the correction to the desired acceleration arising // from the deviation of the block's current position from its // desired position (this deviation is the "position error"). double pErrTerm = kp * ( zdes - z ); // Compute the total desired velocity based on the initial // desired velocity plus the position error term we // computed above. double vErrTerm = kv * ( zdesv - zv ); // Compute the total desired acceleration based on the initial // desired acceleration plus the position error term we // computed above. double desAcc = zdesa + vErrTerm + pErrTerm; // Compute the desired force on the block as the mass of the // block times the total desired acceleration of the block. double desFrc = desAcc * blockMass; // Get the maximum isometric force for the left muscle. double FoptL = leftMuscle->getMaxIsometricForce(); // Get the maximum isometric force for the right muscle. double FoptR = rightMuscle->getMaxIsometricForce(); // If desired force is in direction of one muscle's pull // direction, then set that muscle's control based on desired // force. Otherwise, set the muscle's control to zero. double leftControl = 0.0, rightControl = 0.0; if( desFrc < 0 ) { leftControl = abs( desFrc ) / FoptL; rightControl = 0.0; } else if( desFrc > 0 ) { leftControl = 0.0; rightControl = abs( desFrc ) / FoptR; } // Don't allow any control value to be greater than one. if( leftControl > 1.0 ) leftControl = 1.0; if( rightControl > 1.0 ) rightControl = 1.0; // Thelen muscle has only one control Vector muscleControl(1, leftControl); // Add in the controls computed for this muscle to the set of all model controls leftMuscle->addInControls(muscleControl, controls); // Specify control for other actuator (muscle) controlled by this controller muscleControl[0] = rightControl; rightMuscle->addInControls(muscleControl, controls); }