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 Gait(object):
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]
def update( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
# Update model
time_sources.global_time.updateTime(time)
self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
self.model.updateFootOnGround()
# Init path. Do this after the first update.
if self.path is None:
self.path = Pause(self.model, controller, 1.0)
if self.path.isDone():
self.path = TrapezoidalFootMove(self.model, controller,
array(self.points[self.index][0:3]),
self.points[self.index][3], 0.4)
self.index = (self.index+1)%len(self.points)
# Evaluate path and joint control
present_angles = self.model.getJointAngles()
target_angles = self.path.update()
controller.update(present_angles, target_angles)
control_lin_rates = [self.joint_controllers[i].update(target_angles[i], present_angles[i]) for i in range(3)]
target_lin_rates = [c.getDesiredLengthRate() for c in self.joint_controllers]
measured_lin_rates = [c.getLengthRate() for c in self.joint_controllers]
if 0:
s = ''
m = self.pist_helpers[0].clay_pit_pos.mem
for i in range(len(m)):
if not i%8:
s += '%2.2f '%(m[i])
print s
commands = [self.pist_helpers[i].update(control_lin_rates[i], measured_lin_rates[i]) for i in range(3)]
#commands = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]
if 0:
target_ang_rates = [c.desired_vel for c in self.joint_controllers]
measured_ang_rates = [c.getAngleRate() for c in self.joint_controllers]
s = 'Measured: '
for m in measured_ang_rates:
s += '%1.3f '%(m*180/pi)
print s
s = 'Target: '
for m in target_ang_rates:
s += '%1.3f '%(m*180/pi)
print s
s = 'M Lin'
for m in measured_lin_rates:
s += '%1.3f '%m
print s
s = 'T Lin'
for m in target_lin_rates:
s += '%1.3f '%m
print s
self.pist_helpers[0].clay_pit_pos.printpit()
return commands
def gracefulExit(self):
for i in range(3):
self.pist_helpers[i].saveState()
class SafeMove:
"""This is foot movement that combines up to two trapezoidal foot moves and up to one trapezoidal joint move to move the foot
safely without fear of dragging on the ground.
NOTE: The Z-plane is the highest horizontal plane that ensures a convex volume below it.
This is the boundary of where TrapFootMove is guaranteed to work, so we switch to TrapJointMove above it.
It should be found via side-angle-side of the knee and calf links and the angle they make most folded up.
We use only vertical motions below the Z-plane, as there is a high likelihood of hitting the ground.
"""
def __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration, z_plane=-0.67865):
leg_logger.logger.info("New path.", path_name="TrapezoidalFootMove",
final_foot_pos=final_foot_pos, max_velocity=max_velocity,
acceleration=acceleration)
self.model = leg_model
self.controller = limb_controller
self.final_foot_pos = final_foot_pos
self.final_joint_angles = self.model.jointAnglesFromFootPos(final_foot_pos, shock_depth=0.0)
self.max_vel = max_velocity
self.vel = 0.0
self.acc = acceleration
self.z_plane = z_plane
self.initial_foot_pos = self.model.footPosFromLegState([self.controller.getDesiredPosAngle(), 0])
self.initial_joint_angles = self.controller.getDesiredPosAngle()
self.target_angles = self.initial_joint_angles
self.projected_initial = [self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane]
self.projected_final = [self.final_foot_pos[0], self.final_foot_pos[1], max(self.final_foot_pos[2],z_plane)]
self.done = False
self.LIFT = 1
self.TRANSLATE = 2
self.LOWER = 3
self.state = self.LIFT
self.path = TrapezoidalFootMove(self.model, self.controller, self.projected_initial, self.max_vel, self.acc)
if self.initial_foot_pos[2] > self.z_plane:
self.state = self.TRANSLATE
self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
def isDone(self):
return self.done
def update(self):
if not self.isDone():
self.target_angles = self.path.update()
self.done = self.path.isDone()
if self.isDone():
if self.state == self.LIFT:
self.state = self.TRANSLATE
self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
self.done = False
elif self.state == self.TRANSLATE and self.final_foot_pos[2] < self.z_plane:
self.state = self.LOWER
self.path = TrapezoidalFootMove(self.model, self.controller, self.final_foot_pos, self.max_vel, self.acc)
self.done = False
return self.target_angles
class Gait(object):
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]
def update(self,
time,
yaw,
hip_pitch,
knee_pitch,
shock_depth,
command=None):
# Update model
time_sources.global_time.updateTime(time)
self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
self.model.updateFootOnGround()
# Init path. Do this after the first update.
if self.path is None:
self.path = Pause(self.model, controller, 1.0)
if self.path.isDone():
self.path = TrapezoidalFootMove(
self.model, controller, array(self.points[self.index][0:3]),
self.points[self.index][3], 0.4)
self.index = (self.index + 1) % len(self.points)
# Evaluate path and joint control
present_angles = self.model.getJointAngles()
target_angles = self.path.update()
controller.update(present_angles, target_angles)
control_lin_rates = [
self.joint_controllers[i].update(target_angles[i],
present_angles[i])
for i in range(3)
]
target_lin_rates = [
c.getDesiredLengthRate() for c in self.joint_controllers
]
measured_lin_rates = [
c.getLengthRate() for c in self.joint_controllers
]
if 0:
s = ''
m = self.pist_helpers[0].clay_pit_pos.mem
for i in range(len(m)):
if not i % 8:
s += '%2.2f ' % (m[i])
print s
commands = [
self.pist_helpers[i].update(control_lin_rates[i],
measured_lin_rates[i])
for i in range(3)
]
#commands = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]
if 0:
target_ang_rates = [c.desired_vel for c in self.joint_controllers]
measured_ang_rates = [
c.getAngleRate() for c in self.joint_controllers
]
s = 'Measured: '
for m in measured_ang_rates:
s += '%1.3f ' % (m * 180 / pi)
print s
s = 'Target: '
for m in target_ang_rates:
s += '%1.3f ' % (m * 180 / pi)
print s
s = 'M Lin'
for m in measured_lin_rates:
s += '%1.3f ' % m
print s
s = 'T Lin'
for m in target_lin_rates:
s += '%1.3f ' % m
print s
self.pist_helpers[0].clay_pit_pos.printpit()
return commands
def gracefulExit(self):
for i in range(3):
self.pist_helpers[i].saveState()
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 SafeMove:
"""
This is foot movement that combines up to two trapezoidal foot moves and up to one trapezoidal joint
move to move the foot safely without fear of dragging on the ground.
NOTE: The Z-plane is the highest horizontal plane that ensures a convex volume below it.
This is the boundary of where TrapFootMove is guaranteed to work, so we switch to TrapJointMove above it.
It should be found via side-angle-side of the knee and calf links and the angle they make most folded up.
We use only vertical motions below the Z-plane, as there is a high likelihood of hitting the ground.
"""
def __init__(self,
leg_model,
limb_controller,
final_foot_pos,
max_velocity,
acceleration,
z_plane=-0.67865):
leg_logger.logger.info("New path.",
path_name="TrapezoidalFootMove",
final_foot_pos=final_foot_pos,
max_velocity=max_velocity,
acceleration=acceleration)
self.model = leg_model
self.controller = limb_controller
self.final_foot_pos = final_foot_pos
self.final_joint_angles = self.model.jointAnglesFromFootPos(
final_foot_pos, shock_depth=0.0)
self.max_vel = max_velocity
self.vel = 0.0
self.acc = acceleration
self.z_plane = z_plane
self.initial_foot_pos = self.model.footPosFromLegState(
[self.controller.getDesiredPosAngle(), 0])
self.initial_joint_angles = self.controller.getDesiredPosAngle()
self.target_angles = self.initial_joint_angles
self.projected_initial = [
self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane
]
self.projected_final = [
self.final_foot_pos[0], self.final_foot_pos[1],
max(self.final_foot_pos[2], z_plane)
]
self.done = False
self.LIFT = 1
self.TRANSLATE = 2
self.LOWER = 3
self.state = self.LIFT
self.path = TrapezoidalFootMove(self.model, self.controller,
self.projected_initial, self.max_vel,
self.acc)
if self.initial_foot_pos[2] > self.z_plane:
self.state = self.TRANSLATE
self.path = TrapezoidalJointMove(
self.model, self.controller,
self.model.jointAnglesFromFootPos(self.projected_final),
self.max_vel, self.acc)
def isDone(self):
return self.done
def update(self):
if not self.isDone():
self.target_angles = self.path.update()
self.done = self.path.isDone()
if self.isDone():
if self.state == self.LIFT:
self.state = self.TRANSLATE
self.path = TrapezoidalJointMove(
self.model, self.controller,
self.model.jointAnglesFromFootPos(self.projected_final),
self.max_vel, self.acc)
self.done = False
elif self.state == self.TRANSLATE and self.final_foot_pos[
2] < self.z_plane:
self.state = self.LOWER
self.path = TrapezoidalFootMove(self.model, self.controller,
self.final_foot_pos,
self.max_vel, self.acc)
self.done = False
return self.target_angles