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)