/*
* This is just a sample code. I inserted some methods from classes
* MRController and MRSEAController which are the parent of
* MRAdaptiveController
*/
void MRController::MRUpdate(double ref, double dt)
{
//simplectic method. The phase is very similar to the continuous case (try it in matlab!!)
f2 = -l1 * xr2 - l2 * xr1 + l2*ref;
xr2 = prev_xr2 + dt * f2;
f1 = xr2;
xr1 = prev_xr1 + dt * f1;
//here you can introduce velocity or torque saturations!!
prev_xr1 = xr1;
prev_xr2 = xr2;
}
MRSEAController::MRSEAController(double l1, double l2) : MRController(l1,l2)
{
//reference model parameters
this->l1 =l1;
this->l2 =l2;
}
//-----------------------------------------------
MRAdaptiveController::MRAdaptiveController(double l1, double l2, double a) : MRSEAController(l1,l2,a)
{
adaptationEnabled = 1;
//this parameter does not influence control bandwidth and adaptation speed
//it influences the precision -> this is exactly what we show in ROBOTICA jun2014 - "Adaptive HRI Control" (Special issue on rehabilitation robotics)
//some unstability can be seen for very low setting -> in agreement with ROBOTICA
//the higher the better but too high lead to noise
L = 2*M_PI*20.0;
f = sqrt(l2)/2/M_PI;
psi = l1/(2*sqrt(l2));
//adaptation speed
rho1 = 1;
rho2 = 0.05;
b_est = 0;
c_est = 0;
}
double MRAdaptiveController::_process(double tau, double dtau, double dt)
{
//Updates the reference model
MRUpdate(ref, dt);
x_tilde = tau - xr1;
dx_tilde = dtau - xr2;
nu = f2 - 2.0*L*dx_tilde - L*L*x_tilde;
s = dx_tilde + L*x_tilde;
//A = Jm/k the only needed parameter!!!
out = A * nu + b_est*dtau + c_est*tau;
//adaptivity
if(adaptationEnabled)
{
dc_est = - rho1*s*tau;
c_est = c_est + dt*dc_est;
cout << "c " << c_est;
db_est = - rho2 * G*s*dtau;
b_est = b_est + dt*db_est;
cout << "\t b " << b_est;
//important: to have proper convergence rho1 and rho2 should be tuned such as
//the two parameters have the same convergence speed.
//you actually do not need the parameter sigma (see the paper)
//sigma can be arbitrary small, in practice sigma = 0
}
cout << endl;
//this is just for safety!!
//parameters shouldn't go beyond these values if the human is really a second order system
if(b_est < 0.0) b_est = 0.0;
else if(b_est > A*2*L) b_est = A*2*L;
if(c_est < 0.0) c_est = 0.0;
else if(c_est > A*L*L) c_est = A*L*L;
return out; //this is a torque, should then be converted in the current
}