Commit e9e4de7f authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[FootForceDifferenceController] Add phase input

parent bb88af2c
......@@ -65,6 +65,8 @@ namespace dynamicgraph {
void init();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(dfzAdmittance, double);
DECLARE_SIGNAL_IN(vdcFrequency, double);
DECLARE_SIGNAL_IN(vdcDamping, double);
......
......@@ -245,6 +245,7 @@ vdcDamping = 0.
controller = FootForceDifferenceController("footController")
controller.init()
controller.phase.value = 0
controller.dfzAdmittance.value = 0.
......
......@@ -32,7 +32,7 @@ namespace dynamicgraph
using namespace dg;
using namespace dg::command;
#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 INPUT_SIGNALS m_phaseSIN << 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
......@@ -51,6 +51,7 @@ namespace dynamicgraph
/* ------------------------------------------------------------------- */
FootForceDifferenceController::FootForceDifferenceController(const std::string& name)
: Entity(name)
, CONSTRUCT_SIGNAL_IN(phase, int)
, CONSTRUCT_SIGNAL_IN(dfzAdmittance, double)
, CONSTRUCT_SIGNAL_IN(vdcFrequency, double)
, CONSTRUCT_SIGNAL_IN(vdcDamping, double)
......@@ -64,8 +65,8 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_IN(posLeft, MatrixHomogeneous)
, 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)
, CONSTRUCT_SIGNAL_OUT(vRight, dynamicgraph::Vector, m_phaseSIN << m_dz_ctrlSINNER << m_dz_posSINNER)
, CONSTRUCT_SIGNAL_OUT(vLeft, dynamicgraph::Vector, m_phaseSIN << m_dz_ctrlSINNER << m_dz_posSINNER)
, m_initSucceeded(false)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
......@@ -158,9 +159,12 @@ namespace dynamicgraph
const double & dz_ctrl = m_dz_ctrlSINNER(iter);
const double & dz_pos = m_dz_posSINNER(iter);
const int & phase = m_phaseSIN(iter);
s.setZero(6);
s[2] = 0.5 * (dz_pos + dz_ctrl);
if(phase==0)
s[2] = 0.5 * (dz_pos + dz_ctrl);
return s;
}
......@@ -177,9 +181,12 @@ namespace dynamicgraph
const double & dz_ctrl = m_dz_ctrlSINNER(iter);
const double & dz_pos = m_dz_posSINNER(iter);
const int & phase = m_phaseSIN(iter);
s.setZero(6);
s[2] = 0.5 * (dz_pos - dz_ctrl);
if(phase==0)
s[2] = 0.5 * (dz_pos - dz_ctrl);
return s;
}
......
......@@ -9,20 +9,24 @@ controller = FootForceDifferenceController("footController")
controller.init()
controller.dfzAdmittance.value = 1.
controller.vdcFrequency.value = 0.
controller.vdcDamping.value = 0.
controller.wrenchRight.value = [0.]*2 + [500.] + [0.]*3
controller.wrenchLeft.value = [0.]*2 + [300.] + [0.]*3
controller.wrenchRightDes.value = [0.]*2 + [400.] + [0.]*3
controller.wrenchLeftDes.value = [0.]*2 + [400.] + [0.]*3
print("---- Input ----")
print("wrenchRight: %s" % str(controller.wrenchRight.value))
print("wrenchLeft: %s" % str(controller.wrenchLeft.value))
print("wrenchRightDes: %s" % str(controller.wrenchRightDes.value))
print("wrenchLeftDes: %s" % str(controller.wrenchLeftDes.value))
print()
controller.vdcFrequency.value = 0.
controller.vdcDamping.value = 0.
print("---- Double support ----")
controller.phase.value = 0
controller.posRightDes.value = np.eye(4).tolist()
controller.posLeftDes.value = np.eye(4).tolist()
......@@ -41,7 +45,20 @@ print("Expected vRight: %s" % str(vRight))
print("Actual vRight: %s" % str(controller.vRight.value))
print("Expected vLeft: %s" % str(vLeft))
print("Actual vLeft: %s" % str(controller.vLeft.value))
print()
assertApprox(vRight,controller.vRight.value)
assertApprox(vLeft,controller.vLeft.value)
print("---- Single support ----")
controller.phase.value = 1
controller.vRight.recompute(1)
controller.vLeft.recompute(1)
print("vRight: %s" % str(controller.vRight.value))
print("vLeft: %s" % str(controller.vLeft.value))
print()
assertApprox([0.]*6,controller.vRight.value)
assertApprox([0.]*6,controller.vLeft.value)
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