def __init__(self): self.model = LegModel() self.controller = LimbController() self.firstrun = True # 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_pist = PistonController(YAW_ACT, 0.26, 0.28) pitch_pist = PistonController(PITCH_ACT, 0.27, 0.27) knee_pist = PistonController(KNEE_ACT, 0.22, 0.20) self.pist_helpers = [yaw_pist, pitch_pist, knee_pist] # Hackery for notch filter self.hip_yaw_commands = [ 0.0 for i in range(int((0.5 / 2.00) / (1 / 200.0))) ] self.hip_pitch_commands = [ 0.0 for i in range(int((0.5 / 6.30) / (1 / 200.0))) ] self.knee_commands = [ 0.0 for i in range(int((0.5 / 6.30) / (1 / 200.0))) ]
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()
from ControlsKit import time_sources, LegModel, LimbController from ControlsKit.leg_paths import Pause, TrapezoidalFootMove, PutFootOnGround from ControlsKit.math_utils import array, Z from UI import logger # Initialization model = LegModel() controller = LimbController() path = None S_MOVE1 = 0 S_LOWER = 1 S_MOVE2 = 2 S_MOVE3 = 3 state = S_MOVE3 # Body of control loop 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)
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]