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 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 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 __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 __init__(self, body_model, body_controller, leg_indices, delta_heights,
max_velocity, acceleration):
logger.info("New path.",
path_name="TrapezoidalFeetLiftLower",
leg_indices=leg_indices,
delta_height=delta_heights,
max_velocity=max_velocity,
acceleration=acceleration)
self.leg_indices = leg_indices
self.model = body_model
self.controller = body_controller
self.final_joint_positions = self.controller.getTargetJointAngleMatrix(
)
self.foot_paths = [None for i in range(NUM_LEGS)]
if isinstance(delta_heights, (int, long, float, complex)):
delta_heights = [delta_heights] * 6
for i in self.leg_indices:
current_leg_pos = self.model.getLegs()[i].footPosFromLegState(
[self.controller.getTargetJointAngleMatrix()[i],
0]) # self.model.getFootPositions()[i]
desired_leg_pos = current_leg_pos + [0, 0, delta_heights[i]]
self.foot_paths[i] = TrapezoidalFootMove(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i], desired_leg_pos,
max_velocity, acceleration)
self.done = False
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, 5.0)
# Monitor leg_paths
if path.isDone():
if state == S_INIT:
print "Move" * 1000
path = TrapezoidalFootMove(model, controller,
array([1.5, 0.0, -0.4]), 0.5, 0.5)
state = S_MOVE_JOINT
elif state == S_MOVE_JOINT:
print "Done" * 1000
state = S_DONE
pass
logger.info("State changed.", state=state)
print "Foot:", model.getFootPos()
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def __init__(self, body_model, body_controller, final_height, max_velocity,
acceleration):
logger.info("New path.",
path_name="TrapezoidalSitStand",
final_height=final_height,
max_velocity=max_velocity,
acceleration=acceleration)
self.model = body_model
self.controller = body_controller
self.final_foot_positions = self.model.getFootPositions()
self.feet_path = []
for i in range(NUM_LEGS):
self.final_foot_positions[i][2] = final_height
for i in range(NUM_LEGS):
self.feet_path = append(
self.feet_path,
TrapezoidalFootMove(self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.final_foot_positions[i], max_velocity,
acceleration))
self.done = False
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
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([2.0, 0.0, -0.2]),
0.4, 0.2)
self.controller.update(self.model.getJointAngles(), self.initial_path.update())
if self.initial_path.isDone():
self.callback = self.pause
return self.controller.getLengthRateCommands()
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
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)
# Monitor leg_paths
if path.isDone():
if state == S_MOVE3:
path = TrapezoidalFootMove(model, controller,
array([1.5, -0.6, -0.4]), 0.2, 0.1)
state = S_MOVE1
elif state == S_MOVE1:
path = PutFootOnGround(model, controller, 0.05)
state = S_LOWER
elif state == S_LOWER:
ground_level = model.getFootPos()[Z]
path = TrapezoidalFootMove(model, controller,
array([1.5, .6, ground_level]), 0.2,
0.1)
state = S_MOVE2
elif state == S_MOVE2:
path = TrapezoidalFootMove(model, controller,
array([1.5, 0.6, -0.4]), 0.2, 0.1)
state = S_MOVE3
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def __init__(self, body_model, body_controller, max_velocity=2.0, acceleration=1.0):
leg_logger.logger.info("New path.",
path_name="GoToStandardHexagon",
max_velocity=max_velocity,
acceleration=acceleration)
self.model = body_model
self.controller = body_controller
self.max_velocity = max_velocity
self.acceleration = acceleration
self.final_joint_positions = self.controller.getTargetJointAngleMatrix()
self.foot_paths = [None for i in range(NUM_LEGS)]
self.lifted = array([1.651 , 0 , 0])
self.lowered = array([1.651 , 0 , -1.6764])
self.NONE = 0
self.LOWER_ALL = 1
self.EVENS = 2
self.LOWER_EVENS = 3
self.ODDS = 4
self.LOWER_ODDS = 5
self.STAND = 6
self.state = self.LOWER_ALL
self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()]
self.in_place = [False] * 6
# Put all 6 legs in place
for i in range(NUM_LEGS):
if not self.on_ground[i] and not self.in_place[i]:
self.foot_paths[i] = TrapezoidalFootMove(self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.lifted,
self.max_velocity, self.acceleration)
self.in_place[i] = True
self.phase_done = False
self.done = False
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global points, path, index
# 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)
if path.isDone():
path = TrapezoidalFootMove(model, controller, array(points[index]),
0.5, 0.4)
index = (index + 1) % len(points)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
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 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
def update(self):
active_paths = []
active_index = []
for i in range(NUM_LEGS):
if self.foot_paths[i] is not None:
active_index.append(i)
active_paths.append(self.foot_paths[i])
if self.phase_done:
self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()]
if self.state == self.LOWER_ALL:
for i in range(NUM_LEGS):
if not self.on_ground[i]:
self.foot_paths[i] = PutFootOnGround(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.max_velocity / 5)
self.in_place[i] = True
self.state = self.EVENS
if all(self.in_place):
self.state = self.STAND
self.phase_done = False
elif self.state == self.EVENS:
for i in range(NUM_LEGS):
if not i % 2 and not self.in_place[i]:
self.foot_paths[i] = TrapezoidalFootMove(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.lifted,
self.max_velocity, self.acceleration)
self.state = self.LOWER_EVENS
self.phase_done = False
elif self.state == self.LOWER_EVENS:
for i in range(NUM_LEGS):
if not self.on_ground[i]:
self.foot_paths[i] = PutFootOnGround(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.max_velocity / 5)
self.in_place[i] = True
self.state = self.ODDS
self.phase_done = False
elif self.state == self.ODDS:
for i in range(NUM_LEGS):
if i % 2 and not self.in_place[i]:
self.foot_paths[i] = TrapezoidalFootMove(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.lifted,
self.max_velocity, self.acceleration)
self.state = self.LOWER_ODDS
self.phase_done = False
elif self.state == self.LOWER_ODDS:
for i in range(NUM_LEGS):
if not self.on_ground[i]:
self.foot_paths[i] = PutFootOnGround(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.max_velocity / 5)
self.in_place[i] = True
self.state = self.STAND
self.phase_done = False
elif self.state == self.STAND:
for i in range(NUM_LEGS):
self.foot_paths[i] = TrapezoidalFootMove(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.lowered,
self.max_velocity, self.acceleration * .1)
self.state = self.NONE
self.phase_done = False
elif self.state == self.NONE:
self.done = True
return self.final_joint_positions
elif not self.phase_done:
#logically and all of the isdone results from the joint move paths
self.phase_done = all(map(lambda p: p.isDone(), active_paths))
if not self.done:
for i in active_index:
self.final_joint_positions[i] = self.foot_paths[i].update()
return self.final_joint_positions
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()
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
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()