class Gait: def __init__(self): self.file_out = file('offsets.txt', 'w+') self.vel_IIR = None self.pwm_val = 0 self.last_pos = 0 self.last_t = 0 self.MOVE_THRESH = .01 self.firstrun = True self.dof_list = [] self.dof_list.append('hip_pitch_e') self.dof_list.append('hip_pitch_r') self.dof_list.append('knee_pitch_e') self.dof_list.append('knee_pitch_r') self.dof_list.append('hip_yaw_e') self.dof_list.append('hip_yaw_r') self.callback = self.moveToInitialPosition self.model = LegModel() self.controller = LimbController() def update( self, *args, **kwargs ): global_time.updateTime(args[0]) return self.callback(*args, **kwargs) def moveToInitialPosition( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ): self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) if not hasattr(self, 'initial_path'): print "Starting move to initial position" self.initial_path = TrapezoidalFootMove(self.model, self.controller,\ array([1.6, 0.0, -1.15]),\ 0.4, 0.2) self.controller.update(self.model.getJointAngles(), self.initial_path.update()) if self.initial_path.isDone(): self.callback = self.pause lr = self.controller.getLengthRateCommands() return lr def pause( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ): if not hasattr( self, 'start_pause_time' ): print "Waiting..." self.start_pause_time = time #self.pause_path = Pause( self.model, self.controller, 3.0 ) #self.controller.update(self.model.getJointAngles(), self.pause_path.update()) if time - self.start_pause_time > 3.0: self.callback = self.discoverDeadband del self.start_pause_time return [0.0,0.0,0.0] def discoverDeadband( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ): # Have we made it through all the joints? if not len(self.dof_list): # FIXME: Exit safely self.file_out.close() self.callback = self.pause if self.dof_list[0][:-2] == 'hip_pitch': pos = hip_pitch cmd_i = 1 elif self.dof_list[0][:-2] == 'knee_pitch': pos = knee_pitch cmd_i = 2 elif self.dof_list[0][:-2] == 'hip_yaw': pos = yaw cmd_i = 0 else: raise if self.dof_list[0][-1] == 'e': sign = 1 elif self.dof_list[0][-1] == 'r': sign = -1 else: raise # Default command: command no movement on all valves # valves are yaw, pitch, knee, in that order # Scale depends on the mode of the underlying layer... # sometimes it expects a length rate in meters/sec, # sometimes it expects a command value between -255 and 255 cmd = [0.0,0.0,0.0] # Is this our first tick on a new DOF? if self.firstrun: # Initialize state variables print "Initializing for %s"%self.dof_list[0] self.pwm_val = 0 self.vel_IIR = LowPassFilter( gain=1.0, corner_frequency=.5 ) self.firstrun = False else: # Calculate velocity, update filter dt = time-self.last_t dpos = pos - self.last_pos vel_est = dpos/dt self.vel_IIR.update(vel_est) # Take 10 seconds to ramp valve command self.pwm_val += dt*0.05 cmd[cmd_i] = sign*self.pwm_val self.last_t = time self.last_pos = pos # Are we moving faster than our threshold? if sign*self.vel_IIR.getVal() > self.MOVE_THRESH: # If so, note the PWM value we had to apply and prepare to move the # next joint self.file_out.write( "%s: %f\n"%(self.dof_list[0],self.pwm_val) ) print "%s: %f"%(self.dof_list[0],self.pwm_val) self.dof_list.pop(0) self.firstrun = True self.callback = self.pause return cmd
class Gait(object): def __init__(self): self.model = LegModel() self.controller = LimbController() self.path = None self.index = 0 self.points = [] #self.points.append( (0.4, 0, -0.45, 0.2) ) self.points.append((1.8, -1.00, -0.75, 0.40)) self.points.append((1.8, -1.00, -1.68, 0.20)) self.points.append((1.8, 1.00, -1.68, 0.40)) self.points.append((1.8, 1.00, -0.75, 0.20)) #self.points.append( (1.8, -1.00, -1.45, 0.40) ) #self.points.append( (1.8, -1.00, -1.68, 0.70) ) #self.points.append( (1.8, 1.00, -1.68, 0.70) ) #self.points.append( (1.8, 0.00, -1.35, 0.70) ) # Describe the actuator placements at each joint YAW_ACT = ActuatorMathHelper(bore_diameter=inch2meter * 2.5, rod_diameter=inch2meter * 1.125, act_retracted_len=inch2meter * 14.25, act_extended_len=inch2meter * 18.25, pivot1_dist_from_joint=inch2meter * 16.18, pivot2=(inch2meter * 2.32, inch2meter * 3.3), ang_offset=deg2rad * -30.0, system_pressure=psi2pascal * 2300, axis=(0, 0, -1)) PITCH_ACT = ActuatorMathHelper( bore_diameter=inch2meter * 3.0, rod_diameter=inch2meter * 1.5, act_retracted_len=inch2meter * 20.25, act_extended_len=inch2meter * 30.25, pivot1_dist_from_joint=inch2meter * 8.96, pivot2=(inch2meter * 27.55, inch2meter * 8.03), ang_offset=deg2rad * -84.0, system_pressure=psi2pascal * 2300, axis=(0, -1, 0)) KNEE_ACT = ActuatorMathHelper(bore_diameter=inch2meter * 2.5, rod_diameter=inch2meter * 1.25, act_retracted_len=inch2meter * 22.25, act_extended_len=inch2meter * 34.25, pivot1_dist_from_joint=inch2meter * 28.0, pivot2=(inch2meter * 4.3, inch2meter * 6.17), ang_offset=deg2rad * 61.84, system_pressure=psi2pascal * 2300, axis=(0, -1, 0)) self.act_helpers = [YAW_ACT, PITCH_ACT, KNEE_ACT] yaw_joint = JointController(YAW_ACT, kp=1.0, ki=0.0, kd=0.0, kff=1.0, kfa=0.0, derivative_corner=10, backlash=deg2rad * 1.5) pitch_joint = JointController(PITCH_ACT, kp=1.0, ki=0.0, kd=0.0, kff=1.0, kfa=0.0, derivative_corner=10, backlash=deg2rad * 0.5) knee_joint = JointController(KNEE_ACT, kp=1.0, ki=0.0, kd=0.0, kff=1.0, kfa=0.0, derivative_corner=10, backlash=deg2rad * 0.75) self.joint_controllers = [yaw_joint, pitch_joint, knee_joint] #yaw_pist = LearningPistonController(YAW_ACT , 0.26, 0.28) yaw_pist = LearningPistonController('YAWPIST', YAW_ACT, 0.37, 0.37) pitch_pist = LearningPistonController('PITCHPIST', PITCH_ACT, 0.32, 0.32) knee_pist = LearningPistonController('KNEEPIST', KNEE_ACT, 0.22, 0.20) self.pist_helpers = [yaw_pist, pitch_pist, knee_pist] def update(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): # Update model time_sources.global_time.updateTime(time) self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) self.model.updateFootOnGround() # Init path. Do this after the first update. if self.path is None: self.path = Pause(self.model, controller, 1.0) if self.path.isDone(): self.path = TrapezoidalFootMove( self.model, controller, array(self.points[self.index][0:3]), self.points[self.index][3], 0.4) self.index = (self.index + 1) % len(self.points) # Evaluate path and joint control present_angles = self.model.getJointAngles() target_angles = self.path.update() controller.update(present_angles, target_angles) control_lin_rates = [ self.joint_controllers[i].update(target_angles[i], present_angles[i]) for i in range(3) ] target_lin_rates = [ c.getDesiredLengthRate() for c in self.joint_controllers ] measured_lin_rates = [ c.getLengthRate() for c in self.joint_controllers ] if 0: s = '' m = self.pist_helpers[0].clay_pit_pos.mem for i in range(len(m)): if not i % 8: s += '%2.2f ' % (m[i]) print s commands = [ self.pist_helpers[i].update(control_lin_rates[i], measured_lin_rates[i]) for i in range(3) ] #commands = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0] if 0: target_ang_rates = [c.desired_vel for c in self.joint_controllers] measured_ang_rates = [ c.getAngleRate() for c in self.joint_controllers ] s = 'Measured: ' for m in measured_ang_rates: s += '%1.3f ' % (m * 180 / pi) print s s = 'Target: ' for m in target_ang_rates: s += '%1.3f ' % (m * 180 / pi) print s s = 'M Lin' for m in measured_lin_rates: s += '%1.3f ' % m print s s = 'T Lin' for m in target_lin_rates: s += '%1.3f ' % m print s self.pist_helpers[0].clay_pit_pos.printpit() return commands def gracefulExit(self): for i in range(3): self.pist_helpers[i].saveState()
class Gait(object): def __init__( self ): self.model = LegModel() self.controller = LimbController() self.path = None self.index = 0 self.points = [] #self.points.append( (0.4, 0, -0.45, 0.2) ) self.points.append( (1.8, -1.00, -0.75, 0.40) ) self.points.append( (1.8, -1.00, -1.68, 0.20) ) self.points.append( (1.8, 1.00, -1.68, 0.40) ) self.points.append( (1.8, 1.00, -0.75, 0.20) ) #self.points.append( (1.8, -1.00, -1.45, 0.40) ) #self.points.append( (1.8, -1.00, -1.68, 0.70) ) #self.points.append( (1.8, 1.00, -1.68, 0.70) ) #self.points.append( (1.8, 0.00, -1.35, 0.70) ) # Describe the actuator placements at each joint YAW_ACT = ActuatorMathHelper( bore_diameter = inch2meter*2.5, rod_diameter = inch2meter*1.125, act_retracted_len = inch2meter*14.25, act_extended_len = inch2meter*18.25, pivot1_dist_from_joint = inch2meter*16.18, pivot2 = (inch2meter*2.32,inch2meter*3.3), ang_offset = deg2rad*-30.0, system_pressure = psi2pascal*2300, axis = ( 0, 0, -1 ) ) PITCH_ACT = ActuatorMathHelper( bore_diameter = inch2meter*3.0, rod_diameter = inch2meter*1.5, act_retracted_len = inch2meter*20.25, act_extended_len = inch2meter*30.25, pivot1_dist_from_joint = inch2meter*8.96, pivot2 = (inch2meter*27.55,inch2meter*8.03), ang_offset = deg2rad*-84.0, system_pressure = psi2pascal*2300, axis = ( 0, -1, 0 ) ) KNEE_ACT = ActuatorMathHelper( bore_diameter = inch2meter*2.5, rod_diameter = inch2meter*1.25, act_retracted_len = inch2meter*22.25, act_extended_len = inch2meter*34.25, pivot1_dist_from_joint = inch2meter*28.0, pivot2 = (inch2meter*4.3,inch2meter*6.17), ang_offset = deg2rad*61.84, system_pressure = psi2pascal*2300, axis = ( 0, -1, 0 ) ) self.act_helpers = [YAW_ACT, PITCH_ACT, KNEE_ACT] yaw_joint = JointController(YAW_ACT, kp=1.0, ki=0.0, kd=0.0, kff=1.0, kfa=0.0, derivative_corner=10, backlash=deg2rad*1.5) pitch_joint = JointController(PITCH_ACT, kp=1.0, ki=0.0, kd=0.0, kff=1.0, kfa=0.0, derivative_corner=10, backlash=deg2rad*0.5) knee_joint = JointController(KNEE_ACT, kp=1.0, ki=0.0, kd=0.0, kff=1.0, kfa=0.0, derivative_corner=10, backlash=deg2rad*0.75) self.joint_controllers = [yaw_joint, pitch_joint, knee_joint] #yaw_pist = LearningPistonController(YAW_ACT , 0.26, 0.28) yaw_pist = LearningPistonController('YAWPIST', YAW_ACT , 0.37, 0.37) pitch_pist = LearningPistonController('PITCHPIST', PITCH_ACT, 0.32, 0.32) knee_pist = LearningPistonController('KNEEPIST', KNEE_ACT , 0.22, 0.20) self.pist_helpers = [yaw_pist, pitch_pist, knee_pist] def update( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): # Update model time_sources.global_time.updateTime(time) self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) self.model.updateFootOnGround() # Init path. Do this after the first update. if self.path is None: self.path = Pause(self.model, controller, 1.0) if self.path.isDone(): self.path = TrapezoidalFootMove(self.model, controller, array(self.points[self.index][0:3]), self.points[self.index][3], 0.4) self.index = (self.index+1)%len(self.points) # Evaluate path and joint control present_angles = self.model.getJointAngles() target_angles = self.path.update() controller.update(present_angles, target_angles) control_lin_rates = [self.joint_controllers[i].update(target_angles[i], present_angles[i]) for i in range(3)] target_lin_rates = [c.getDesiredLengthRate() for c in self.joint_controllers] measured_lin_rates = [c.getLengthRate() for c in self.joint_controllers] if 0: s = '' m = self.pist_helpers[0].clay_pit_pos.mem for i in range(len(m)): if not i%8: s += '%2.2f '%(m[i]) print s commands = [self.pist_helpers[i].update(control_lin_rates[i], measured_lin_rates[i]) for i in range(3)] #commands = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0] if 0: target_ang_rates = [c.desired_vel for c in self.joint_controllers] measured_ang_rates = [c.getAngleRate() for c in self.joint_controllers] s = 'Measured: ' for m in measured_ang_rates: s += '%1.3f '%(m*180/pi) print s s = 'Target: ' for m in target_ang_rates: s += '%1.3f '%(m*180/pi) print s s = 'M Lin' for m in measured_lin_rates: s += '%1.3f '%m print s s = 'T Lin' for m in target_lin_rates: s += '%1.3f '%m print s self.pist_helpers[0].clay_pit_pos.printpit() return commands def gracefulExit(self): for i in range(3): self.pist_helpers[i].saveState()
class Gait: def __init__(self): self.file_out = file('offsets.txt', 'w+') self.vel_IIR = None self.pwm_val = 0 self.last_pos = 0 self.last_t = 0 self.MOVE_THRESH = .01 self.firstrun = True self.dof_list = [] self.dof_list.append('hip_pitch_e') self.dof_list.append('hip_pitch_r') self.dof_list.append('knee_pitch_e') self.dof_list.append('knee_pitch_r') self.dof_list.append('hip_yaw_e') self.dof_list.append('hip_yaw_r') self.callback = self.moveToInitialPosition self.model = LegModel() self.controller = LimbController() def update(self, *args, **kwargs): global_time.updateTime(args[0]) return self.callback(*args, **kwargs) def moveToInitialPosition(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) if not hasattr(self, 'initial_path'): print "Starting move to initial position" self.initial_path = TrapezoidalFootMove(self.model, self.controller,\ array([1.6, 0.0, -1.15]),\ 0.4, 0.2) self.controller.update(self.model.getJointAngles(), self.initial_path.update()) if self.initial_path.isDone(): self.callback = self.pause lr = self.controller.getLengthRateCommands() return lr def pause(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): if not hasattr(self, 'start_pause_time'): print "Waiting..." self.start_pause_time = time #self.pause_path = Pause( self.model, self.controller, 3.0 ) #self.controller.update(self.model.getJointAngles(), self.pause_path.update()) if time - self.start_pause_time > 3.0: self.callback = self.discoverDeadband del self.start_pause_time return [0.0, 0.0, 0.0] def discoverDeadband(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): # Have we made it through all the joints? if not len(self.dof_list): # FIXME: Exit safely self.file_out.close() self.callback = self.pause if self.dof_list[0][:-2] == 'hip_pitch': pos = hip_pitch cmd_i = 1 elif self.dof_list[0][:-2] == 'knee_pitch': pos = knee_pitch cmd_i = 2 elif self.dof_list[0][:-2] == 'hip_yaw': pos = yaw cmd_i = 0 else: raise if self.dof_list[0][-1] == 'e': sign = 1 elif self.dof_list[0][-1] == 'r': sign = -1 else: raise # Default command: command no movement on all valves # valves are yaw, pitch, knee, in that order # Scale depends on the mode of the underlying layer... # sometimes it expects a length rate in meters/sec, # sometimes it expects a command value between -255 and 255 cmd = [0.0, 0.0, 0.0] # Is this our first tick on a new DOF? if self.firstrun: # Initialize state variables print "Initializing for %s" % self.dof_list[0] self.pwm_val = 0 self.vel_IIR = LowPassFilter(gain=1.0, corner_frequency=.5) self.firstrun = False else: # Calculate velocity, update filter dt = time - self.last_t dpos = pos - self.last_pos vel_est = dpos / dt self.vel_IIR.update(vel_est) # Take 10 seconds to ramp valve command self.pwm_val += dt * 0.05 cmd[cmd_i] = sign * self.pwm_val self.last_t = time self.last_pos = pos # Are we moving faster than our threshold? if sign * self.vel_IIR.getVal() > self.MOVE_THRESH: # If so, note the PWM value we had to apply and prepare to move the # next joint self.file_out.write("%s: %f\n" % (self.dof_list[0], self.pwm_val)) print "%s: %f" % (self.dof_list[0], self.pwm_val) self.dof_list.pop(0) self.firstrun = True self.callback = self.pause return cmd