Commit 93815b58 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[foot-force-difference-controller] Gain management

parent 836cea87
......@@ -67,6 +67,10 @@ namespace dynamicgraph {
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(gainSwing, double);
DECLARE_SIGNAL_IN(gainStance, double);
DECLARE_SIGNAL_IN(gainDouble, double);
DECLARE_SIGNAL_IN(dfzAdmittance, double);
DECLARE_SIGNAL_IN(vdcFrequency, double);
DECLARE_SIGNAL_IN(vdcDamping, double);
......@@ -87,6 +91,9 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(vRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(vLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(gainRight, double);
DECLARE_SIGNAL_OUT(gainLeft, double);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
......
......@@ -32,11 +32,11 @@ namespace dynamicgraph
using namespace dg;
using namespace dg::command;
#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 INPUT_SIGNALS m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN << 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
#define OUTPUT_SIGNALS m_vRightSOUT << m_vLeftSOUT
#define OUTPUT_SIGNALS m_vRightSOUT << m_vLeftSOUT << m_gainRightSOUT << m_gainLeftSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
......@@ -52,6 +52,9 @@ namespace dynamicgraph
FootForceDifferenceController::FootForceDifferenceController(const std::string& name)
: Entity(name)
, CONSTRUCT_SIGNAL_IN(phase, int)
, CONSTRUCT_SIGNAL_IN(gainSwing, double)
, CONSTRUCT_SIGNAL_IN(gainStance, double)
, CONSTRUCT_SIGNAL_IN(gainDouble, double)
, CONSTRUCT_SIGNAL_IN(dfzAdmittance, double)
, CONSTRUCT_SIGNAL_IN(vdcFrequency, double)
, CONSTRUCT_SIGNAL_IN(vdcDamping, double)
......@@ -67,6 +70,8 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_INNER(dz_pos, double, m_vdcFrequencySIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN)
, 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)
, CONSTRUCT_SIGNAL_OUT(gainRight, double, m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN)
, CONSTRUCT_SIGNAL_OUT(gainLeft, double, m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN)
, m_initSucceeded(false)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
......@@ -191,6 +196,52 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(gainRight, double)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Can't compute gainRight before initialization!");
return s;
}
const int & phase = m_phaseSIN(iter);
const double & gainSwing = m_gainSwingSIN(iter);
const double & gainStance = m_gainStanceSIN(iter);
const double & gainDouble = m_gainDoubleSIN(iter);
if(phase>0)
s = gainSwing;
else if (phase<0)
s = gainStance;
else
s = gainDouble;
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(gainLeft, double)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Can't compute gainLeft before initialization!");
return s;
}
const int & phase = m_phaseSIN(iter);
const double & gainSwing = m_gainSwingSIN(iter);
const double & gainStance = m_gainStanceSIN(iter);
const double & gainDouble = m_gainDoubleSIN(iter);
if(phase>0)
s = gainStance;
else if (phase<0)
s = gainSwing;
else
s = gainDouble;
return s;
}
/* --- COMMANDS ---------------------------------------------------------- */
/* ------------------------------------------------------------------- */
......
......@@ -12,11 +12,19 @@ controller.dfzAdmittance.value = 1.
controller.vdcFrequency.value = 0.
controller.vdcDamping.value = 0.
gainSwing = 1.
gainStance = 2.
gainDouble = 3.
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
controller.gainSwing.value = gainSwing
controller.gainStance.value = gainStance
controller.gainDouble.value = gainDouble
print("---- Input ----")
print("wrenchRight: %s" % str(controller.wrenchRight.value))
print("wrenchLeft: %s" % str(controller.wrenchLeft.value))
......@@ -24,6 +32,11 @@ print("wrenchRightDes: %s" % str(controller.wrenchRightDes.value))
print("wrenchLeftDes: %s" % str(controller.wrenchLeftDes.value))
print()
print("gainSwing: %s" % str(controller.gainSwing.value))
print("gainStance: %s" % str(controller.gainStance.value))
print("gainDouble: %s" % str(controller.gainDouble.value))
print()
print("---- Double support ----")
controller.phase.value = 0
......@@ -35,6 +48,8 @@ controller.posLeft.value = np.eye(4).tolist()
controller.vRight.recompute(0)
controller.vLeft.recompute(0)
controller.gainRight.recompute(0)
controller.gainLeft.recompute(0)
# There is more pressure on the right foot.
# Therefore, the right foot must go up to reduce it
......@@ -50,11 +65,20 @@ print()
assertApprox(vRight,controller.vRight.value)
assertApprox(vLeft,controller.vLeft.value)
print("---- Single support ----")
print("gainRight: %s" % str(controller.gainRight.value))
print("gainLeft: %s" % str(controller.gainLeft.value))
print()
assertApprox(gainDouble,controller.gainRight.value)
assertApprox(gainDouble,controller.gainLeft.value)
print("---- Left support ----")
controller.phase.value = 1
controller.vRight.recompute(1)
controller.vLeft.recompute(1)
controller.gainRight.recompute(1)
controller.gainLeft.recompute(1)
print("vRight: %s" % str(controller.vRight.value))
print("vLeft: %s" % str(controller.vLeft.value))
print()
......@@ -62,3 +86,31 @@ print()
assertApprox([0.]*6,controller.vRight.value)
assertApprox([0.]*6,controller.vLeft.value)
print("gainRight: %s" % str(controller.gainRight.value))
print("gainLeft: %s" % str(controller.gainLeft.value))
print()
assertApprox(gainSwing,controller.gainRight.value)
assertApprox(gainStance,controller.gainLeft.value)
print("---- Right support ----")
controller.phase.value = -1
controller.vRight.recompute(2)
controller.vLeft.recompute(2)
controller.gainRight.recompute(2)
controller.gainLeft.recompute(2)
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)
print("gainRight: %s" % str(controller.gainRight.value))
print("gainLeft: %s" % str(controller.gainLeft.value))
print()
assertApprox(gainStance,controller.gainRight.value)
assertApprox(gainSwing,controller.gainLeft.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