Commit 211d060c authored by andreadelprete's avatar andreadelprete
Browse files

[adm-ctrl] Add filtered version of force sensor signals for proportional...

[adm-ctrl] Add filtered version of force sensor signals for proportional control. Add force integral dead-zone. Use Jacobian transpose by default.
parent 9a95781d
...@@ -69,19 +69,22 @@ namespace dynamicgraph { ...@@ -69,19 +69,22 @@ namespace dynamicgraph {
void init(const double& dt, const std::string& robotRef); void init(const double& dt, const std::string& robotRef);
/* --- SIGNALS --- */ /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector); DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector); DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector); DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector); DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// 6d reference force DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector); /// damping factors used for the 4 end-effectors DECLARE_SIGNAL_IN(fRightFootFiltered,dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector); /// damping factors used for the 4 end-effectors
// DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force // DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force // DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force
...@@ -91,8 +94,8 @@ namespace dynamicgraph { ...@@ -91,8 +94,8 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// control DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// control
// DEBUG SIGNALS // DEBUG SIGNALS
DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f) DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(fRightFootError, dynamicgraph::Vector); /// fRef-f DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector); ///
DECLARE_SIGNAL_OUT(fLeftFootError, dynamicgraph::Vector); /// fRef-f DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector); ///
// DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f // DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f
// DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f // DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f
......
...@@ -499,8 +499,10 @@ def create_admittance_ctrl(robot, conf, dt=0.001, robot_name='robot'): ...@@ -499,8 +499,10 @@ def create_admittance_ctrl(robot, conf, dt=0.001, robot_name='robot'):
admit_ctrl = AdmittanceController("adm_ctrl"); admit_ctrl = AdmittanceController("adm_ctrl");
plug(robot.encoders.sout, admit_ctrl.encoders); plug(robot.encoders.sout, admit_ctrl.encoders);
plug(robot.filters.estimator_kin.dx, admit_ctrl.jointsVelocities); plug(robot.filters.estimator_kin.dx, admit_ctrl.jointsVelocities);
plug(robot.estimator_ft.contactWrenchRightSole, admit_ctrl.fRightFoot); plug(robot.device.forceRLEG, admit_ctrl.fRightFoot);
plug(robot.estimator_ft.contactWrenchLeftSole, admit_ctrl.fLeftFoot); plug(robot.device.forceLLEG, admit_ctrl.fLeftFoot);
plug(robot.filters.ft_RF_filter.x_filtered, admit_ctrl.fRightFootFiltered);
plug(robot.filters.ft_LF_filter.x_filtered, admit_ctrl.fLeftFootFiltered);
plug(robot.inv_dyn.f_des_right_foot, admit_ctrl.fRightFootRef); plug(robot.inv_dyn.f_des_right_foot, admit_ctrl.fRightFootRef);
plug(robot.inv_dyn.f_des_left_foot, admit_ctrl.fLeftFootRef); plug(robot.inv_dyn.f_des_left_foot, admit_ctrl.fLeftFootRef);
...@@ -511,6 +513,14 @@ def create_admittance_ctrl(robot, conf, dt=0.001, robot_name='robot'): ...@@ -511,6 +513,14 @@ def create_admittance_ctrl(robot, conf, dt=0.001, robot_name='robot'):
admit_ctrl.kp_vel.value = conf.kp_vel; admit_ctrl.kp_vel.value = conf.kp_vel;
admit_ctrl.ki_vel.value = conf.ki_vel; admit_ctrl.ki_vel.value = conf.ki_vel;
admit_ctrl.force_integral_saturation.value = conf.force_integral_saturation; admit_ctrl.force_integral_saturation.value = conf.force_integral_saturation;
admit_ctrl.force_integral_deadzone.value = conf.force_integral_deadzone;
# connect it to torque control
from dynamic_graph.sot.core import Add_of_vector
robot.sum_torque_adm = Add_of_vector('sum_torque_adm');
plug(robot.inv_dyn.tau_des, robot.sum_torque_adm.sin1);
plug(admit_ctrl.u, robot.sum_torque_adm.sin2);
plug(robot.sum_torque_adm.sout, robot.torque_ctrl.jointsTorquesDesired);
admit_ctrl.init(dt, robot_name); admit_ctrl.init(dt, robot_name);
return admit_ctrl; return admit_ctrl;
......
...@@ -25,7 +25,9 @@ def get_sim_conf(): ...@@ -25,7 +25,9 @@ def get_sim_conf():
import dynamic_graph.sot.torque_control.hrp2.joint_torque_controller_conf as joint_torque_controller_conf import dynamic_graph.sot.torque_control.hrp2.joint_torque_controller_conf as joint_torque_controller_conf
import dynamic_graph.sot.torque_control.hrp2.joint_pos_ctrl_gains_sim as pos_ctrl_gains import dynamic_graph.sot.torque_control.hrp2.joint_pos_ctrl_gains_sim as pos_ctrl_gains
import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params
import dynamic_graph.sot.torque_control.hrp2.admittance_ctrl_conf as admittance_ctrl_conf
conf = Bunch(); conf = Bunch();
conf.adm_ctrl = admittance_ctrl_conf;
conf.balance_ctrl = balance_ctrl_conf; conf.balance_ctrl = balance_ctrl_conf;
conf.base_estimator = base_estimator_conf; conf.base_estimator = base_estimator_conf;
conf.control_manager = control_manager_conf; conf.control_manager = control_manager_conf;
......
...@@ -41,16 +41,18 @@ namespace dynamicgraph ...@@ -41,16 +41,18 @@ namespace dynamicgraph
#define REF_FORCE_SIGNALS m_fRightFootRefSIN << m_fLeftFootRefSIN #define REF_FORCE_SIGNALS m_fRightFootRefSIN << m_fLeftFootRefSIN
// m_fRightHandRefSIN << m_fLeftHandRefSIN // m_fRightHandRefSIN << m_fLeftHandRefSIN
#define FORCE_SIGNALS m_fRightFootSIN << m_fLeftFootSIN #define FORCE_SIGNALS m_fRightFootSIN << m_fLeftFootSIN << m_fRightFootFilteredSIN << m_fLeftFootFilteredSIN
// m_fRightHandSIN << m_fLeftHandSIN // m_fRightHandSIN << m_fLeftHandSIN
#define GAIN_SIGNALS m_kp_forceSIN << m_ki_forceSIN << m_kp_velSIN << m_ki_velSIN << m_force_integral_saturationSIN #define GAIN_SIGNALS m_kp_forceSIN << m_ki_forceSIN << \
m_kp_velSIN << m_ki_velSIN << \
m_force_integral_saturationSIN << m_force_integral_deadzoneSIN
#define STATE_SIGNALS m_encodersSIN << m_jointsVelocitiesSIN #define STATE_SIGNALS m_encodersSIN << m_jointsVelocitiesSIN
#define INPUT_SIGNALS STATE_SIGNALS << REF_FORCE_SIGNALS << \ #define INPUT_SIGNALS STATE_SIGNALS << REF_FORCE_SIGNALS << \
FORCE_SIGNALS << GAIN_SIGNALS << m_controlledJointsSIN << m_dampingSIN FORCE_SIGNALS << GAIN_SIGNALS << m_controlledJointsSIN << m_dampingSIN
#define FORCE_ERROR_SIGNALS m_fRightFootErrorSOUT << m_fLeftFootErrorSOUT //<< m_fRightHandErrorSOUT << m_fLeftHandErrorSOUT #define DES_VEL_SIGNALS m_vDesRightFootSOUT << m_vDesLeftFootSOUT //<< m_fRightHandErrorSOUT << m_fLeftHandErrorSOUT
#define OUTPUT_SIGNALS m_uSOUT << m_dqDesSOUT << FORCE_ERROR_SIGNALS #define OUTPUT_SIGNALS m_uSOUT << m_dqDesSOUT << DES_VEL_SIGNALS
/// Define EntityClassName here rather than in the header file /// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
...@@ -74,35 +76,38 @@ namespace dynamicgraph ...@@ -74,35 +76,38 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(kp_vel, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(kp_vel, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(ki_vel, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(ki_vel, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(fRightFoot, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(fRightFoot, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(controlledJoints, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(controlledJoints, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(damping, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(damping, dynamicgraph::Vector)
// ,CONSTRUCT_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector) // ,CONSTRUCT_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector)
// ,CONSTRUCT_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector) // ,CONSTRUCT_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector)
// ,CONSTRUCT_SIGNAL_IN(fRightHand, dynamicgraph::Vector) // ,CONSTRUCT_SIGNAL_IN(fRightHand, dynamicgraph::Vector)
// ,CONSTRUCT_SIGNAL_IN(fLeftHand, dynamicgraph::Vector) // ,CONSTRUCT_SIGNAL_IN(fLeftHand, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, STATE_SIGNALS<< ,CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, STATE_SIGNALS<<
FORCE_SIGNALS<< m_controlledJointsSIN)
REF_FORCE_SIGNALS<< ,CONSTRUCT_SIGNAL_OUT(dqDes, dynamicgraph::Vector, STATE_SIGNALS <<
GAIN_SIGNALS<< DES_VEL_SIGNALS <<
m_controlledJointsSIN) m_dampingSIN)
,CONSTRUCT_SIGNAL_OUT(dqDes, dynamicgraph::Vector, STATE_SIGNALS << ,CONSTRUCT_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector, m_fRightFootSIN <<
GAIN_SIGNALS << m_fRightFootFilteredSIN <<
FORCE_ERROR_SIGNALS << m_fRightFootRefSIN <<
m_dampingSIN) GAIN_SIGNALS)
,CONSTRUCT_SIGNAL_OUT(fRightFootError, dynamicgraph::Vector, m_fRightFootSIN << ,CONSTRUCT_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector, m_fLeftFootSIN <<
m_fRightFootRefSIN) m_fLeftFootFilteredSIN <<
,CONSTRUCT_SIGNAL_OUT(fLeftFootError, dynamicgraph::Vector, m_fLeftFootSIN << m_fLeftFootRefSIN <<
m_fLeftFootRefSIN) GAIN_SIGNALS)
// ,CONSTRUCT_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector, m_fRightHandSIN << // ,CONSTRUCT_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector, m_fRightHandSIN <<
// m_fRightHandRefSIN) // m_fRightHandRefSIN)
// ,CONSTRUCT_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector, m_fLeftHandSIN << // ,CONSTRUCT_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector, m_fLeftHandSIN <<
// m_fLeftHandRefSIN) // m_fLeftHandRefSIN)
,m_initSucceeded(false) ,m_initSucceeded(false)
,m_useJacobianTranspose(false) ,m_useJacobianTranspose(true)
,m_firstIter(true) ,m_firstIter(true)
{ {
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS ); Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
...@@ -225,57 +230,14 @@ namespace dynamicgraph ...@@ -225,57 +230,14 @@ namespace dynamicgraph
getProfiler().start(PROFILE_DQ_DES_COMPUTATION); getProfiler().start(PROFILE_DQ_DES_COMPUTATION);
{ {
const Vector6& e_f_RF = m_fRightFootErrorSOUT(iter); // 6 const Eigen::Vector6d v_des_LF = m_vDesLeftFootSOUT(iter);
const Vector6& e_f_LF = m_fLeftFootErrorSOUT(iter); // 6 const Eigen::Vector6d v_des_RF = m_vDesRightFootSOUT(iter);
const Vector6d& kp = m_kp_forceSIN(iter);
const Vector6d& ki = m_ki_forceSIN(iter);
const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
const Vector& q_sot = m_encodersSIN(iter); // n const Vector& q_sot = m_encodersSIN(iter); // n
// const Vector& dq_sot = m_jointsVelocitiesSIN(iter); // n // const Vector& dq_sot = m_jointsVelocitiesSIN(iter); // n
//const Vector& qMask = m_controlledJointsSIN(iter); // n //const Vector& qMask = m_controlledJointsSIN(iter); // n
//const Eigen::Vector4d& damping = m_dampingSIN(iter); // 4 //const Eigen::Vector4d& damping = m_dampingSIN(iter); // 4
assert(q_sot.size()==m_nj && "Unexpected size of signal encoder"); assert(q_sot.size()==m_nj && "Unexpected size of signal encoder");
// assert(dq.size()==m_nj && "Unexpected size of signal dq");
// assert(qMask.size()==m_nj && "Unexpected size of signal controlledJoints");
Eigen::Vector6d v_des_RF = -kp.cwiseProduct(e_f_RF);
Eigen::Vector6d v_des_LF = -kp.cwiseProduct(e_f_LF);
m_v_RF_int -= m_dt*ki.cwiseProduct(e_f_RF);
m_v_LF_int -= m_dt*ki.cwiseProduct(e_f_LF);
// saturate
bool saturating_LF = false;
bool saturating_RF = false;
for(int i=0; i<6; i++)
{
if(m_v_RF_int(i) > f_sat(i))
{
saturating_RF = true;
m_v_RF_int(i) = f_sat(i);
}
else if(m_v_RF_int(i) < -f_sat(i))
{
saturating_RF = true;
m_v_RF_int(i) = -f_sat(i);
}
if(m_v_LF_int(i) > f_sat(i))
{
saturating_LF = true;
m_v_LF_int(i) = f_sat(i);
}
else if(m_v_LF_int(i) < -f_sat(i))
{
saturating_LF = true;
m_v_LF_int(i) = -f_sat(i);
}
}
if(saturating_LF)
SEND_INFO_STREAM_MSG("Saturate m_v_LF_int integral: "+toString(m_v_LF_int.transpose()));
if(saturating_RF)
SEND_INFO_STREAM_MSG("Saturate m_v_RF_int integral: "+toString(m_v_RF_int.transpose()));
/// *** Compute all Jacobians *** /// *** Compute all Jacobians ***
...@@ -293,19 +255,16 @@ namespace dynamicgraph ...@@ -293,19 +255,16 @@ namespace dynamicgraph
/// Compute admittance control law /// Compute admittance control law
if(m_useJacobianTranspose) if(m_useJacobianTranspose)
{ {
m_dq_des_urdf = m_J_RF.rightCols(m_nj).transpose()*(v_des_RF+m_v_RF_int); m_dq_des_urdf = m_J_RF.rightCols(m_nj).transpose()*v_des_RF;
m_dq_des_urdf += m_J_LF.rightCols(m_nj).transpose()*(v_des_LF+m_v_LF_int); m_dq_des_urdf += m_J_LF.rightCols(m_nj).transpose()*v_des_LF;
} }
else else
{ {
m_J_RF_QR.compute(m_J_RF.rightCols(m_nj)); m_J_RF_QR.compute(m_J_RF.rightCols(m_nj));
m_J_LF_QR.compute(m_J_LF.rightCols(m_nj)); m_J_LF_QR.compute(m_J_LF.rightCols(m_nj));
m_dq_des_urdf = m_J_RF_QR.solve(v_des_RF+m_v_RF_int); m_dq_des_urdf = m_J_RF_QR.solve(v_des_RF);
// SEND_INFO_STREAM_MSG("v_des_RF+m_v_RF_int:" + toString((v_des_RF+m_v_RF_int).transpose())); m_dq_des_urdf += m_J_LF_QR.solve(v_des_LF);
// SEND_INFO_STREAM_MSG("dq_des_urdf (RF only):" + toString(m_dq_des_urdf.transpose()));
m_dq_des_urdf += m_J_LF_QR.solve(v_des_LF+m_v_LF_int);
// SEND_INFO_STREAM_MSG("dq_des_urdf (RF+LF):" + toString(m_dq_des_urdf.transpose()));
} }
if(s.size()!=m_nj) if(s.size()!=m_nj)
...@@ -318,34 +277,99 @@ namespace dynamicgraph ...@@ -318,34 +277,99 @@ namespace dynamicgraph
return s; return s;
} }
DEFINE_SIGNAL_OUT_FUNCTION(fRightFootError,dynamicgraph::Vector) DEFINE_SIGNAL_OUT_FUNCTION(vDesRightFoot,dynamicgraph::Vector)
{ {
if(!m_initSucceeded) if(!m_initSucceeded)
{ {
SEND_MSG("Cannot compute signal fRightFootError before initialization!", MSG_TYPE_WARNING_STREAM); SEND_MSG("Cannot compute signal vDesRightFoot before initialization!", MSG_TYPE_WARNING_STREAM);
return s; return s;
} }
const Vector6& f = m_fRightFootSIN(iter); // 6 const Vector6& f = m_fRightFootSIN(iter);
const Vector6& fRef = m_fRightFootRefSIN(iter); // 6 const Vector6& f_filt = m_fRightFootFilteredSIN(iter);
if(s.size()!=6) const Vector6& fRef = m_fRightFootRefSIN(iter);
s.resize(6); const Vector6d& kp = m_kp_forceSIN(iter);
s = fRef - f; const Vector6d& ki = m_ki_forceSIN(iter);
const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
Eigen::Vector6d err = fRef - f;
Eigen::Vector6d err_filt = fRef - f_filt;
Eigen::Vector6d v_des = -kp.cwiseProduct(err_filt);
for(int i=0; i<6; i++)
{
if(err(i)>dz(i))
m_v_RF_int(i) -= m_dt * ki(i) * (err(i)-dz(i));
else if(err(i)<-dz(i))
m_v_RF_int(i) -= m_dt * ki(i) * (err(i)+dz(i));
}
// saturate
bool saturating = false;
for(int i=0; i<6; i++)
{
if(m_v_RF_int(i) > f_sat(i))
{
saturating = true;
m_v_RF_int(i) = f_sat(i);
}
else if(m_v_RF_int(i) < -f_sat(i))
{
saturating = true;
m_v_RF_int(i) = -f_sat(i);
}
}
if(saturating)
SEND_INFO_STREAM_MSG("Saturate m_v_RF_int integral: "+toString(m_v_RF_int.transpose()));
s = v_des + m_v_RF_int;
return s; return s;
} }
DEFINE_SIGNAL_OUT_FUNCTION(fLeftFootError,dynamicgraph::Vector) DEFINE_SIGNAL_OUT_FUNCTION(vDesLeftFoot,dynamicgraph::Vector)
{ {
if(!m_initSucceeded) if(!m_initSucceeded)
{ {
SEND_MSG("Cannot compute signal fLeftFootError before initialization!", MSG_TYPE_WARNING_STREAM); SEND_MSG("Cannot compute signal vDesLeftFoot before initialization!", MSG_TYPE_WARNING_STREAM);
return s; return s;
} }
const Vector6& f = m_fLeftFootSIN(iter); // 6 const Vector6& f = m_fLeftFootSIN(iter);
const Vector6& fRef = m_fLeftFootRefSIN(iter); // 6 const Vector6& f_filt = m_fLeftFootFilteredSIN(iter);
if(s.size()!=6) const Vector6& fRef = m_fLeftFootRefSIN(iter);
s.resize(6); const Vector6d& kp = m_kp_forceSIN(iter);
s = fRef - f; const Vector6d& ki = m_ki_forceSIN(iter);
return s; const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
Eigen::Vector6d err = fRef - f;
Eigen::Vector6d err_filt = fRef - f_filt;
Eigen::Vector6d v_des = -kp.cwiseProduct(err_filt);
for(int i=0; i<6; i++)
{
if(err(i)>dz(i))
m_v_LF_int(i) -= m_dt * ki(i) * (err(i)-dz(i));
else if(err(i)<-dz(i))
m_v_LF_int(i) -= m_dt * ki(i) * (err(i)+dz(i));
}
// saturate
bool saturating = false;
for(int i=0; i<6; i++)
{
if(m_v_LF_int(i) > f_sat(i))
{
saturating = true;
m_v_LF_int(i) = f_sat(i);
}
else if(m_v_LF_int(i) < -f_sat(i))
{
saturating = true;
m_v_LF_int(i) = -f_sat(i);
}
}
if(saturating)
SEND_INFO_STREAM_MSG("Saturate m_v_LF_int integral: "+toString(m_v_LF_int.transpose()));
s = v_des + m_v_LF_int;
} }
// DEFINE_SIGNAL_OUT_FUNCTION(fRightHandError,dynamicgraph::Vector) // DEFINE_SIGNAL_OUT_FUNCTION(fRightHandError,dynamicgraph::Vector)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment