def __init__(self, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10):
"""
kp = proportional gain term
ki = integral gain term
kd = derivative gain term
kff= feed forward velocity term. Applies signal relative to dervative of the target
derivative_corner = corner frequency of derivative filter.
Our real world signals are too noisy to use without significant filtering
kfa= feed forward acceleration term. Applies signal relative to the difference in
rate of change of the target and desired.
"""
self.updateGainConstants(kp, ki, kd, kff, kfa)
self.max_movement_rate = 1e6
# NOTE: it is important that these three variables are floating point to avoid truncation
self.prev_error = 0.0
self.prev_desired_pos = 0.0
self.integral_error_accumulator = 0.0
self.peak_detector = HystereticPeakDetector(0.0, -1.0, 1.0, math.pi / 20)
self.d_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner)
class PIDController:
def __init__(self, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10):
"""
kp = proportional gain term
ki = integral gain term
kd = derivative gain term
kff= feed forward velocity term. Applies signal relative to dervative of the target
derivative_corner = corner frequency of derivative filter.
Our real world signals are too noisy to use without significant filtering
kfa= feed forward acceleration term. Applies signal relative to the difference in
rate of change of the target and desired.
"""
self.updateGainConstants(kp, ki, kd, kff, kfa)
self.max_movement_rate = 1e6
# NOTE: it is important that these three variables are floating point to avoid truncation
self.prev_error = 0.0
self.prev_desired_pos = 0.0
self.integral_error_accumulator = 0.0
self.peak_detector = HystereticPeakDetector(0.0, -1.0, 1.0, math.pi / 20)
self.d_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner)
def updateGainConstants(self, kp, ki, kd, kff=None, kfa=None):
self.kp = kp
self.ki = ki
self.kd = kd
if kff is not None:
self.kff = kff
else:
self.kff = 0.0
if kfa is not None:
self.kfa = kfa
else:
self.kfa = 0.0
def update(self, desired_pos, measured_pos):
delta_time = time_sources.global_time.getDelta()
# bound the desired position
#desired_pos = self.boundDesiredPosition(desired_pos)
desired_vel = (desired_pos - self.prev_desired_pos) / delta_time
error = desired_pos - measured_pos
self.peak_detector.update(error)
if 0:
if self.peak_detector.hasConverged():
if self.peak_detector.isUnstable():
warningstring = ("LimbController: Maximum error for the" +
" desired point has increased for %d seconds," +
" but is within converged range. Might be unstable." %
self.peak_detector.getResolveTime())
#logger.warning(warningstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
elif self.peak_detector.isLimitCycle():
warningstring = ("LimbController: Maximum error for the" +
" desired point has increased once or more for %d seconds," +
" but is within converged range. Might be unstable." %
self.peak_detector.getResolveTime())
#logger.warning(warningstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
else:
if self.peak_detector.isUnstable():
errorstring = ("LimbController: Maximum error for the desired point" +
"has increased for %d seconds. System potentially unstable." %
self.peak_detector.getResolveTime())
#logger.error(errorstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
raise ValueError(errorstring)
elif self.peak_detector.isLimitCycle():
errorstring = ("LimbController: Controller has not converged" +
"over %d seconds. System potentially in a limit cycle." %
self.peak_detector.getResolveTime())
#logger.error(errorstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
raise ValueError(errorstring)
self.integral_error_accumulator += self.ki * error * delta_time
derivative_error = self.d_lowpass.update((error - self.prev_error) / delta_time)
velocity_error = desired_vel - derivative_error
self.prev_error = error
self.prev_desired_pos = desired_pos
actuator_command = (self.kp * error +
self.integral_error_accumulator +
self.kd * derivative_error +
self.kff * desired_vel +
self.kfa * velocity_error)
#actuator_command = self.boundActuatorCommand(actuator_command, measured_pos)
return actuator_command
def isErrorInBounds(self, error, measured_pos):
"""tests whether or not the error signal is within reasonable range
not checking for NaN, since both desired and measured position
are tested for that
"""
#makes sure the error is bounded by a single leg rotation
error = error % (2 * math.pi)
error_min = -math.pi / 2
error_max = math.pi / 2
#is error within available soft range?
if error > (measured_pos - error_min) or error > (error_max - measured_pos):
#logger.error("LimbController.isErrorInBounds: error out of soft bounds.",
# error=error,
# measured_pos=measured_pos)
raise ValueError("Error signal points to a position out of soft bounds.")
return error
def boundDesiredPosition(self, desired_pos):
#caps desired position to soft movement range
if math.isnan(desired_pos):
#logger.error("LimbController.boundDesiredPosition: NaN where aN expected!",
# desired_pos=desired_pos,
# bad_value="desired_pos")
raise ValueError("LimbController: desired_pos cannot be NaN.")
command_min = -20
command_max = 20
if desired_pos < command_min or desired_pos > command_max:
#logger.error("LimbController.boundDesiredPosition:"+
# " desired position out of bounds!",
# desired_pos=desired_pos,
# command_min=command_min,
# command_max=command_max,
# bad_value="desired_pos")
raise ValueError("LimbController.boundDesiredPosition:" +
" desired position out of soft bounds")
bounded_pos = saturate(desired_pos, command_min, command_max)
return bounded_pos
def boundActuatorCommand(self, actuator_command, measured_pos):
#prevent the controller from commanding an unsafely fast actuator move
if (abs(actuator_command - measured_pos) / time_sources.global_time.getDelta() > self.max_movement_rate):
raise ValueError("LimbController: Actuator command would cause" +
"joint to move at an unsafe rate.")
return actuator_command
