def __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration, z_plane=-0.67865): leg_logger.logger.info("New path.", path_name="TrapezoidalFootMove", final_foot_pos=final_foot_pos, max_velocity=max_velocity, acceleration=acceleration) self.model = leg_model self.controller = limb_controller self.final_foot_pos = final_foot_pos self.final_joint_angles = self.model.jointAnglesFromFootPos(final_foot_pos, shock_depth=0.0) self.max_vel = max_velocity self.vel = 0.0 self.acc = acceleration self.z_plane = z_plane self.initial_foot_pos = self.model.footPosFromLegState([self.controller.getDesiredPosAngle(), 0]) self.initial_joint_angles = self.controller.getDesiredPosAngle() self.target_angles = self.initial_joint_angles self.projected_initial = [self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane] self.projected_final = [self.final_foot_pos[0], self.final_foot_pos[1], max(self.final_foot_pos[2],z_plane)] self.done = False self.LIFT = 1 self.TRANSLATE = 2 self.LOWER = 3 self.state = self.LIFT self.path = TrapezoidalFootMove(self.model, self.controller, self.projected_initial, self.max_vel, self.acc) if self.initial_foot_pos[2] > self.z_plane: self.state = self.TRANSLATE self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
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 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 __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration, z_plane=-0.67865): leg_logger.logger.info("New path.", path_name="TrapezoidalFootMove", final_foot_pos=final_foot_pos, max_velocity=max_velocity, acceleration=acceleration) self.model = leg_model self.controller = limb_controller self.final_foot_pos = final_foot_pos self.final_joint_angles = self.model.jointAnglesFromFootPos( final_foot_pos, shock_depth=0.0) self.max_vel = max_velocity self.vel = 0.0 self.acc = acceleration self.z_plane = z_plane self.initial_foot_pos = self.model.footPosFromLegState( [self.controller.getDesiredPosAngle(), 0]) self.initial_joint_angles = self.controller.getDesiredPosAngle() self.target_angles = self.initial_joint_angles self.projected_initial = [ self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane ] self.projected_final = [ self.final_foot_pos[0], self.final_foot_pos[1], max(self.final_foot_pos[2], z_plane) ] self.done = False self.LIFT = 1 self.TRANSLATE = 2 self.LOWER = 3 self.state = self.LIFT self.path = TrapezoidalFootMove(self.model, self.controller, self.projected_initial, self.max_vel, self.acc) if self.initial_foot_pos[2] > self.z_plane: self.state = self.TRANSLATE self.path = TrapezoidalJointMove( self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
def __init__(self, body_model, body_controller, leg_indices, delta_heights, max_velocity, acceleration): logger.info("New path.", path_name="TrapezoidalFeetLiftLower", leg_indices=leg_indices, delta_height=delta_heights, max_velocity=max_velocity, acceleration=acceleration) self.leg_indices = leg_indices self.model = body_model self.controller = body_controller self.final_joint_positions = self.controller.getTargetJointAngleMatrix( ) self.foot_paths = [None for i in range(NUM_LEGS)] if isinstance(delta_heights, (int, long, float, complex)): delta_heights = [delta_heights] * 6 for i in self.leg_indices: current_leg_pos = self.model.getLegs()[i].footPosFromLegState( [self.controller.getTargetJointAngleMatrix()[i], 0]) # self.model.getFootPositions()[i] desired_leg_pos = current_leg_pos + [0, 0, delta_heights[i]] self.foot_paths[i] = TrapezoidalFootMove( self.model.getLegs()[i], self.controller.getLimbControllers()[i], desired_leg_pos, max_velocity, acceleration) self.done = False
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): global path, state # Update model time_sources.global_time.updateTime(time) model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) model.updateFootOnGround() # Init path. Do this after the first update. if path is None: path = Pause(model, controller, 5.0) # Monitor leg_paths if path.isDone(): if state == S_INIT: print "Move" * 1000 path = TrapezoidalFootMove(model, controller, array([1.5, 0.0, -0.4]), 0.5, 0.5) state = S_MOVE_JOINT elif state == S_MOVE_JOINT: print "Done" * 1000 state = S_DONE pass logger.info("State changed.", state=state) print "Foot:", model.getFootPos() # Evaluate path and joint control controller.update(model.getJointAngles(), path.update()) # Send commands return controller.getLengthRateCommands()
def __init__(self, body_model, body_controller, final_height, max_velocity, acceleration): logger.info("New path.", path_name="TrapezoidalSitStand", final_height=final_height, max_velocity=max_velocity, acceleration=acceleration) self.model = body_model self.controller = body_controller self.final_foot_positions = self.model.getFootPositions() self.feet_path = [] for i in range(NUM_LEGS): self.final_foot_positions[i][2] = final_height for i in range(NUM_LEGS): self.feet_path = append( self.feet_path, TrapezoidalFootMove(self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.final_foot_positions[i], max_velocity, acceleration)) self.done = False
def update(self): if not self.isDone(): self.target_angles = self.path.update() self.done = self.path.isDone() if self.isDone(): if self.state == self.LIFT: self.state = self.TRANSLATE self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc) self.done = False elif self.state == self.TRANSLATE and self.final_foot_pos[2] < self.z_plane: self.state = self.LOWER self.path = TrapezoidalFootMove(self.model, self.controller, self.final_foot_pos, self.max_vel, self.acc) self.done = False return self.target_angles
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([2.0, 0.0, -0.2]), 0.4, 0.2) self.controller.update(self.model.getJointAngles(), self.initial_path.update()) if self.initial_path.isDone(): self.callback = self.pause return self.controller.getLengthRateCommands()
def update(self): if not self.isDone(): self.target_angles = self.path.update() self.done = self.path.isDone() if self.isDone(): if self.state == self.LIFT: self.state = self.TRANSLATE self.path = TrapezoidalJointMove( self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc) self.done = False elif self.state == self.TRANSLATE and self.final_foot_pos[ 2] < self.z_plane: self.state = self.LOWER self.path = TrapezoidalFootMove(self.model, self.controller, self.final_foot_pos, self.max_vel, self.acc) self.done = False return self.target_angles
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): global path, state # Update model time_sources.global_time.updateTime(time) model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) model.updateFootOnGround() # Init path. Do this after the first update. if path is None: path = Pause(model, controller, 1.0) # Monitor leg_paths if path.isDone(): if state == S_MOVE3: path = TrapezoidalFootMove(model, controller, array([1.5, -0.6, -0.4]), 0.2, 0.1) state = S_MOVE1 elif state == S_MOVE1: path = PutFootOnGround(model, controller, 0.05) state = S_LOWER elif state == S_LOWER: ground_level = model.getFootPos()[Z] path = TrapezoidalFootMove(model, controller, array([1.5, .6, ground_level]), 0.2, 0.1) state = S_MOVE2 elif state == S_MOVE2: path = TrapezoidalFootMove(model, controller, array([1.5, 0.6, -0.4]), 0.2, 0.1) state = S_MOVE3 logger.info("State changed.", state=state) # Evaluate path and joint control controller.update(model.getJointAngles(), path.update()) # Send commands return controller.getLengthRateCommands()
def __init__(self, body_model, body_controller, max_velocity=2.0, acceleration=1.0): leg_logger.logger.info("New path.", path_name="GoToStandardHexagon", max_velocity=max_velocity, acceleration=acceleration) self.model = body_model self.controller = body_controller self.max_velocity = max_velocity self.acceleration = acceleration self.final_joint_positions = self.controller.getTargetJointAngleMatrix() self.foot_paths = [None for i in range(NUM_LEGS)] self.lifted = array([1.651 , 0 , 0]) self.lowered = array([1.651 , 0 , -1.6764]) self.NONE = 0 self.LOWER_ALL = 1 self.EVENS = 2 self.LOWER_EVENS = 3 self.ODDS = 4 self.LOWER_ODDS = 5 self.STAND = 6 self.state = self.LOWER_ALL self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()] self.in_place = [False] * 6 # Put all 6 legs in place for i in range(NUM_LEGS): if not self.on_ground[i] and not self.in_place[i]: self.foot_paths[i] = TrapezoidalFootMove(self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.lifted, self.max_velocity, self.acceleration) self.in_place[i] = True self.phase_done = False self.done = False
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): global points, path, index # Update model time_sources.global_time.updateTime(time) model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) model.updateFootOnGround() # Init path. Do this after the first update. if path is None: path = Pause(model, controller, 1.0) if path.isDone(): path = TrapezoidalFootMove(model, controller, array(points[index]), 0.5, 0.4) index = (index + 1) % len(points) # Evaluate path and joint control controller.update(model.getJointAngles(), path.update()) # Send commands return controller.getLengthRateCommands()
class SafeMove: """This is foot movement that combines up to two trapezoidal foot moves and up to one trapezoidal joint move to move the foot safely without fear of dragging on the ground. NOTE: The Z-plane is the highest horizontal plane that ensures a convex volume below it. This is the boundary of where TrapFootMove is guaranteed to work, so we switch to TrapJointMove above it. It should be found via side-angle-side of the knee and calf links and the angle they make most folded up. We use only vertical motions below the Z-plane, as there is a high likelihood of hitting the ground. """ def __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration, z_plane=-0.67865): leg_logger.logger.info("New path.", path_name="TrapezoidalFootMove", final_foot_pos=final_foot_pos, max_velocity=max_velocity, acceleration=acceleration) self.model = leg_model self.controller = limb_controller self.final_foot_pos = final_foot_pos self.final_joint_angles = self.model.jointAnglesFromFootPos(final_foot_pos, shock_depth=0.0) self.max_vel = max_velocity self.vel = 0.0 self.acc = acceleration self.z_plane = z_plane self.initial_foot_pos = self.model.footPosFromLegState([self.controller.getDesiredPosAngle(), 0]) self.initial_joint_angles = self.controller.getDesiredPosAngle() self.target_angles = self.initial_joint_angles self.projected_initial = [self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane] self.projected_final = [self.final_foot_pos[0], self.final_foot_pos[1], max(self.final_foot_pos[2],z_plane)] self.done = False self.LIFT = 1 self.TRANSLATE = 2 self.LOWER = 3 self.state = self.LIFT self.path = TrapezoidalFootMove(self.model, self.controller, self.projected_initial, self.max_vel, self.acc) if self.initial_foot_pos[2] > self.z_plane: self.state = self.TRANSLATE self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc) def isDone(self): return self.done def update(self): if not self.isDone(): self.target_angles = self.path.update() self.done = self.path.isDone() if self.isDone(): if self.state == self.LIFT: self.state = self.TRANSLATE self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc) self.done = False elif self.state == self.TRANSLATE and self.final_foot_pos[2] < self.z_plane: self.state = self.LOWER self.path = TrapezoidalFootMove(self.model, self.controller, self.final_foot_pos, self.max_vel, self.acc) self.done = False return self.target_angles
class SafeMove: """ This is foot movement that combines up to two trapezoidal foot moves and up to one trapezoidal joint move to move the foot safely without fear of dragging on the ground. NOTE: The Z-plane is the highest horizontal plane that ensures a convex volume below it. This is the boundary of where TrapFootMove is guaranteed to work, so we switch to TrapJointMove above it. It should be found via side-angle-side of the knee and calf links and the angle they make most folded up. We use only vertical motions below the Z-plane, as there is a high likelihood of hitting the ground. """ def __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration, z_plane=-0.67865): leg_logger.logger.info("New path.", path_name="TrapezoidalFootMove", final_foot_pos=final_foot_pos, max_velocity=max_velocity, acceleration=acceleration) self.model = leg_model self.controller = limb_controller self.final_foot_pos = final_foot_pos self.final_joint_angles = self.model.jointAnglesFromFootPos( final_foot_pos, shock_depth=0.0) self.max_vel = max_velocity self.vel = 0.0 self.acc = acceleration self.z_plane = z_plane self.initial_foot_pos = self.model.footPosFromLegState( [self.controller.getDesiredPosAngle(), 0]) self.initial_joint_angles = self.controller.getDesiredPosAngle() self.target_angles = self.initial_joint_angles self.projected_initial = [ self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane ] self.projected_final = [ self.final_foot_pos[0], self.final_foot_pos[1], max(self.final_foot_pos[2], z_plane) ] self.done = False self.LIFT = 1 self.TRANSLATE = 2 self.LOWER = 3 self.state = self.LIFT self.path = TrapezoidalFootMove(self.model, self.controller, self.projected_initial, self.max_vel, self.acc) if self.initial_foot_pos[2] > self.z_plane: self.state = self.TRANSLATE self.path = TrapezoidalJointMove( self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc) def isDone(self): return self.done def update(self): if not self.isDone(): self.target_angles = self.path.update() self.done = self.path.isDone() if self.isDone(): if self.state == self.LIFT: self.state = self.TRANSLATE self.path = TrapezoidalJointMove( self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc) self.done = False elif self.state == self.TRANSLATE and self.final_foot_pos[ 2] < self.z_plane: self.state = self.LOWER self.path = TrapezoidalFootMove(self.model, self.controller, self.final_foot_pos, self.max_vel, self.acc) self.done = False return self.target_angles
def update(self): active_paths = [] active_index = [] for i in range(NUM_LEGS): if self.foot_paths[i] is not None: active_index.append(i) active_paths.append(self.foot_paths[i]) if self.phase_done: self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()] if self.state == self.LOWER_ALL: for i in range(NUM_LEGS): if not self.on_ground[i]: self.foot_paths[i] = PutFootOnGround( self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.max_velocity / 5) self.in_place[i] = True self.state = self.EVENS if all(self.in_place): self.state = self.STAND self.phase_done = False elif self.state == self.EVENS: for i in range(NUM_LEGS): if not i % 2 and not self.in_place[i]: self.foot_paths[i] = TrapezoidalFootMove( self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.lifted, self.max_velocity, self.acceleration) self.state = self.LOWER_EVENS self.phase_done = False elif self.state == self.LOWER_EVENS: for i in range(NUM_LEGS): if not self.on_ground[i]: self.foot_paths[i] = PutFootOnGround( self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.max_velocity / 5) self.in_place[i] = True self.state = self.ODDS self.phase_done = False elif self.state == self.ODDS: for i in range(NUM_LEGS): if i % 2 and not self.in_place[i]: self.foot_paths[i] = TrapezoidalFootMove( self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.lifted, self.max_velocity, self.acceleration) self.state = self.LOWER_ODDS self.phase_done = False elif self.state == self.LOWER_ODDS: for i in range(NUM_LEGS): if not self.on_ground[i]: self.foot_paths[i] = PutFootOnGround( self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.max_velocity / 5) self.in_place[i] = True self.state = self.STAND self.phase_done = False elif self.state == self.STAND: for i in range(NUM_LEGS): self.foot_paths[i] = TrapezoidalFootMove( self.model.getLegs()[i], self.controller.getLimbControllers()[i], self.lowered, self.max_velocity, self.acceleration * .1) self.state = self.NONE self.phase_done = False elif self.state == self.NONE: self.done = True return self.final_joint_positions elif not self.phase_done: #logically and all of the isdone results from the joint move paths self.phase_done = all(map(lambda p: p.isDone(), active_paths)) if not self.done: for i in active_index: self.final_joint_positions[i] = self.foot_paths[i].update() return self.final_joint_positions
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()
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
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()