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)
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
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)
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)
