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 update(self, signal):
"""
Calculate and return response.
"""
# If this is the first signal, update signal and response history,
# then return the signal as the response
if not hasattr(self, 'last_response'):
self.last_signal = signal
self.last_response = signal
return signal
a, b, c = self.a, self.b, self.c # Filter constants
time_delta = global_time.getDelta()
# First order filter implementation
out = (self.last_response + (a + b * time_delta) * signal -
a * self.last_signal) / (1.0 + c * time_delta)
# Update history
self.last_response = out
self.last_signal = signal
return out
def update(self, signal):
"""
Calculate and return response.
"""
# If this is the first signal, update signal and response history,
# then return the signal as the response
if not hasattr(self, 'last_response'):
self.last_signal = signal
self.last_response = signal
return signal
a, b, c = self.a, self.b, self.c # Filter constants
time_delta = global_time.getDelta()
# First order filter implementation
out = (self.last_response + \
(a + b * time_delta) * signal - \
a * self.last_signal) / (1.0 + c * time_delta)
# Update history
self.last_response = out
self.last_signal = signal
return out
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 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
