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