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 __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()
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
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]
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