Commit 19a19674 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[FootForceDifferenceController] Add damping

parent 4e15b25b
......@@ -67,6 +67,7 @@ namespace dynamicgraph {
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(dfzAdmittance, double);
DECLARE_SIGNAL_IN(vdcFrequency, double);
DECLARE_SIGNAL_IN(vdcDamping, double);
DECLARE_SIGNAL_IN(wrenchRightDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchLeftDes, dynamicgraph::Vector);
......
......@@ -32,7 +32,7 @@ namespace dynamicgraph
using namespace dg;
using namespace dg::command;
#define INPUT_SIGNALS m_dfzAdmittanceSIN << m_vdcFrequencySIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN
#define INPUT_SIGNALS m_dfzAdmittanceSIN << m_vdcFrequencySIN << m_vdcDampingSIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN
#define INNER_SIGNALS m_dz_ctrlSOUT << m_dz_posSOUT
......@@ -53,6 +53,7 @@ namespace dynamicgraph
: Entity(name)
, CONSTRUCT_SIGNAL_IN(dfzAdmittance, double)
, CONSTRUCT_SIGNAL_IN(vdcFrequency, double)
, CONSTRUCT_SIGNAL_IN(vdcDamping, double)
, CONSTRUCT_SIGNAL_IN(wrenchRightDes, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(wrenchLeftDes, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(wrenchRight, dynamicgraph::Vector)
......@@ -61,7 +62,7 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_IN(posLeftDes, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_IN(posRight, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_IN(posLeft, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_INNER(dz_ctrl, double, m_dfzAdmittanceSIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN)
, CONSTRUCT_SIGNAL_INNER(dz_ctrl, double, m_dfzAdmittanceSIN << m_vdcDampingSIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN << m_posRightSIN << m_posLeftSIN)
, CONSTRUCT_SIGNAL_INNER(dz_pos, double, m_vdcFrequencySIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN)
, CONSTRUCT_SIGNAL_OUT(vRight, dynamicgraph::Vector, m_dz_ctrlSINNER << m_dz_posSINNER)
, CONSTRUCT_SIGNAL_OUT(vLeft, dynamicgraph::Vector, m_dz_ctrlSINNER << m_dz_posSINNER)
......@@ -91,19 +92,26 @@ namespace dynamicgraph
}
const double & dfzAdmittance = m_dfzAdmittanceSIN(iter);
const double & vdcDamping = m_vdcDampingSIN(iter);
const Eigen::VectorXd & wrenchRightDes = m_wrenchRightDesSIN(iter);
const Eigen::VectorXd & wrenchLeftDes = m_wrenchLeftDesSIN(iter);
const Eigen::VectorXd & wrenchRight = m_wrenchRightSIN(iter);
const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSIN(iter);
const MatrixHomogeneous & posRight = m_posRightSIN(iter);
const MatrixHomogeneous & posLeft = m_posLeftSIN(iter);
const double RTz = posRight.translation()[2];
const double LTz = posLeft.translation()[2];
const double RFz_d = wrenchRightDes[2];
const double LFz_d = wrenchLeftDes[2];
const double RFz = wrenchRight[2];
const double LFz = wrenchLeft[2];
const double dz_ctrl = dfzAdmittance * ((LFz_d - RFz_d) - (LFz - RFz));
const double dz_ctrl = dfzAdmittance * ((LFz_d - RFz_d) - (LFz - RFz)) - vdcDamping * (RTz - LTz);
s = dz_ctrl;
......
......@@ -19,6 +19,7 @@ controller.wrenchRightDes.value = [0.]*6
controller.wrenchLeftDes.value = [0.]*6
controller.vdcFrequency.value = 0.
controller.vdcDamping.value = 0.
controller.posRightDes.value = np.eye(4).tolist()
controller.posLeftDes.value = np.eye(4).tolist()
......
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