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 )
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()
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
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)
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
def __init__(self):
self.last_state = 0.0
self.vel_est = LowPassFilter(gain=1.0, corner_frequency=10.0)
if type(retval) == tuple:
update, controller = retval
else:
update = retval
controller = None
leg1 = ControlBus(device='/dev/ttyUSB1')
yaw_valve = ValveNode(leg1 , node_id=1 , name="yaw_valve" , bore=0.0254 , rod=0.0189 , lpm=22.712 , e_deadband=89 , r_deadband=75)
pitch_valve = ValveNode(leg1 , node_id=2 , name="pitch_valve" , bore=0.0381 , rod=0.0254 , lpm=22.712 , e_deadband=77 , r_deadband=70)
knee_valve = ValveNode(leg1 , node_id=3 , name="knee_valve" , bore=0.0254 , rod=0.0159 , lpm=22.712 , e_deadband=87 , r_deadband=69)
yaw_encoder = EncoderNode(leg1 , node_id=4 , name="yaw_encoder" , gain=1 , offset=35 * pi / 180 , bias=(1200 , 1200))
pitch_encoder = EncoderNode(leg1 , node_id=5 , name="pitch_encoder" , gain=-1 , offset=45 * pi / 180 , bias=(1200 , 1200))
knee_encoder = EncoderNode(leg1 , node_id=6 , name="knee_encoder" , gain=1 , offset=225 * pi / 180 , bias=(1200 , 1200))
shock_encoder = EncoderNode(leg1 , node_id=7 , name="shock_encoder" , gain=1 , offset=0 , bias=(1200 , 1200))
pitch_filt = LowPassFilter(gain=1.0, corner_frequency=20)
yaw_filt = LowPassFilter(gain=1.0, corner_frequency=20)
# yaw_cmd_filt = LowPassFilter( gain=1.0, corner_frequency=100 )
publisher = Publisher(5055)
publisher.addToCatalog('time', time.time)
publisher.addToCatalog('yaw_ang', yaw_encoder.getAngleDeg)
publisher.addToCatalog('yaw_ang_filt', lambda: 180 * yaw_filt.getVal() / pi)
publisher.addToCatalog('yaw_sin', yaw_encoder.getSinValue)
publisher.addToCatalog('yaw_cos', yaw_encoder.getCosValue)
publisher.addToCatalog('pitch_ang', pitch_encoder.getAngleDeg)
publisher.addToCatalog('pitch_ang_filt', lambda: 180 * pitch_filt.getVal() / pi)
publisher.addToCatalog('pitch_sin', pitch_encoder.getSinValue)
publisher.addToCatalog('pitch_cos', pitch_encoder.getCosValue)
publisher.addToCatalog('knee_ang', knee_encoder.getAngleDeg)
publisher.addToCatalog('knee_sin', knee_encoder.getSinValue)
print "Hats: ", stick.get_numhats()
print "Buttons: ", stick.get_numbuttons()
stick_neutrals = []
d = {'offset': (0, 0, 1.5)}
s = Simulator(dt=2e-3,
plane=1,
pave=0,
graphical=1,
ground_grade=.00,
robot=SpiderWHydraulics,
robot_kwargs=d,
render_objs=1,
draw_contacts=0)
last_t = 0
x_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
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
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
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
def __init__(self):
self.last_state = 0.0
self.vel_est = LowPassFilter(gain=1.0, corner_frequency=10.0)
offset=45 * pi / 180,
bias=(1200, 1200))
knee_encoder = EncoderNode(leg1,
node_id=6,
name="knee_encoder",
gain=1,
offset=225 * pi / 180,
bias=(1200, 1200))
shock_encoder = EncoderNode(leg1,
node_id=7,
name="shock_encoder",
gain=1,
offset=0,
bias=(1200, 1200))
pitch_filt = LowPassFilter(gain=1.0, corner_frequency=20)
yaw_filt = LowPassFilter(gain=1.0, corner_frequency=20)
# yaw_cmd_filt = LowPassFilter( gain=1.0, corner_frequency=100 )
publisher = Publisher(5055)
publisher.addToCatalog('time', time.time)
publisher.addToCatalog('yaw_ang', yaw_encoder.getAngleDeg)
publisher.addToCatalog('yaw_ang_filt', lambda: 180 * yaw_filt.getVal() / pi)
publisher.addToCatalog('yaw_sin', yaw_encoder.getSinValue)
publisher.addToCatalog('yaw_cos', yaw_encoder.getCosValue)
publisher.addToCatalog('pitch_ang', pitch_encoder.getAngleDeg)
publisher.addToCatalog('pitch_ang_filt',
lambda: 180 * pitch_filt.getVal() / pi)
publisher.addToCatalog('pitch_sin', pitch_encoder.getSinValue)
publisher.addToCatalog('pitch_cos', pitch_encoder.getCosValue)
publisher.addToCatalog('knee_ang', knee_encoder.getAngleDeg)
