class rateEstimator(object):
def __init__( self ):
self.last_state = 0.0
self.vel_est = LowPassFilter( gain=1.0, corner_frequency=10.0 )
def update( new_state ):
return self.vel_est.update((new_state-self.last_state)/global_time.getDelta())
def getVal( self ):
return self.vel_est.getVal()
class rateEstimator(object):
def __init__(self):
self.last_state = 0.0
self.vel_est = LowPassFilter(gain=1.0, corner_frequency=10.0)
def update(new_state):
return self.vel_est.update(
(new_state - self.last_state) / global_time.getDelta())
def getVal(self):
return self.vel_est.getVal()
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
yaw_encoder.startProbe()
pitch_encoder.startProbe()
knee_encoder.startProbe()
shock_encoder.startProbe()
time.sleep(0.5)
manual_mode = False
yaw_cmd = 0.0
pitch_cmd = 0.0
knee_cmd = 0.0
while run_flag:
time_0 = time.time()
if int(time_0 * 200.0) != int(time_1 * 200.0):
leg1.tick()
a = pitch_encoder.getAngle()
pitch_filt.update(a)
a = yaw_encoder.getAngle()
yaw_filt.update(a)
lr = update(time_0, yaw_filt.getVal(), pitch_filt.getVal(), knee_encoder.getAngle(), shock_encoder.getPosition())
if not manual_mode:
# if abs(lr[0])>1 or abs(lr[1])>1 or abs(lr[2])>1:
# yaw_valve.setPWM(lr[0])
# pitch_valve.setPWM(lr[1])
# knee_valve.setPWM(lr[2])
# else:
# yaw_cmd_filt.update( lr[0] )
# yaw_valve.setLengthRate(yaw_cmd_filt.getVal())
# print "%.3f, %.3f"%(lr[0], yaw_cmd_filt.getVal())
yaw_valve.setLengthRate(lr[0])
# error = controller.getDesiredYaw()-yaw_filt.getVal()
# print 'Error: %.2f'%(error)
y_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
z_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
r_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
while True:
s.step()
global_time.updateTime(s.getSimTime())
if s.getSimTime() - last_t > .001:
x = -(stick.get_axis(1) + .15466)
if x < 0:
x *= 2
x /= .7
y = -(stick.get_axis(0) + .15466)
if y < 0:
y *= 2
y /= .7
z = -(stick.get_axis(3) - .29895)
if z < 0:
z *= 2
rot = stick.get_axis(2) + .226806
x = x_low.update(x)
y = y_low.update(y)
z = z_low.update(z)
rot = r_low.update(rot)
print x, y, z, rot
s.robot.constantSpeedWalkSmart(x_scale=x,
y_scale=y,
z_scale=z,
rot_scale=rot)
last_t = s.getSimTime()
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 JointController(object):
"""
Produces a linear rate trying to control an angle.
Requires a description of a joint in the form of an ActuatorMathHelper
"""
def __init__(self,
act_math,
kp=0,
ki=0,
kd=0,
kff=None,
kfa=None,
derivative_corner=10,
backlash=0.0):
"""
act_math is the ActuatorMathHelper that contains the description of the joint
and actuator
kp = proportional gain term
ki = integral gain term
kd = derivative gain term
kff= feed forward velocity term. Applies signal relative to dervative of the target
derivative_corner = corner frequency of derivative filter.
Our real world signals are too noisy to use without significant filtering
kfa= feed forward acceleration term. Applies signal relative to the difference in
rate of change of the target and desired.
backlash = if we are within this angle of the target, we apply no corrective signal.
"""
self.act_math = act_math
self.updateGainConstants(kp, ki, kd, kff, kfa)
self.backlash = backlash
# NOTE: it is important that these three variables are floating point to avoid truncation
self.prev_error = 0.0
self.prev_desired_ang = 0.0
self.prev_ang = 0.0
self.integral_error_accumulator = 0.0
self.d_lowpass = LowPassFilter(gain=1.0,
corner_frequency=derivative_corner)
self.arate_lowpass = LowPassFilter(gain=1.0,
corner_frequency=derivative_corner)
def updateGainConstants(self, kp, ki, kd, kff=None, kfa=None):
self.kp = kp
self.ki = ki
self.kd = kd
if kff != None:
self.kff = kff
else:
self.kff = 0.0
if kfa != None:
self.kfa = kfa
else:
self.kfa = 0.0
def getAngleRate(self):
return self.arate_lowpass.getVal()
def getLengthRate(self):
self.act_math.setAngle(self.prev_ang)
self.act_math.setAngleRate(self.arate_lowpass.getVal())
#self.act_math.setAngleRate( self.measured_deriv )
present_length_rate = self.act_math.getLengthRate()
return present_length_rate
def getDesiredLengthRate(self):
self.act_math.setAngle(self.prev_ang)
self.act_math.setAngleRate(self.desired_vel)
desired_length_rate = self.act_math.getLengthRate()
return desired_length_rate
def update(self, desired_ang, measured_ang):
delta_time = time_sources.global_time.getDelta()
delta_ang = measured_ang - self.prev_ang
self.prev_ang = measured_ang
self.arate_lowpass.update(delta_ang / delta_time)
desired_vel = (desired_ang - self.prev_desired_ang) / delta_time
self.desired_vel = desired_vel
error = desired_ang - measured_ang
if abs(error) < self.backlash:
error = 0.0
else:
if error < 0.0:
error += self.backlash
else:
error -= self.backlash
self.integral_error_accumulator += self.ki * error * delta_time
derivative_error = self.d_lowpass.update(
(error - self.prev_error) / delta_time)
velocity_error = desired_vel - derivative_error
self.prev_error = error
self.prev_desired_ang = desired_ang
# Calculate a desired angle rate
control_ang_rate = self.kp * error + self.integral_error_accumulator + self.kd * derivative_error + self.kff * desired_vel + self.kfa * velocity_error
# Use joint geometry to determine a target piston length rate
self.act_math.setAngle(measured_ang)
self.act_math.setAngleRate(control_ang_rate)
target_length_rate = self.act_math.getLengthRate()
return target_length_rate
class JointController(object):
"""
Produces a linear rate trying to control an angle.
Requires a description of a joint in the form of an ActuatorMathHelper
"""
def __init__(self, act_math, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10, backlash=0.0):
"""
act_math is the ActuatorMathHelper that contains the description of the joint
and actuator
kp = proportional gain term
ki = integral gain term
kd = derivative gain term
kff= feed forward velocity term. Applies signal relative to dervative of the target
derivative_corner = corner frequency of derivative filter.
Our real world signals are too noisy to use without significant filtering
kfa= feed forward acceleration term. Applies signal relative to the difference in
rate of change of the target and desired.
backlash = if we are within this angle of the target, we apply no corrective signal.
"""
self.act_math = act_math
self.updateGainConstants(kp, ki, kd, kff, kfa)
self.backlash = backlash
# NOTE: it is important that these three variables are floating point to avoid truncation
self.prev_error = 0.0
self.prev_desired_ang = 0.0
self.prev_ang = 0.0
self.integral_error_accumulator = 0.0
self.d_lowpass = LowPassFilter( gain=1.0, corner_frequency=derivative_corner )
self.arate_lowpass = LowPassFilter( gain=1.0, corner_frequency=derivative_corner )
def updateGainConstants(self, kp, ki, kd, kff = None, kfa=None):
self.kp = kp
self.ki = ki
self.kd = kd
if kff != None:
self.kff = kff
else:
self.kff = 0.0
if kfa != None:
self.kfa = kfa
else:
self.kfa = 0.0
def getAngleRate( self ):
return self.arate_lowpass.getVal()
def getLengthRate( self ):
self.act_math.setAngle( self.prev_ang )
self.act_math.setAngleRate( self.arate_lowpass.getVal() )
#self.act_math.setAngleRate( self.measured_deriv )
present_length_rate = self.act_math.getLengthRate()
return present_length_rate
def getDesiredLengthRate( self ):
self.act_math.setAngle( self.prev_ang )
self.act_math.setAngleRate( self.desired_vel )
desired_length_rate = self.act_math.getLengthRate()
return desired_length_rate
def update(self, desired_ang, measured_ang):
delta_time = time_sources.global_time.getDelta()
delta_ang = measured_ang - self.prev_ang
self.prev_ang = measured_ang
self.arate_lowpass.update( delta_ang / delta_time )
desired_vel = (desired_ang - self.prev_desired_ang)/delta_time
self.desired_vel = desired_vel
error = desired_ang - measured_ang
if abs(error) < self.backlash:
error = 0.0
else:
if error < 0.0:
error += self.backlash
else:
error -= self.backlash
self.integral_error_accumulator += self.ki * error * delta_time
derivative_error = self.d_lowpass.update((error - self.prev_error) / delta_time)
velocity_error = desired_vel - derivative_error
self.prev_error = error
self.prev_desired_ang = desired_ang
# Calculate a desired angle rate
control_ang_rate = self.kp * error + self.integral_error_accumulator + self.kd * derivative_error + self.kff*desired_vel + self.kfa*velocity_error
# Use joint geometry to determine a target piston length rate
self.act_math.setAngle( measured_ang )
self.act_math.setAngleRate( control_ang_rate )
target_length_rate = self.act_math.getLengthRate()
return target_length_rate
yaw_encoder.startProbe()
pitch_encoder.startProbe()
knee_encoder.startProbe()
shock_encoder.startProbe()
time.sleep(0.5)
manual_mode = False
yaw_cmd = 0.0
pitch_cmd = 0.0
knee_cmd = 0.0
while run_flag:
time_0 = time.time()
if int(time_0 * 200.0) != int(time_1 * 200.0):
leg1.tick()
a = pitch_encoder.getAngle()
pitch_filt.update(a)
a = yaw_encoder.getAngle()
yaw_filt.update(a)
lr = update(time_0, yaw_filt.getVal(), pitch_filt.getVal(),
knee_encoder.getAngle(), shock_encoder.getPosition())
if not manual_mode:
# if abs(lr[0])>1 or abs(lr[1])>1 or abs(lr[2])>1:
# yaw_valve.setPWM(lr[0])
# pitch_valve.setPWM(lr[1])
# knee_valve.setPWM(lr[2])
# else:
# yaw_cmd_filt.update( lr[0] )
# yaw_valve.setLengthRate(yaw_cmd_filt.getVal())
# print "%.3f, %.3f"%(lr[0], yaw_cmd_filt.getVal())
yaw_valve.setLengthRate(lr[0])
# error = controller.getDesiredYaw()-yaw_filt.getVal()
