Skip to content
GitLab
Commit e13deb15 authored by andreadelprete's avatar andreadelprete
Browse files

[balance-ctrl] Remove computation of reference joint velocity for admittance control.

parent 211d060c
Hide whitespace changes
Inline Side-by-side
include/sot/torque_control/inverse-dynamics-balance-controller.hh
View file @ e13deb15
......@@ -115,8 +115,6 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_admittance, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_admittance, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(w_feet, double);
......@@ -154,7 +152,6 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix);
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dq_admittance, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector);
......
python/dynamic_graph/sot/torque_control/create_entities_utils.py
View file @ e13deb15
......@@ -316,7 +316,6 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
plug(robot.estimator_ft.contactWrenchRightSole, ctrl.wrench_right_foot);
plug(robot.estimator_ft.contactWrenchLeftSole, ctrl.wrench_left_foot);
plug(ctrl.tau_des, robot.torque_ctrl.jointsTorquesDesired);
plug(ctrl.dq_admittance, robot.torque_ctrl.dq_des);
plug(ctrl.tau_des, robot.estimator_ft.tauDes);
plug(ctrl.right_foot_pos, robot.rf_traj_gen.initial_value);
......@@ -364,8 +363,6 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
ctrl.kd_posture.value = conf.kd_posture;
ctrl.kp_pos.value = conf.kp_pos;
ctrl.kd_pos.value = conf.kd_pos;
ctrl.kp_admittance.value = conf.kp_admittance;
ctrl.ki_admittance.value = conf.ki_admittance;
ctrl.w_com.value = conf.w_com;
ctrl.w_feet.value = conf.w_feet;
......
src/inverse-dynamics-balance-controller.cpp
View file @ e13deb15
......@@ -60,16 +60,10 @@ namespace dynamicgraph
#define PROFILE_HQP_SOLUTION "InvDynBalCtrl: HQP"
#define PROFILE_PREPARE_INV_DYN "InvDynBalCtrl: prepare inv-dyn"
#define PROFILE_READ_INPUT_SIGNALS "InvDynBalCtrl: read input signals"
#define PROFILE_DQ_ADMITTANCE "InvDynBalCtrl: dq admittance"
#define ZERO_FORCE_THRESHOLD 1e-3
#define ADMITTANCE_SIGNALS m_kp_admittanceSIN \
<< m_ki_admittanceSIN \
<< m_wrench_left_footSIN \
<< m_wrench_right_footSIN
#define INV_DYN_SIGNALS m_com_ref_posSIN \
#define INPUT_SIGNALS m_com_ref_posSIN \
<< m_com_ref_velSIN \
<< m_com_ref_accSIN \
<< m_rf_ref_posSIN \
......@@ -123,9 +117,9 @@ namespace dynamicgraph
<< m_qSIN \
<< m_vSIN \
<< m_wrench_baseSIN \
<< m_active_jointsSIN
#define INPUT_SIGNALS ADMITTANCE_SIGNALS << INV_DYN_SIGNALS
<< m_active_jointsSIN \
<< m_wrench_left_footSIN \
<< m_wrench_right_footSIN
#define OUTPUT_SIGNALS m_tau_desSOUT \
<< m_f_des_right_footSOUT \
......@@ -153,7 +147,6 @@ namespace dynamicgraph
<< m_right_foot_acc_desSOUT \
<< m_left_foot_acc_desSOUT \
<< m_dv_desSOUT \
<< m_dq_admittanceSOUT \
<< m_MSOUT
/// Define EntityClassName here rather than in the header file
......@@ -202,8 +195,6 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_admittance, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(ki_admittance, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(w_com, double)
,CONSTRUCT_SIGNAL_IN(w_feet, double)
,CONSTRUCT_SIGNAL_IN(w_posture, double)
......@@ -232,7 +223,7 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INV_DYN_SIGNALS)
,CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS)
,CONSTRUCT_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector, m_f_des_right_footSOUT)
......@@ -250,9 +241,6 @@ namespace dynamicgraph
m_zmp_left_footSOUT<<
m_zmp_right_footSOUT)
,CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(dq_admittance, dg::Vector, ADMITTANCE_SIGNALS <<
m_f_des_right_footSOUT <<
m_f_des_left_footSOUT)
,CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT)
......@@ -881,53 +869,6 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(dq_admittance, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal dq_admittance before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints)
s.resize(m_robot_util->m_nbJoints);
const Eigen::Vector6d& f_des_RF = m_f_des_right_footSOUT(iter);
const Eigen::Vector6d& f_des_LF = m_f_des_left_footSOUT(iter);
const Eigen::Vector6d& f_RF = m_wrench_right_footSIN(iter);
const Eigen::Vector6d& f_LF = m_wrench_left_footSIN(iter);
const Eigen::Vector6d& kp = m_kp_admittanceSIN(iter);
const Eigen::Vector6d& ki = m_ki_admittanceSIN(iter);
getProfiler().start(PROFILE_DQ_ADMITTANCE);
Eigen::Vector6d v_des_RF = kp.cwiseProduct(f_RF - f_des_RF);
Eigen::Vector6d v_des_LF = kp.cwiseProduct(f_LF - f_des_LF);
m_v_RF_int += m_dt*ki.cwiseProduct(f_RF - f_des_RF);
m_v_LF_int += m_dt*ki.cwiseProduct(f_LF - f_des_LF);
m_robot->frameJacobianLocal(m_invDyn->data(), m_taskRF->frame_id(), m_J_RF);
m_robot->frameJacobianLocal(m_invDyn->data(), m_taskLF->frame_id(), m_J_LF);
m_J_RF_QR.compute(m_J_RF.rightCols(m_robot_util->m_nbJoints));
m_J_LF_QR.compute(m_J_LF.rightCols(m_robot_util->m_nbJoints));
Vector dq_adm_urdf = m_J_RF_QR.solve(v_des_RF+m_v_RF_int);
// SEND_MSG("J RF:\n"+toString(m_J_RF.rightCols(m_robot_util->m_nbJoints), 1), MSG_TYPE_DEBUG);
// SEND_MSG("v_des RF: "+toString(v_des_RF.transpose()), MSG_TYPE_DEBUG);
// SEND_MSG("dq_adm RF: "+toString(s.transpose()), MSG_TYPE_DEBUG);
dq_adm_urdf += m_J_LF_QR.solve(v_des_LF+m_v_LF_int);
// SEND_MSG("J LF:\n"+toString(m_J_LF.rightCols(m_robot_util->m_nbJoints), 1), MSG_TYPE_DEBUG);
// SEND_MSG("v_des LF: "+toString(v_des_LF.transpose()), MSG_TYPE_DEBUG);
// SEND_MSG("dq_adm LF: "+toString(s.transpose()), MSG_TYPE_DEBUG);
m_robot_util->joints_urdf_to_sot(dq_adm_urdf, s);
getProfiler().stop(PROFILE_DQ_ADMITTANCE);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(com_acc_des, dynamicgraph::Vector)
{
if(!m_initSucceeded)
......
Supports Markdown
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