def update(self, measured_pos_array, desired_pos_array):
if hasattr(self, 'desired_pos_array'):
# update target velocities
for i in range(3):
self.desired_vel_array[i] = (
desired_pos_array[i] -
self.desired_pos_array[i]) / global_time.getDelta()
else:
self.desired_vel_array = array([0, 0, 0])
self.desired_pos_array = desired_pos_array
actuator_commands = []
if not (len(self.desired_pos_array) == self.amount_of_joints ==
len(measured_pos_array)):
logger.error(
"LimbController.update: position array sizes mismatched!",
measured_pos_array=measured_pos_array,
desired_pos_array=self.desired_pos_array)
raise ValueError(
"LimbController.update: position array sizes mismatched!")
for i in range(len(self.pid_controllers)):
actuator_command = self.pid_controllers[i].update(
self.desired_pos_array[i], measured_pos_array[i])
# JWHONG COMPENSATION FOR EXTEND VS RETRACT SURFACE AREA HACK
# JWHONG DEADBAND HACK
if actuator_command > 0:
actuator_command += self.dearray[i]
elif actuator_command < 0:
actuator_command *= self.proparray[i]
actuator_command -= self.drarray[i]
actuator_commands.append(actuator_command)
self.length_rate_commands = actuator_commands
def setSensorReadings(self, yaw, hip_pitch, knee_pitch, shock_depth):
self.joint_angles = array([yaw, hip_pitch, knee_pitch])
#throw errors if measured joint angles are NaN, out of bounds, etc
for angle, soft_min, soft_max in zip(self.joint_angles, self.SOFT_MINS, self.SOFT_MAXES):
if math.isnan(angle):
logger.error("LegModel.setSensorReadings: NaN where aN expected!",
angle=angle,
angle_index=self.joint_angles.searchsorted(angle),
yaw=yaw,
hip_pitch=hip_pitch,
knee_pitch=knee_pitch,
shock_depth=shock_depth,
bad_value="angle")
raise ValueError("LegModel: Measured angle cannot be NaN.")
if (soft_min > angle) or (angle > soft_max):
logger.error("LegModel: Measured position outside of soft range!",
angle=angle,
angle_index=self.joint_angles.searchsorted(angle),
yaw=yaw,
hip_pitch=hip_pitch,
knee_pitch=knee_pitch,
shock_depth=shock_depth,
soft_min=soft_min,
soft_max=soft_max,
bad_value="angle")
print soft_min
print soft_max
print angle
raise ValueError("LegModel: Measured angle out of soft range!")
self.shock_depth = shock_depth
self.joint_velocities = self.jv_filter.update(self.joint_angles)
def update(self, measured_pos_array, desired_pos_array):
if hasattr(self, 'desired_pos_array'):
# update target velocities
for i in range(3):
self.desired_vel_array[i] = (desired_pos_array[i] - self.desired_pos_array[i]) / global_time.getDelta()
else:
self.desired_vel_array = array([0, 0, 0])
self.desired_pos_array = desired_pos_array
actuator_commands = []
if not (len(self.desired_pos_array) == self.amount_of_joints == len(measured_pos_array)):
logger.error("LimbController.update: position array sizes mismatched!",
measured_pos_array=measured_pos_array,
desired_pos_array=self.desired_pos_array)
raise ValueError("LimbController.update: position array sizes mismatched!")
for i in range(len(self.pid_controllers)):
actuator_command = self.pid_controllers[i].update(self.desired_pos_array[i],
measured_pos_array[i])
# JWHONG COMPENSATION FOR EXTEND VS RETRACT SURFACE AREA HACK
# JWHONG DEADBAND HACK
if actuator_command > 0:
actuator_command += self.dearray[i]
elif actuator_command < 0:
actuator_command *= self.proparray[i]
actuator_command -= self.drarray[i]
actuator_commands.append(actuator_command)
self.length_rate_commands = actuator_commands
def updateDelta(self, delta):
if delta <= 0.0 and self.time != 0.0:
logger.error("TimeSource.updateDelta: Reversed time error!",
initial_time=self.initial_time,
initial_delta=self.initial_delta,
current_time=self.time,
bad_value=delta)
raise ValueError("TimeSource.updateDelta(): time must be increasing")
self.delta = delta
self.time += delta
def updateTime(self, time):
if time <= self.time and self.time != 0.0:
logger.error("TimeSource.updateTime: Reversed time error!",
initial_time=self.initial_time,
initial_delta=self.initial_delta,
current_time=self.time,
bad_value=time)
raise ValueError("TimeSource.updateTime(): time must be increasing.\nLast time: %f\nNew time: %d"%(self.time,time))
self.delta = time - self.time
self.time = time
def attributeChanged(self, item):
if not self.isFillAttributeTree and self.selected_item:
try:
# check value chaned
if item.oldValue == item.text(1):
return
item.oldValue = item.text(1)
index = item.index
# check array type, then combine components
parent = item.parent()
if type(parent
) == QtGui.QTreeWidgetItem and parent.dataType in (
tuple, list, numpy.ndarray):
attributeName = parent.text(0)
value = []
for i in range(parent.childCount()):
child = parent.child(i)
value.append(child.dataType(child.text(1)))
# numpy array
if parent.dataType == numpy.ndarray:
value = numpy.array(value)
# list or tuple
else:
value = parent.dataType(value)
else:
attributeName = item.text(0)
if item.dataType == bool:
value = item.dataType(item.text(1) == "True")
else:
value = item.dataType(item.text(1))
selectedItems = []
command = None
if self.selected_item_categoty == 'Object':
command = COMMAND.SET_OBJECT_ATTRIBUTE
selectedItems = self.objectList.selectedItems()
elif self.selected_item_categoty == 'Resource':
command = COMMAND.SET_RESOURCE_ATTRIBUTE
selectedItems = self.resourceListWidget.selectedItems()
for selectedItem in selectedItems:
selected_item_name = selectedItem.text(0)
selected_item_type = selectedItem.text(1)
# send changed data
self.appCmdQueue.put(
command, (selected_item_name, selected_item_type,
attributeName, value, index))
except:
logger.error(traceback.format_exc())
# failed to convert string to dataType, so restore to old value
item.setText(1, item.oldValue)
def updateGainConstants(self, kparray, kiarray, kdarray, kffarray, kfaarray):
self.kp = kparray
self.ki = kiarray
self.kd = kdarray
self.kff = kffarray
self.kfa = kfaarray
if (len(self.kp) != len(self.ki) or len(self.ki) != len(self.kd)):
logger.error("LimbController.init: Gain array sizes mismatched!",
kparray=self.kp,
kiarray=self.ki,
kdarray=self.kd)
raise ValueError("LimbController.init: Gain array sizes mismatched!")
for controller, kp, ki, kd, kff, kfa in zip(self.pid_controllers, self.kp, self.ki, self.kd, self.kff, self.kfa):
controller.updateGainConstants(kp, ki, kd, kff, kfa)
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 update(self, measured_pos_array, desired_pos_array):
if hasattr(self, "desired_pos_array"):
# update target velocities
for i in range(3):
self.desired_vel_array[i] = (desired_pos_array[i] - self.desired_pos_array[i]) / global_time.getDelta()
else:
self.desired_vel_array = array([0, 0, 0])
self.desired_pos_array = desired_pos_array
actuator_commands = []
if len(self.desired_pos_array) != self.amount_of_joints or len(measured_pos_array) != self.amount_of_joints:
logger.error(
"LimbController.update: position array sizes mismatched!",
measured_pos_array=measured_pos_array,
desired_pos_array=self.desired_pos_array,
)
raise ValueError("LimbController.update:" + " position array sizes mismatched!")
for i in range(len(self.pid_controllers)):
actuator_command = self.pid_controllers[i].update(self.desired_pos_array[i], measured_pos_array[i])
actuator_commands.append(actuator_command)
self.length_rate_commands = actuator_commands
def updateGainConstants(self, kparray, kiarray, kdarray, kffarray,
kfaarray):
self.kp = kparray
self.ki = kiarray
self.kd = kdarray
self.kff = kffarray
self.kfa = kfaarray
if (len(self.kp) != len(self.ki) or len(self.ki) != len(self.kd)):
logger.error("LimbController.init: Gain array sizes mismatched!",
kparray=self.kp,
kiarray=self.ki,
kdarray=self.kd)
raise ValueError(
"LimbController.init: Gain array sizes mismatched!")
for controller, kp, ki, kd, kff, kfa in zip(self.pid_controllers,
self.kp, self.ki, self.kd,
self.kff, self.kfa):
controller.updateGainConstants(kp, ki, kd, kff, kfa)
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 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 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
