def test_apply_noise_filter(): """ Tests 4.3 Filter library Tests 4.3.2 Noise filtering """ data = DataCapture() low_noise_filter = LowPassFilter("Chebyshev", 100) high_noise_filter = HighPassFilter("Chebyshev", 5000) low_noise = [] # random <100HZ noise high_noise = [] # random >5000HZ noise old_data = data.dataCapture() low_filtered_data = data.applyFilter(low_noise_filter, 2) high_filtered_data = data.applyFilter(high_noise_filter, 2) errors = [] #TODO # replace == with a in comparison to check if the noise is in the data if low_noise == low_filtered_data: errors.append("Low noise detected in filtered data") if high_noise == high_filtered_data: errors.append("High noise detected in filtered data") # NOTE: currently returns PASSED as the two data types are both "NONE" and python considers them equal assert not errors, "Assert errors occured:\n{}".format("\n".join(errors))
def __init__(self, config_file="leg_model.conf", section="LegModel"): c = ConfigParser() if not path.exists(path.abspath(config_file)): print 'Config file %s not found!' % config_file raise IOError c.read(config_file) # Link lengths self.YAW_LEN = c.getfloat(section, "yaw_len") self.THIGH_LEN = c.getfloat(section, "thigh_len") self.CALF_LEN = c.getfloat(section, "calf_len") # Actuator soft bounds soft_stops_section = 'SoftStops' self.SOFT_MINS = array([ c.getfloat(soft_stops_section, 'yaw_stop_low'), c.getfloat(soft_stops_section, 'pitch_stop_low'), c.getfloat(soft_stops_section, 'knee_stop_low') ]) self.SOFT_MAXES = array([ c.getfloat(soft_stops_section, 'yaw_stop_high'), c.getfloat(soft_stops_section, 'pitch_stop_high'), c.getfloat(soft_stops_section, 'knee_stop_high') ]) # State vel_corner = 100.0 # rad/s self.jv_filter = HighPassFilter( vel_corner, vel_corner) # band limited differentiator self.setSensorReadings(self.SOFT_MINS[0], self.SOFT_MINS[1], self.SOFT_MINS[2], 0.0) # Events self.SHOCK_DEPTH_THRESHOLD_LOW = c.getfloat( section, "shock_depth_threshold_low") self.SHOCK_DEPTH_THRESHOLD_HIGH = c.getfloat( section, "shock_depth_threshold_high") self.foot_on_ground = False
def test_check_avail_filters(): """ Tests 4.3 Filter library Tests 4.3.3 Data filtering """ data = DataCapture() lpf = LowPassFilter("Chebyshev", 250) hpf = HighPassFilter("Bessel", 500) maf = MovingAverageFilter(10) pkf = PeakFilter(10, 10) errors = [] # replace == with a check if noise data is contained in filtered data if not lpf.getFilterInfo(): errors.append("Low pass filter does not exist") if not hpf.getFilterInfo(): errors.append("High pass filter does not exist") if not maf.getFilterInfo(): errors.append("Moving average filter does not exist") if not pkf.getFilterInfo(): errors.append("Peak filter does not exist") assert not errors, "Assert errors occured:\n{}".format("\n".join(errors))
def __init__(self, config_file="leg_model.conf", section="LegModel"): c = ConfigParser() if not path.exists(path.abspath(config_file)): print 'Config file %s not found!'%config_file raise IOError c.read(config_file) # Link lengths self.YAW_LEN = c.getfloat(section, "yaw_len") self.THIGH_LEN = c.getfloat(section, "thigh_len") self.CALF_LEN = c.getfloat(section, "calf_len") self.YAW_COM_COORD = [c.getfloat(section, "yaw_com_x"), c.getfloat(section, "yaw_com_y"), c.getfloat(section, "yaw_com_z")] self.THIGH_COM_COORD = [c.getfloat(section, "thigh_com_x"), c.getfloat(section, "thigh_com_y"), c.getfloat(section, "thigh_com_z")] self.CALF_COM_COORD = [c.getfloat(section, "calf_com_x"), c.getfloat(section, "calf_com_y"), c.getfloat(section, "calf_com_z")] self.YAW_MASS = c.getfloat(section, "yaw_mass") self.THIGH_MASS = c.getfloat(section, "thigh_mass") self.CALF_MASS = c.getfloat(section, "calf_mass") self.LEG_MASS = self.YAW_MASS+self.THIGH_MASS+self.CALF_MASS # Actuator soft bounds soft_stops_section = 'SoftStops' self.SOFT_MINS=array( [c.getfloat(soft_stops_section,'yaw_stop_low'), c.getfloat(soft_stops_section,'pitch_stop_low'), c.getfloat(soft_stops_section,'knee_stop_low')]) self.SOFT_MAXES=array( [c.getfloat(soft_stops_section,'yaw_stop_high'), c.getfloat(soft_stops_section,'pitch_stop_high'), c.getfloat(soft_stops_section,'knee_stop_high') ]) # State vel_corner = 100.0 # rad/s self.jv_filter = HighPassFilter(vel_corner, vel_corner) # band limited differentiator self.setSensorReadings(self.SOFT_MINS[0], self.SOFT_MINS[1], self.SOFT_MINS[2], 0.0) # Events self.SHOCK_DEPTH_THRESHOLD_LOW = c.getfloat(section, "shock_depth_threshold_low") self.SHOCK_DEPTH_THRESHOLD_HIGH = c.getfloat(section, "shock_depth_threshold_high") self.foot_on_ground = False
class LegModel: def __init__(self, config_file="leg_model.conf", section="LegModel"): c = ConfigParser() if not path.exists(path.abspath(config_file)): print 'Config file %s not found!'%config_file raise IOError c.read(config_file) # Link lengths self.YAW_LEN = c.getfloat(section, "yaw_len") self.THIGH_LEN = c.getfloat(section, "thigh_len") self.CALF_LEN = c.getfloat(section, "calf_len") self.YAW_COM_COORD = [c.getfloat(section, "yaw_com_x"), c.getfloat(section, "yaw_com_y"), c.getfloat(section, "yaw_com_z")] self.THIGH_COM_COORD = [c.getfloat(section, "thigh_com_x"), c.getfloat(section, "thigh_com_y"), c.getfloat(section, "thigh_com_z")] self.CALF_COM_COORD = [c.getfloat(section, "calf_com_x"), c.getfloat(section, "calf_com_y"), c.getfloat(section, "calf_com_z")] self.YAW_MASS = c.getfloat(section, "yaw_mass") self.THIGH_MASS = c.getfloat(section, "thigh_mass") self.CALF_MASS = c.getfloat(section, "calf_mass") self.LEG_MASS = self.YAW_MASS+self.THIGH_MASS+self.CALF_MASS # Actuator soft bounds soft_stops_section = 'SoftStops' self.SOFT_MINS=array( [c.getfloat(soft_stops_section,'yaw_stop_low'), c.getfloat(soft_stops_section,'pitch_stop_low'), c.getfloat(soft_stops_section,'knee_stop_low')]) self.SOFT_MAXES=array( [c.getfloat(soft_stops_section,'yaw_stop_high'), c.getfloat(soft_stops_section,'pitch_stop_high'), c.getfloat(soft_stops_section,'knee_stop_high') ]) # State vel_corner = 100.0 # rad/s self.jv_filter = HighPassFilter(vel_corner, vel_corner) # band limited differentiator self.setSensorReadings(self.SOFT_MINS[0], self.SOFT_MINS[1], self.SOFT_MINS[2], 0.0) # Events self.SHOCK_DEPTH_THRESHOLD_LOW = c.getfloat(section, "shock_depth_threshold_low") self.SHOCK_DEPTH_THRESHOLD_HIGH = c.getfloat(section, "shock_depth_threshold_high") self.foot_on_ground = False # Store sensor readings 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) # Access state def getJointAngles(self): return self.joint_angles def getJointVelocities(self): return self.joint_velocities def getShockDepth(self): return self.shock_depth def getLegState(self): return [self.getJointAngles(), self.getShockDepth()] # Kinematics def getFootPos(self): return self.footPosFromLegState(self.getLegState()) def footPosFromLegState(self, leg_state): pos = array([self.CALF_LEN - leg_state[1], 0.0, 0.0]) pos = rotateY(pos, leg_state[0][KP]) pos[X] += self.THIGH_LEN pos = rotateY(pos, leg_state[0][HP]) pos[X] += self.YAW_LEN return rotateZ(pos, leg_state[0][YAW]) def jointAnglesFromFootPos(self, pos, shock_depth=0.0): # Calculate hip yaw hip_yaw_angle = atan2( pos[1], pos[0] ) # Calculate hip yaw offset hip_p = norm2( (pos[0], pos[1]) ) hip_p = (self.YAW_LEN*hip_p[0], self.YAW_LEN*hip_p[1], 0.0) # Calculate leg length leg_l = dist3( pos, hip_p ) # Use law of cosines on leg length to calculate knee angle knee_angle = pi-thetaFromABC( self.THIGH_LEN,\ self.CALF_LEN-shock_depth, leg_l ) # Calculate hip pitch hip_offset_angle = thetaFromABC( self.THIGH_LEN, leg_l, self.CALF_LEN-shock_depth ) target_p = sub3(pos, hip_p) hip_depression_angle = atan2( -target_p[2], len2((target_p[0], target_p[1])) ) hip_pitch_angle = hip_depression_angle - hip_offset_angle return (hip_yaw_angle, hip_pitch_angle, knee_angle) def posIsPossible(self, pos, lower_limits, upper_limits): ikSolution = self.jointAnglesFromFootPos(pos) for i in range(len(pos)): if lower_limits[i] > pos[i] or pos[i] > upper_limits[i]: return False return True # Touch-down detection def isFootOnGround(self): return self.foot_on_ground def updateFootOnGround(self): sd = self.getShockDepth() if sd < self.SHOCK_DEPTH_THRESHOLD_LOW: self.foot_on_ground = False elif sd > self.SHOCK_DEPTH_THRESHOLD_HIGH: self.foot_on_ground = True def isMoving(self, tolerance=0.001): return (abs(self.joint_velocities) >= tolerance).any() def getCOM(self): """ This function returns the COM of the leg given its current joint positions. """ joint_angles = self.getJointAngles() # Calculate Yaw position yaw_pos = rotateZ(self.YAW_COM_COORD, joint_angles[YAW]) # Calculate Thigh position thigh_pos = rotateY(self.THIGH_COM_COORD, joint_angles[HP]) thigh_pos[X] += self.YAW_LEN thigh_pos = rotateZ(thigh_pos, joint_angles[YAW]) # Calculate Calf position calf_pos = rotateY(self.CALF_COM_COORD, joint_angles[KP]) calf_pos[X] += self.THIGH_LEN calf_pos = rotateY(calf_pos, joint_angles[HP]) calf_pos[X] += self.YAW_LEN calf_pos = rotateZ(calf_pos, joint_angles[YAW]) COM_pos = (yaw_pos*self.YAW_MASS+thigh_pos*self.THIGH_MASS+calf_pos*self.CALF_MASS)/(self.LEG_MASS) return COM_pos
class LegModel: def __init__(self, config_file="leg_model.conf", section="LegModel"): c = ConfigParser() if not path.exists(path.abspath(config_file)): print 'Config file %s not found!' % config_file raise IOError c.read(config_file) # Link lengths self.YAW_LEN = c.getfloat(section, "yaw_len") self.THIGH_LEN = c.getfloat(section, "thigh_len") self.CALF_LEN = c.getfloat(section, "calf_len") # Actuator soft bounds soft_stops_section = 'SoftStops' self.SOFT_MINS = array([ c.getfloat(soft_stops_section, 'yaw_stop_low'), c.getfloat(soft_stops_section, 'pitch_stop_low'), c.getfloat(soft_stops_section, 'knee_stop_low') ]) self.SOFT_MAXES = array([ c.getfloat(soft_stops_section, 'yaw_stop_high'), c.getfloat(soft_stops_section, 'pitch_stop_high'), c.getfloat(soft_stops_section, 'knee_stop_high') ]) # State vel_corner = 100.0 # rad/s self.jv_filter = HighPassFilter( vel_corner, vel_corner) # band limited differentiator self.setSensorReadings(self.SOFT_MINS[0], self.SOFT_MINS[1], self.SOFT_MINS[2], 0.0) # Events self.SHOCK_DEPTH_THRESHOLD_LOW = c.getfloat( section, "shock_depth_threshold_low") self.SHOCK_DEPTH_THRESHOLD_HIGH = c.getfloat( section, "shock_depth_threshold_high") self.foot_on_ground = False # Store sensor readings 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) # Access state def getJointAngles(self): return self.joint_angles def getJointVelocities(self): return self.joint_velocities def getShockDepth(self): return self.shock_depth def getLegState(self): return [self.getJointAngles(), self.getShockDepth()] # Kinematics def getFootPos(self): return self.footPosFromLegState(self.getLegState()) def footPosFromLegState(self, leg_state): pos = array([self.CALF_LEN - leg_state[1], 0.0, 0.0]) pos = rotateY(pos, leg_state[0][KP]) pos[X] += self.THIGH_LEN pos = rotateY(pos, leg_state[0][HP]) pos[X] += self.YAW_LEN return rotateZ(pos, leg_state[0][YAW]) def jointAnglesFromFootPos(self, pos, shock_depth=0.0): # Calculate hip yaw hip_yaw_angle = atan2(pos[1], pos[0]) # Calculate hip yaw offset hip_p = norm2((pos[0], pos[1])) hip_p = (self.YAW_LEN * hip_p[0], self.YAW_LEN * hip_p[1], 0.0) # Calculate leg length leg_l = dist3(pos, hip_p) # Use law of cosines on leg length to calculate knee angle knee_angle = pi - thetaFromABC(self.THIGH_LEN, self.CALF_LEN - shock_depth, leg_l) # Calculate hip pitch hip_offset_angle = thetaFromABC(self.THIGH_LEN, leg_l, self.CALF_LEN - shock_depth) target_p = sub3(pos, hip_p) hip_depression_angle = atan2(-target_p[2], len2((target_p[0], target_p[1]))) hip_pitch_angle = hip_depression_angle - hip_offset_angle return (hip_yaw_angle, hip_pitch_angle, knee_angle) def posIsPossible(self, pos, lower_limits, upper_limits): ikSolution = self.jointAnglesFromFootPos(pos) for i in range(len(pos)): if lower_limits[i] > pos[i] or pos[i] > upper_limits[i]: return False return True # Touch-down detection def isFootOnGround(self): return self.foot_on_ground def updateFootOnGround(self): sd = self.getShockDepth() if sd < self.SHOCK_DEPTH_THRESHOLD_LOW: self.foot_on_ground = False elif sd > self.SHOCK_DEPTH_THRESHOLD_HIGH: self.foot_on_ground = True def isMoving(self, tolerance=0.001): return (abs(self.joint_velocities) >= tolerance).any()