def __init__(self, feetFollowerWithCorrection, robot, corba = None): ErrorEstimationStrategy.__init__(self, robot, feetFollowerWithCorrection) self.errorEstimator = ErrorMerger('error_merger') self.feetFollowerWithCorrection = feetFollowerWithCorrection self.robot = robot
class MotionPlanErrorEstimationStrategy(ErrorEstimationStrategy): errorEstimatorId = 0 localizationPlannedBody = 'waist' errorEstimators = [] # reference toward the motion plan, added after this object # construction and before starting the error estimation. motionPlan = None def __init__(self, feetFollowerWithCorrection, robot, corba = None): ErrorEstimationStrategy.__init__(self, robot, feetFollowerWithCorrection) self.errorEstimator = ErrorMerger('error_merger') self.feetFollowerWithCorrection = feetFollowerWithCorrection self.robot = robot def start(self, interactive = False): for control in self.motionPlan.control: name = 'error_estimator' + \ str(MotionPlanErrorEstimationStrategy.errorEstimatorId) MotionPlanErrorEstimationStrategy.errorEstimatorId += 1 self.errorEstimator.addErrorEstimation(name) estimator = None if interactive: estimator = control.interactiveStart(name, self.feetFollowerWithCorrection) else: estimator = control.start(name, self.feetFollowerWithCorrection) if type(estimator) == ErrorEstimator: plug(estimator.error, self.errorEstimator.signal("error_" + name)) self.errorEstimators.append(estimator) else: # If this is not an error estimator, we suppose it is a constant # value that can be used to set the signal. self.errorEstimator.signal("error_" + name).value = estimator self.errorEstimator.signal("weight_" + name).value = \ (control.weight,) if self.motionPlan.trace and type(estimator) == ErrorEstimator: addTrace(self.motionPlan.robot, self.motionPlan.trace, name, 'error') addTrace(self.motionPlan.robot, self.motionPlan.trace, name, 'dbgPositionWorldFrame') addTrace(self.motionPlan.robot, self.motionPlan.trace, name, 'dbgPlanned') addTrace(self.motionPlan.robot, self.motionPlan.trace, name, 'dbgIndex') if self.motionPlan.trace: addTrace(self.motionPlan.robot, self.motionPlan.trace, self.errorEstimator.name, 'error') return True def interactiveStart(self): return self.start(interactive = True) def __str__(self): return "motion plan error estimation strategy"