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]
