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 = TrapezoidalJointMove(model, controller, final_angles=[0, -0.59483773, 1.81300376],
max_velocity=1, acceleration=.1) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = S_MOVE_JOINT
elif state == S_MOVE_JOINT:
print "Done"*1000
state = S_DONE
pass
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
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 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 = TrapezoidalJointMove(
model,
controller,
final_angles=[0, -0.59483773, 1.81300376],
max_velocity=1,
acceleration=.1
) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = S_MOVE_JOINT
elif state == S_MOVE_JOINT:
print "Done" * 1000
state = S_DONE
pass
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
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 pause(self,
time,
yaw,
hip_pitch,
knee_pitch,
shock_depth,
command=None):
if not hasattr(self, 'pause_path'):
print "Waiting..."
self.pause_path = Pause(self.model, self.controller, 3.0)
self.controller.update(self.model.getJointAngles(),
self.pause_path.update())
if self.pause_path.isDone():
self.callback = self.discoverDeadband
del self.pause_path
return [0, 0, 0]
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update leg 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)
path.initial_angles[HP] = -0.6
# Monitor model_path
if path.isDone():
if state == S_INIT:
print "Move" * 1000
path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=1, velocity=0.2)
state = S_MOVE1
elif state == S_MOVE1:
print "Move" * 1000
path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=-1, velocity=0.2)
state = S_MOVE2
elif state == S_MOVE2:
print "Done" * 1000
state = S_INIT
pass
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state, ja
# 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:
ja = model.getJointAngles()
path = Pause(model, controller, 1.0)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def pause(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
if not hasattr(self, 'pause_path'):
print "Waiting..."
self.pause_path = Pause(self.model, self.controller, 3.0)
self.controller.update(self.model.getJointAngles(), self.pause_path.update())
if self.pause_path.isDone():
self.callback = self.discoverDeadband
del self.pause_path
return [0, 0, 0]
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state, ja
# 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:
ja = model.getJointAngles()
path = Pause(model, controller, 1.0)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
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()
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update leg 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)
path.initial_angles[HP] = -0.6
# Monitor model_path
if path.isDone():
if state == S_INIT:
print "Move"*1000
path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=1, velocity=0.2)
state = S_MOVE1
elif state == S_MOVE1:
print "Move"*1000
path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=-1, velocity=0.2)
state = S_MOVE2
elif state == S_MOVE2:
print "Done"*1000
state = S_INIT
pass
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
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()
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:
path = SafeMove(
model, controller, [2, 1, .5], 1, .1, 0
) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = S_MOVE_JOINT
elif state == S_MOVE_JOINT:
path = SafeMove(
model, controller, [1.5, -.5, -1], 1, .1, 0
) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = 3
elif state == 3:
path = SafeMove(
model, controller, [1.5, .5, -1], 1, .1, 0
) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = 4
elif state == 4:
path = SafeMove(
model, controller, [2, -1, .5], 1, .1, 0
) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = S_INIT
elif state == 5:
state = S_DONE
pass
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, duration):
logger.info("New path.", path_name="BodyPause", duration=duration)
self.body = body_model
self.body_controller = body_controller
self.duration = duration
self.feet_path = []
current_positions = self.body.getFootPositions()
for i in range(NUM_LEGS):
self.feet_path = append(
self.feet_path,
Pause(self.body.getLegs()[i],
self.body_controller.getLimbControllers()[i],
self.duration))
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, 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 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 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:
path = SafeMove(model, controller, [2, 1, .5], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = S_MOVE_JOINT
elif state == S_MOVE_JOINT:
path = SafeMove(model, controller, [1.5, -.5, -1], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = 3
elif state == 3:
path = SafeMove(model, controller, [1.5, .5, -1], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = 4
elif state == 4:
path = SafeMove(model, controller, [2, -1, .5], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
state = S_INIT
elif state == 5:
state = S_DONE
pass
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
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 = .03
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):
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([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 pause(self,
time,
yaw,
hip_pitch,
knee_pitch,
shock_depth,
command=None):
if not hasattr(self, 'pause_path'):
print "Waiting..."
self.pause_path = Pause(self.model, self.controller, 3.0)
self.controller.update(self.model.getJointAngles(),
self.pause_path.update())
if self.pause_path.isDone():
self.callback = self.discoverDeadband
del self.pause_path
return [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()
exit()
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]
# 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 = 1
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)
self.pwm_val += dt * 15
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 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 = .03
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):
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([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 pause(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
if not hasattr(self, 'pause_path'):
print "Waiting..."
self.pause_path = Pause(self.model, self.controller, 3.0)
self.controller.update(self.model.getJointAngles(), self.pause_path.update())
if self.pause_path.isDone():
self.callback = self.discoverDeadband
del self.pause_path
return [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()
exit()
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]
# 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 = 1
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)
self.pwm_val += dt * 15
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