/**
* 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);
}
/**
* Filter the controls. This method was introduced as a means of attempting
* to reduce the sizes of residuals. Unfortunately, the approach was
* generally unsuccessful because the desired accelerations were not
* achieved.
*
* @param aControlSet Set of controls computed by CMC.
* @param aDT Current time window.
* @param rControls Array of filtered controls.
*/
void CMC::
FilterControls(const SimTK::State& s, const ControlSet &aControlSet,double aDT,
OpenSim::Array<double> &rControls,bool aVerbosePrinting)
{
if(aDT <= SimTK::Zero) {
if(aVerbosePrinting) cout<<"\nCMC.filterControls: aDT is practically 0.0, skipping!\n\n";
return;
}
if(aVerbosePrinting) cout<<"\n\nFiltering controls to limit curvature...\n";
int i;
int size = rControls.getSize();
Array<double> x0(0.0,size),x1(0.0,size),x2(0.0,size);
// SET TIMES
double t0,t1/*,t2*/;
// t2 = s.getTime();
t1 = s.getTime() - aDT;
t0 = t1 - aDT;
// SET CONTROL VALUES
x2 = rControls;
aControlSet.getControlValues(t1,x1);
aControlSet.getControlValues(t0,x0);
// LOOP OVER CONTROLS
double m1,m2;
double curvature;
double thresholdCurvature = 2.0 * 0.05 / (aDT * aDT);
//double thresholdSlopeDiff = 0.2 / aDT;
for(i=0;i<size;i++) {
m2 = (x2[i]-x1[i]) / aDT;
m1 = (x1[i]-x0[i]) / aDT;
curvature = (m2 - m1) / aDT;
curvature = fabs(curvature);
if(curvature<=thresholdCurvature) continue;
// diff = fabs(m2) - fabs(m1);
// cout<<"thresholdSlopeDiff="<<thresholdSlopeDiff<<" slopeDiff="<<diff<<endl;
// if(diff>thresholdSlopeDiff) continue;
// ALTER CONTROL VALUE
rControls[i] = (3.0*x2[i] + 2.0*x1[i] + x0[i]) / 6.0;
// PRINT
if(aVerbosePrinting) cout<<aControlSet[i].getName()<<": old="<<x2[i]<<" new="<<rControls[i]<<endl;
}
if(aVerbosePrinting) cout<<endl<<endl;
}
/**
* 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";
}
}
//==========================================================================================================
void testControlSetControllerOnBlock()
{
using namespace SimTK;
// Create a new OpenSim model
Model osimModel;
osimModel.setName("osimModel");
// Get the ground body
OpenSim::Body& ground = osimModel.getGroundBody();
// Create a 20 kg, 0.1 m^3 block body
double blockMass = 20.0, blockSideLength = 0.1;
Vec3 blockMassCenter(0), groundOrigin(0), blockInGround(0, blockSideLength/2, 0);
Inertia blockIntertia = Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
OpenSim::Body block("block", blockMass, blockMassCenter, blockMass*blockIntertia);
//Create a free joint with 6 degrees-of-freedom
SimTK::Vec3 noRotation(0);
SliderJoint blockToGround("",ground, blockInGround, noRotation, block, blockMassCenter, noRotation);
// Create 6 coordinates (degrees-of-freedom) between the ground and block
CoordinateSet& jointCoordinateSet = blockToGround.upd_CoordinateSet();
double posRange[2] = {-1, 1};
jointCoordinateSet[0].setName("xTranslation");
jointCoordinateSet[0].setMotionType(Coordinate::Translational);
jointCoordinateSet[0].setRange(posRange);
// Add the block body to the model
osimModel.addBody(&block);
// Define a single coordinate actuator.
CoordinateActuator actuator(jointCoordinateSet[0].getName());
actuator.setName("actuator");
// Add the actuator to the model
osimModel.addForce(&actuator);
double initialTime = 0;
double finalTime = 1.0;
// Define the initial and final control values
double controlForce[1] = {100};
// Create two control signals
ControlLinear control;
control.setName("actuator");
// Create a control set and add the controls to the set
ControlSet actuatorControls;
actuatorControls.adoptAndAppend(&control);
actuatorControls.setMemoryOwner(false);
actuatorControls.setControlValues(initialTime, controlForce);
actuatorControls.setControlValues(finalTime, controlForce);
// Create a control set controller that simply applies controls from a ControlSet
ControlSetController actuatorController;
// Make a copy and set it on the ControlSetController as it takes ownership of the
// ControlSet passed in
actuatorController.setControlSet((ControlSet*)Object::SafeCopy(&actuatorControls));
// add the controller to the model
osimModel.addController(&actuatorController);
// Initialize the system and get the state representing the state system
SimTK::State& si = osimModel.initSystem();
// Specify zero slider joint kinematic states
CoordinateSet &coordinates = osimModel.updCoordinateSet();
coordinates[0].setValue(si, 0.0); // x translation
coordinates[0].setSpeedValue(si, 0.0); // x speed
// Create the integrator and manager for the simulation.
double accuracy = 1.0e-3;
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(accuracy);
Manager manager(osimModel, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\n\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
manager.integrate(si);
si.getQ().dump("Final position:");
double x_err = fabs(coordinates[0].getValue(si) - 0.5*(controlForce[0]/blockMass)*finalTime*finalTime);
ASSERT(x_err <= accuracy, __FILE__, __LINE__, "ControlSetControllerOnBlock failed to produce the expected motion.");
// Save the simulation results
Storage states(manager.getStateStorage());
states.print("block_push.sto");
osimModel.disownAllComponents();
}// end of testControlSetControllerOnBlock()
