def __init__(self, robot, solver,
                 feetFollowerGraph,
                 errorEstimationStrategyType,
                 maxX = 0.04,
                 maxY = 0.04,
                 maxTheta = 0.1):
        """
        To setup a correction, an initial feet follower graph
        is required.
        """

        self.feetFollowerGraph = feetFollowerGraph

        # Fill inherited attributes.
        self.feetFollower = feetFollowerGraph.feetFollower

        self.postureTask = feetFollowerGraph.postureTask
        self.postureFeature = feetFollowerGraph.postureFeature
        self.trace = feetFollowerGraph.trace

        self.solver = solver
        self.robot = robot


        # Fill local attributes.
        self.maxX = maxX
        self.maxY = maxY
        self.maxTheta = maxTheta
        self.onRobot = type(self.robot.device) != RobotSimu

        # Store the reference trajectory (uncorrected).
        self.referenceTrajectory = self.feetFollower

        # Create the correction entity.
        self.feetFollower = FeetFollowerWithCorrection('correction')

        # Set the reference trajectory.
        self.feetFollower.setReferenceTrajectory(self.referenceTrajectory.name)

        # Set the safety limits.
        self.feetFollower.setSafetyLimits(self.maxX, self.maxY, self.maxTheta)

        # Replug.
        plug(self.feetFollower.zmp, self.robot.device.zmp)
        plug(self.feetFollower.com, self.robot.featureComDes.errorIN)
        plug(self.feetFollower.signal('left-ankle'),
             self.robot.features['left-ankle'].reference)
        plug(self.feetFollower.signal('right-ankle'),
             self.robot.features['right-ankle'].reference)
        plug(self.feetFollower.signal('waistYaw'),
             self.robot.features['waist'].reference)

        # Replug velocities.
        plug(self.feetFollower.comVelocity, self.robot.comTask.errorDot)
        for op in ['left-ankle', 'right-ankle']:
            plug(self.feetFollower.signal(op + 'Velocity'),
                 self.robot.tasks[op].errorDot)
        plug(self.feetFollower.waistYawVelocity,
             self.robot.tasks['waist'].errorDot)

        # Setup error estimation strategy.
        self.errorEstimationStrategy = errorEstimationStrategyType(self, robot)

        plug(self.robot.dynamic.signal(
                self.errorEstimationStrategy.localizationPlannedBody),
             self.feetFollower.position)

        # Plug the estimated error into the correction entity.
        plug(self.errorEstimationStrategy.errorEstimator.error,
             self.feetFollower.offset)
class FeetFollowerGraphWithCorrection(FeetFollowerGraph):
    """
    Enable online rectification of an existing FeetFollowerGraph using
    robot localization.

    This class is a decorator over a FeetFollowerGraph.


    It also takes as input an error estimation strategy. By default,
    it uses the motion capture system to estimate the position error.
    """

    """
    Uncorrected graph
    """
    feetFollowerGraph = None

    """
    Initial, uncorrected trajectory.
    """
    referenceTrajectory = None

    """
    Define the maximum correction.

    This has been empirically validated on HRP-2 14.

    This stores the maximum distance between the original and
    corrected footsteps (x, y in meters and rotation theta in rad).
    """
    (maxX, maxY, maxTheta) = (0.04, 0.04, 0.1)

    """
    Are we in simulation or in OpenHRP?
    """
    onRobot = None

    """
    Strategy used to compute the position error.

    This must be an instance of a subclass of ErrorEstimationStrategy.
    """
    errorEstimationStrategy = None

    def __str__(self):
        msg = \
            "* Is this the robot? "  + str(self.onRobot) + "\n" \
            "* Error estimation strategy: " + \
            str(self.errorEstimationStrategy) + "\n" + \
            "Safe limits: %f %f %f\n" % (self.maxX, self.maxY, self.maxTheta) \
            + "\n"
        if (self.onRobot):
            msg += "Please type 'f.start()' to start walking.\n"
        return msg

    def __init__(self, robot, solver,
                 feetFollowerGraph,
                 errorEstimationStrategyType,
                 maxX = 0.04,
                 maxY = 0.04,
                 maxTheta = 0.1):
        """
        To setup a correction, an initial feet follower graph
        is required.
        """

        self.feetFollowerGraph = feetFollowerGraph

        # Fill inherited attributes.
        self.feetFollower = feetFollowerGraph.feetFollower

        self.postureTask = feetFollowerGraph.postureTask
        self.postureFeature = feetFollowerGraph.postureFeature
        self.trace = feetFollowerGraph.trace

        self.solver = solver
        self.robot = robot


        # Fill local attributes.
        self.maxX = maxX
        self.maxY = maxY
        self.maxTheta = maxTheta
        self.onRobot = type(self.robot.device) != RobotSimu

        # Store the reference trajectory (uncorrected).
        self.referenceTrajectory = self.feetFollower

        # Create the correction entity.
        self.feetFollower = FeetFollowerWithCorrection('correction')

        # Set the reference trajectory.
        self.feetFollower.setReferenceTrajectory(self.referenceTrajectory.name)

        # Set the safety limits.
        self.feetFollower.setSafetyLimits(self.maxX, self.maxY, self.maxTheta)

        # Replug.
        plug(self.feetFollower.zmp, self.robot.device.zmp)
        plug(self.feetFollower.com, self.robot.featureComDes.errorIN)
        plug(self.feetFollower.signal('left-ankle'),
             self.robot.features['left-ankle'].reference)
        plug(self.feetFollower.signal('right-ankle'),
             self.robot.features['right-ankle'].reference)
        plug(self.feetFollower.signal('waistYaw'),
             self.robot.features['waist'].reference)

        # Replug velocities.
        plug(self.feetFollower.comVelocity, self.robot.comTask.errorDot)
        for op in ['left-ankle', 'right-ankle']:
            plug(self.feetFollower.signal(op + 'Velocity'),
                 self.robot.tasks[op].errorDot)
        plug(self.feetFollower.waistYawVelocity,
             self.robot.tasks['waist'].errorDot)

        # Setup error estimation strategy.
        self.errorEstimationStrategy = errorEstimationStrategyType(self, robot)

        plug(self.robot.dynamic.signal(
                self.errorEstimationStrategy.localizationPlannedBody),
             self.feetFollower.position)

        # Plug the estimated error into the correction entity.
        plug(self.errorEstimationStrategy.errorEstimator.error,
             self.feetFollower.offset)

    def start(self, beforeStart = None):
        if self.onRobot:
            start = self.errorEstimationStrategy.start
        else:
            start = self.errorEstimationStrategy.interactiveStart

        if start():
            self.robot.comTask.controlGain.value = self.gain
            self.robot.tasks['left-ankle'].controlGain.value = self.gain
            self.robot.tasks['right-ankle'].controlGain.value = self.gain
            self.feetFollowerGraph.postureTask.controlGain.value = self.gain
            self.robot.tasks['waist'].controlGain.value = self.gain
            self.setupTrace()
            if beforeStart:
                beforeStart()
            self.feetFollowerGraph.trace.start()
            self.feetFollower.start()
        else:
            print("failed to start")

    def setupTrace(self):
        self.feetFollowerGraph.setupTrace()
        for s in self.tracedSignals['FeetFollower']:
            addTrace(self.robot, self.trace, self.referenceTrajectory.name, s)
        for s in self.tracedSignals['FeetFollower'] + ['offset']:
            addTrace(self.robot, self.trace, self.feetFollower.name, s)
        for s in ['error']:
            addTrace(self.robot, self.trace,
                     self.errorEstimationStrategy.errorEstimator.name, s)