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