def setup(self):
blocker = Blocker()
blocks = {
"left": pose.BlockLeft(),
"right": pose.BlockRight(),
"center": pose.Squat() # Does not get up after squatting
}
for name in blocks:
b = blocks[name]
stand = pose.Stand()
# Time less than 6 turns off robot after squatting
#self.add_transition(blocker, S(name), b, T(3.25), blocker)
self.add_transition(blocker, S(name), b, T(6.0), blocker)
def run(self):
commands.setHeadTilt()
commands.setStiffness()
if behavior_mem.test_odom_new:
self.otimer.reset()
behavior_mem.test_odom_new = False
if self.otimer.elapsed() > behavior_mem.test_odom_walk_time:
if self.stance != core.Poses.SITTING:
self.stance = core.Poses.SITTING
return pose.Sit()
return
vel_x = behavior_mem.test_odom_fwd
vel_y = behavior_mem.test_odom_side
vel_theta = behavior_mem.test_odom_turn
reqstance = behavior_mem.test_stance
if reqstance != self.stance:
self.stance = reqstance
if reqstance == core.Poses.SITTING:
return pose.Sit()
elif not util.isStanding():
return pose.Stand()
if self.stance == core.Poses.STANDING:
commands.setWalkVelocity(vel_x, vel_y, vel_theta)
def __init__(self):
HeadBodyTask.__init__(self,
head.Scan(period = 3.0, maxPan = 105.0 * core.DEG_T_RAD, numSweeps = 4),
pose.Stand()
)
def __init__(self):
super(StandNode, self).__init__()
self.task = pose.Stand()