def __init__(self,robot,taskname = 'com-stabilized'):
        
        from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_free_flex_estimator import HRP2ModelFreeFlexEstimator

        HRP2LQRDecoupledStabilizer.__init__(self,taskname)
        robot.dynamic.com.recompute(0)
        robot.dynamic.Jcom.recompute(0)
        
        plug (robot.dynamic.com, self.com)
        plug (robot.dynamic.Jcom, self.Jcom)
        plug (robot.device.forceLLEG,self.force_lf)
        plug (robot.device.forceRLEG,self.force_rf)
        plug (robot.frames['rightFootForceSensor'].position,self.rightFootPosition)
        plug (robot.frames['leftFootForceSensor'].position,self.leftFootPosition)
        #TODO ///// SELEC ??

        self.estimator = HRP2ModelFreeFlexEstimator(robot, taskname+"Estimator")
        plug (self.nbSupport,self.estimator.contactNbr)
        
        plug(self.supportPos1,self.estimator.contact1)
        plug(self.supportPos2,self.estimator.contact2)
        
        plug(self.estimator.flexTransformationMatrix, self.stateFlex )        
        plug(self.estimator.flexOmega, self.stateFlexDot )
        plug(self.estimator.flexOmegaDot, self.stateFlexDDot )
    def __init__(self,robot,taskname = 'com-stabilized'):
        
        from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator import HRP2ModelBaseFlexEstimator

        HRP2LQRDecoupledStabilizer.__init__(self,taskname)
        robot.dynamic.com.recompute(0)
        robot.dynamic.Jcom.recompute(0)
        
        plug (robot.dynamic.com, self.com)
        plug (robot.dynamic.Jcom, self.Jcom)
        plug (robot.device.forceLLEG,self.force_lf)
        plug (robot.device.forceRLEG,self.force_rf)
        plug (robot.frames['rightFootForceSensor'].position,self.rightFootPosition)
        plug (robot.frames['leftFootForceSensor'].position,self.leftFootPosition)
        #TODO ///// SELEC ??

        self.estimator = HRP2ModelBaseFlexEstimator(robot, taskname+"Estimator")
        plug (self.nbSupport,self.estimator.contactNbr)
        self.estimator.setContactModel(1)


        self.contacts = Stack_of_vector (taskname+'contacts')

        plug(self.supportPos1,self.contacts.sin1)
        plug(self.supportPos2,self.contacts.sin2)
        
        self.contacts.selec1 (0, 6)
        self.contacts.selec2 (0, 6)
        plug(self.contacts.sout, self.estimator.inputVector.contactsPosition)
        
        plug(self.estimator.flexTransformationMatrix, self.stateFlex )        
        plug(self.estimator.flexOmega, self.stateFlexDot )
        plug(self.estimator.flexOmegaDot, self.stateFlexDDot )

        self.comVectorPV = Stack_of_vector (taskname+'ComVectorPV')
        self.comVector = Stack_of_vector (taskname+'ComVector')
        
        self.comVectorPV.selec1 (0, 3)
        self.comVectorPV.selec2 (0, 3)        
        self.comVector.selec1 (0, 6)
        self.comVector.selec2 (0, 3)        
        

        plug(robot.dynamic.com,     self.comVectorPV.sin1)
        plug(self.comdot,           self.comVectorPV.sin2)
        plug(self.comVectorPV.sout, self.comVector.sin1)
        plug(self.comddot,          self.comVector.sin2)