SimTK::State StateInitializer::getOptimizedInitState()
{
if(_n_Z == 0)
return _initState;
StateInitializerQP qp(this);
qp.setNumParameters(_n_controls);
PrintVector(_lower_bd_actuatorforces,"_lower_bd_actuatorforces",std::cout);
PrintVector(_upper_bd_actuatorforces,"_upper_bd_actuatorforces",std::cout);
qp.setParameterLimits(_lower_bd_actuatorforces,_upper_bd_actuatorforces);
qp.setNumEqualityConstraints(_UDotRef.size());
qp.setNumInequalityConstraints(0);
//SimTK::Optimizer opt(qp,SimTK::CFSQP);
SimTK::Optimizer opt(qp, SimTK::InteriorPoint);
opt.setConvergenceTolerance(1e-4); //tol
opt.setMaxIterations(200); //200
Vector result(_n_controls);
result.setToZero();
SimTK::Real f = 0.0;
// qp.testQP();
try{
f = opt.optimize(result);
}
catch(const std::exception& ex)
{
std::cout<<ex.what()<<std::endl;
}
std::cout<<"Initial State Optimization Error: "<<f<<std::endl;
SimTK::State stateOpt = _initState;
Vector muscleForces(_n_muscles);
Vector result_forces = result.elementwiseMultiply(_opt_actuator_forces);
muscleForces = result_forces.block(0,0,_n_muscles,1).getAsVector();
Vector tol_lm(_n_muscles), tol_acts(_n_muscles);
Vector muscleFiberLens(_n_muscles), activations(_n_muscles);
tol_lm.setTo(1.0e-6);
tol_acts.setTo(1.0e-3);
PrintVector(result,"result",std::cout);
rootSolveMuscleFiberLength(stateOpt,muscleForces,_lm_min,_lm_max,tol_lm,muscleFiberLens);
ROCINForceController::setStateFiberLength(*_model,muscleFiberLens,stateOpt);
Vector lm_dot(_n_muscles);
lm_dot.setToZero();
_model->getMultibodySystem().realize(_initState,SimTK::Stage::Dynamics);
int idx_musc = 0;
for(int i=0;i<_n_controls;i++)
{
Actuator& act = _model->getActuators().get(i);
Muscle* m = dynamic_cast<Muscle*>(&act);
if(m!= NULL)
{
lm_dot[idx_musc] = m->getSpeed(_initState);//m->getSpeed(_initState)/m->getCosPennationAngle(_initState);//0;//m->getSpeed(_initState);
idx_musc++;
}
}
rootSolveActivations(stateOpt,muscleForces,muscleFiberLens,lm_dot,_acts_min,_acts_max,tol_acts,activations);
ROCINForceController::setStateActivation(*_model,activations,stateOpt);
return stateOpt;
}
