def processFrame(self):
if self._subtask and self._subtask.finished() and self._queue:
UTdebug.log(97, "finalizing queued completion for %s" % (str(self)))
self.finish()
return
super(Node, self).processFrame()
if self.aborted(): return
def processFrame(self):
if self._subtask and self._subtask.finished() and self._queue:
UTdebug.log(97, "finalizing queued completion for %s" % (str(self)))
self.finish()
return
super(Node, self).processFrame()
if self.aborted(): return
def handle():
lines = traceback.format_exception(*(sys.exc_info()))
message = ''.join('!! ' + line for line in lines)
UTdebug.log(0, 'PYTHON ERROR:')
UTdebug.log(0, message)
core.speech.say("python")
core.pythonC.python_ok_ = False
def handle():
lines = traceback.format_exception(*(sys.exc_info()))
message = ''.join('!! ' + line for line in lines)
UTdebug.log(0,'PYTHON ERROR:')
UTdebug.log(0, message)
core.speech.say("python")
core.pythonC.python_ok_ = False
def checkTemperatures():
for i in range(core.NUM_JOINTS):
temp = sensors.getJointTemperature(i)
if (temp > 65):
# figure out stiffness
stiff = 1.0
if (temp > 75):
pct = (temp-75.0) / 10.0
stiff = 1-pct
if (stiff < 0): stiff = 0
UTdebug.log(10, "Joint ", i, core.getJointName(i), " has temperature ", temp, " stiffness: ", stiff)
def checkTemperatures():
for i in range(core.NUM_JOINTS):
temp = sensors.getJointTemperature(i)
if (temp > 65):
# figure out stiffness
stiff = 1.0
if (temp > 75):
pct = (temp - 75.0) / 10.0
stiff = 1 - pct
if (stiff < 0): stiff = 0
UTdebug.log(10, "Joint ", i, core.getJointName(i),
" has temperature ", temp, " stiffness: ", stiff)
def handle():
lines = traceback.format_exception(*(sys.exc_info()))
message = ''.join('!! ' + line for line in lines)
if core.instance.type_ == core.CORE_TOOLSIM:
print message
else:
UTdebug.log(0, 'PYTHON ERROR:')
UTdebug.log(0, message)
memory.speech.say("python")
core.pythonC.is_ok_ = False
with open(os.path.expanduser('~/error.txt'), 'w') as f:
f.write(message)
def run(self):
ball = mem_objects.world_objects[core.WO_BALL]
commands.setHeadPan(ball.bearing, 0.1)
if ball.distance < 500:
UTdebug.log(15, "Ball is close, blocking!")
if ball.bearing > 30 * core.DEG_T_RAD:
choice = "left"
elif ball.bearing < -30 * core.DEG_T_RAD:
choice = "right"
else:
choice = "center"
self.postSignal(choice)
def handle():
lines = traceback.format_exception(*(sys.exc_info()))
message = ''.join('!! ' + line for line in lines)
if core.instance.type_ == core.CORE_TOOLSIM:
print message
else:
UTdebug.log(0,'PYTHON ERROR:')
UTdebug.log(0, message)
memory.speech.say("python")
core.pythonC.is_ok_ = False
with open(os.path.expanduser('~/error.txt'), 'w') as f:
f.write(message)
def run(self):
dt = self.getTime() - self.prevTime
# if not ball.seen, move the head back to the center
ball = mem_objects.world_objects[core.WO_BALL]
if not ball.seen:
commands.setHeadPan(0.0, 0.2)
else:
commands.setHeadPan(ball.bearing, 0.2)
# print("Ball distance: %f Ball bearing: %f\n" % (ball.distance,ball.bearing))
v_mag = math.sqrt(ball.relVel.x * ball.relVel.x +
ball.relVel.y * ball.relVel.y)
x_ball = ball.distance * math.cos(ball.bearing) / 10.0
y_ball = ball.distance * math.sin(ball.bearing) / 10.0
delta_t = 2.0
v_thresh = -(x_ball / delta_t)
print("V Thresh: %f" % (v_thresh))
disp = odometry.displacement
if not dt == 0:
print("Disp: [%f, %f, %f]" %
(0.1 * disp.translation.x / dt,
0.1 * disp.translation.y / dt, 0.1 * disp.rotation / dt))
# print("X: %f, Y: %f, VThresh: %f, VMag: %f, Vx: %f, Vy: %f" % (x_ball, y_ball, v_thresh, v_mag, ball.relVel.x, ball.relVel.y))
self.prevTime = self.getTime()
if ball.relVel.x < v_thresh and ball.seen and ball.relVel.x < 0.0:
# ball moving away from the robot, reset to regular position
print("Current Vx: %f, Current Vy: %f, x ball: %f, y ball: %f" %
(ball.relVel.x, ball.relVel.y, x_ball, y_ball))
y_intercept = -ball.relVel.y * x_ball / ball.relVel.x + y_ball
print("Y intercept: %f" % y_intercept)
## can do if xvel < yvel && xvel < 1, etc.
## maybe look at distance between robot and goal?
UTdebug.log(15, "Ball is close, blocking!")
if y_intercept > 45.0 or y_intercept < -45.0 or not ball.seen:
choice = "miss"
elif y_intercept > 18.5:
choice = "left"
elif y_intercept < -18.5:
choice = "right"
elif y_intercept <= 18.5 and y_intercept >= -18.5:
choice = "center"
self.postSignal(choice)
return
elif ball.seen:
choice = "moveBall"
# print("Move ball signal")
self.postSignal(choice)
return
else:
return
def run(self):
UTdebug.log(15, "Not Blocking")
ball = mem_objects.world_objects[core.WO_BALL]
commands.setStiffness(cfgstiff.OneArmsOnly)
commands.setHeadPan(ball.bearing, 0.2)
newPose = dict()
newPose[core.LShoulderRoll] = 15
newPose[core.LShoulderPitch] = -100
newPose[core.LElbowRoll] = 0
newPose[core.RShoulderRoll] = 15
newPose[core.RShoulderPitch] = -100
newPose[core.RElbowRoll] = 0
newPose[core.RHipPitch] = -46.5
newPose[core.LHipPitch] = -46.5
return #pose.ToPoseMoveHead(pose=newPose, tilt=-15, pan = ball.bearing, time=0.5)
def setFallCounter():
tilt = sensors.getValue(core.angleY)
roll = sensors.getValue(core.angleX)
tiltFalling = core.DEG_T_RAD * 45
rollFalling = core.DEG_T_RAD * 45
if (abs(tilt) > tiltFalling):
if tilt > 0:
# on stomach
if walk_request.tilt_fallen_counter_ < 0:
walk_request.tilt_fallen_counter_ = 1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ + 1
else:
# on back
if walk_request.tilt_fallen_counter_ > 0:
walk_request.tilt_fallen_counter_ = -1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ - 1
else:
walk_request.tilt_fallen_counter_ = 0
if (abs(roll) > rollFalling):
if roll > 0:
# on stomach
if walk_request.roll_fallen_counter_ < 0:
walk_request.roll_fallen_counter_ = 1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ + 1
else:
# on back
if walk_request.roll_fallen_counter_ > 0:
walk_request.roll_fallen_counter_ = -1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ - 1
else:
walk_request.roll_fallen_counter_ = 0
UTdebug.log(10, "fall counter: ", walk_request.tilt_fallen_counter_,
walk_request.roll_fallen_counter_)
return tilt, roll, tiltFalling, rollFalling
def setRobotSpecificKickParams():
id = robot_state.robot_id_
if id == 92: # Gouda
UTdebug.log(0,'Gouda Parameters')
#StraightKick.ideal_ball_side_right_swing_ = 20
else:
UTdebug.log(0,'*** No Robot specific kick params')
UTdebug.log(0,'Done setting robot specific kick params')
def setRobotSpecificKickParams():
id = robot_state.robot_id_
if id == 92: # Gouda
UTdebug.log(0, 'Gouda Parameters')
#StraightKick.ideal_ball_side_right_swing_ = 20
else:
UTdebug.log(0, '*** No Robot specific kick params')
UTdebug.log(0, 'Done setting robot specific kick params')
def setFallCounter():
tilt = sensors.getValue(core.angleY)
roll = sensors.getValue(core.angleX)
tiltFalling = core.DEG_T_RAD * 45
rollFalling = core.DEG_T_RAD * 45
if (abs(tilt) > tiltFalling):
if tilt > 0:
# on stomach
if walk_request.tilt_fallen_counter_ < 0:
walk_request.tilt_fallen_counter_ = 1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ + 1
else:
# on back
if walk_request.tilt_fallen_counter_ > 0:
walk_request.tilt_fallen_counter_ = -1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ - 1
else:
walk_request.tilt_fallen_counter_ = 0
if (abs(roll) > rollFalling):
if roll > 0:
# on stomach
if walk_request.roll_fallen_counter_ < 0:
walk_request.roll_fallen_counter_ = 1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ + 1
else:
# on back
if walk_request.roll_fallen_counter_ > 0:
walk_request.roll_fallen_counter_ = -1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ - 1
else:
walk_request.roll_fallen_counter_ = 0
UTdebug.log(10, "fall counter: ", walk_request.tilt_fallen_counter_, walk_request.roll_fallen_counter_)
return tilt,roll,tiltFalling,rollFalling
def run(self):
ball = mem_objects.world_objects[core.WO_BALL]
# commands.setStiffness()
# print('x ', ball.px,' y ', ball.py)
if ball.seen:
# print("updated position estimate: ",ball.px, " ", ball.py)
# print("updated velocity estimate: ",ball.relVel.x, " ", ball.relVel.y)
self.xhat = [ball.loc.x,ball.loc.y,ball.relVel.x,ball.relVel.y]
commands.setHeadPan(ball.bearing, 0.15)
commands.setStiffness()
if ball.dirn < 3:
UTdebug.log(15, "Ball is close, blocking!")
if ball.dirn == 0:
print('Moving left arm')
choice = "left"
elif ball.dirn == 2:
choice = "center"
print('Moving both arms')
else:
choice = "right"
print('Moving right arm')
self.postSignal(choice)
def processFrame(self):
self._frames += 1
if UTdebug.TIME: UTdebug.stimer(str(self))
if self.aborted():
if UTdebug.TIME: UTdebug.ttimer(str(self))
return
if self._subtask and not self._subtask.finished():
self._subtask.processFrame()
else:
sub = self._startrun()
if sub:
if isinstance(sub, list): self.setChain(sub)
else: self.setSubtask(sub)
if UTdebug.TIME: UTdebug.ttimer(str(self))
def processFrame(self):
self._frames += 1
if UTdebug.TIME: UTdebug.stimer(str(self))
if self.aborted():
if UTdebug.TIME: UTdebug.ttimer(str(self))
return
if self._subtask and not self._subtask.finished():
self._subtask.processFrame()
else:
sub = self._startrun()
if sub:
if isinstance(sub, list): self.setChain(sub)
else: self.setSubtask(sub)
if UTdebug.TIME: UTdebug.ttimer(str(self))
def checkFallen():
tilt, roll, tiltFalling, rollFalling = setFallCounter()
#UTdebug.log(80,"acc:", accx, accy, accz, "fallen counter: ", walk_request.fallen_counter_)
UTdebug.log(80, "tilt,roll:", tilt, roll, "fallen counter: ",
walk_request.tilt_fallen_counter_,
walk_request.roll_fallen_counter_)
# Check if sensors think we have fallen
if (abs(walk_request.tilt_fallen_counter_) > 2
or abs(walk_request.roll_fallen_counter_) > 2):
state = game_state.state()
# set fall direction
if (roll > rollFalling):
odometry.fall_direction_ = core.Fall.RIGHT
#speech:say("right");
elif (roll < -rollFalling):
odometry.fall_direction_ = core.Fall.LEFT
#speech:say("left");
elif (tilt > tiltFalling):
odometry.fall_direction_ = core.Fall.FORWARD
#speech:say("forward");
elif (tilt < -tiltFalling):
odometry.fall_direction_ = core.Fall.BACKWARD
#speech:say("backward");
else:
# default option??
UTdebug.log(0, "default fall is forward")
odometry.fall_direction_ = core.Fall.FORWARD
#speech:say("default");
UTdebug.log(30, "fall detected in direction", odometry.fall_direction_,
tilt, roll)
# Get up only in playing or ready
if state in [core.READY, core.PLAYING, core.MANUAL_CONTROL]:
# and not keeper (keeper will decide on its own when to do get up)
if (robot_state.WO_SELF != core.KEEPER):
return True
else:
# if keeper is not diving, we can still call get up
if (behavior_mem.keeperDiving == core.Dive.NONE):
return True
else:
# wait for dive to tell us to get up
return False
odometry.fall_direction_ = core.Fall.NONE
return False
def checkFallen():
tilt,roll,tiltFalling,rollFalling = setFallCounter()
#UTdebug.log(80,"acc:", accx, accy, accz, "fallen counter: ", walk_request.fallen_counter_)
UTdebug.log(80,"tilt,roll:", tilt, roll, "fallen counter: ", walk_request.tilt_fallen_counter_, walk_request.roll_fallen_counter_)
# Check if sensors think we have fallen
if (abs(walk_request.tilt_fallen_counter_) > 2 or abs(walk_request.roll_fallen_counter_) > 2):
state = game_state.state()
# set fall direction
if (roll > rollFalling):
odometry.fall_direction_ = core.Fall.RIGHT
#speech:say("right");
elif (roll < -rollFalling):
odometry.fall_direction_ = core.Fall.LEFT
#speech:say("left");
elif (tilt > tiltFalling):
odometry.fall_direction_ = core.Fall.FORWARD
#speech:say("forward");
elif (tilt < -tiltFalling):
odometry.fall_direction_ = core.Fall.BACKWARD
#speech:say("backward");
else:
# default option??
UTdebug.log(0, "default fall is forward")
odometry.fall_direction_ = core.Fall.FORWARD
#speech:say("default");
UTdebug.log(30, "fall detected in direction", odometry.fall_direction_, tilt, roll)
# Get up only in playing or ready
if state in [core.READY, core.PLAYING, core.MANUAL_CONTROL]:
# and not keeper (keeper will decide on its own when to do get up)
if (robot_state.WO_SELF != core.KEEPER):
return True
else:
# if keeper is not diving, we can still call get up
if (behavior_mem.keeperDiving == core.Dive.NONE):
return True
else:
# wait for dive to tell us to get up
return False
odometry.fall_direction_ = core.Fall.NONE
return False
def processFrame(self):
self._frames += 1
if core.instance.type_ == core.CORE_TOOL:
self.trace("process frame")
if UTdebug.TIME: UTdebug.stimer(str(self))
if self.aborted():
if UTdebug.TIME: UTdebug.ttimer(str(self))
return
someFinished = False
allFinished = True
for s in self._subtasks:
if not s:
someFinished = True
elif s.finished():
someFinished = True
else:
allFinished = False
s.processFrame()
if someFinished and self.idpChains: self._startrun()
elif allFinished: self._startrun()
if UTdebug.TIME: UTdebug.ttimer(str(self))
def processFrame(self):
self._frames += 1
if core.instance.type_ == core.CORE_TOOL:
self.trace("process frame")
if UTdebug.TIME: UTdebug.stimer(str(self))
if self.aborted():
if UTdebug.TIME: UTdebug.ttimer(str(self))
return
someFinished = False
allFinished = True
for s in self._subtasks:
if not s:
someFinished = True
elif s.finished():
someFinished = True
else:
allFinished = False
s.processFrame()
if someFinished and self.idpChains: self._startrun()
elif allFinished: self._startrun()
if UTdebug.TIME: UTdebug.ttimer(str(self))
def run(self):
UTdebug.log(15, "Blocking center")
return pose.Squat2()
def trace(self, msg): UTdebug.taskTrace(msg, self)
def trace(self, msg):
UTdebug.taskTrace(msg, self)
def processFrame(self):
self._frames += 1
if UTdebug.TIME: UTdebug.stimer(str(self))
self._startrun()
if UTdebug.TIME: UTdebug.ttimer(str(self))
def _startrun(self):
if not self._started:
self.start()
if not self._finished:
UTdebug.log(99, "%s: run()" % str(self))
return self.run()
def _startrun(self):
if not self._started:
self.start()
if not self._finished:
UTdebug.log(99, "%s: run()" % str(self))
return self.run()
def initWalk():
walk_param.send_params_ = True
walk_param.bh_params_.set = True # DON'T FORGET THIS
# defaults
walk_param.bh_params_.speedMax = core.Pose2D(0.8,120,50)
walk_param.bh_params_.speedMaxBackwards = 50
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
walk_param.bh_params_.rsSpeedMax = core.Pose2D(0.87,300,180)# 0.87, 300, 200
walk_param.bh_params_.rsMaxChange = core.Pose2D(0.87,50,50)#0.87,50,50
walk_param.bh_params_.rs_turn_angle_offset = 0 #TODO 07/19/2015
# Default settings for walk speed control
walk_param.sagittal_prior_mean = 3;
walk_param.sagittal_prior_stddev = 6.0;
walk_param.coronal_prior_mean = 1.5;
walk_param.coronal_prior_stddev = 6.0;
# Default settings for forwards/backwards transition
walk_param.dir_change_wait_duration = 100;
walk_param.turn_limit = 1.0;
walk_param.walking_turn_limit = 1.0;
# robot specific
id = core.CONFIG_ID
set = True
# Austin Villa IDs
if id == 35:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
elif id == 36:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
elif id == 41:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
elif id == 43:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
elif id == 42:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by katie 1/25/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
elif id == 47:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by katie 1/25/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
elif id == 40:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 4/30/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,20,10)
elif id == 46:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,10,10)
elif id == 50:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,10,10)
elif id == 51:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,10,10)
elif id == 52:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,10,10)
elif id == 53:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,10,10)
elif id == 54:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,10,10)
# carpet challenge
#walk_param.bh_params_.rsSpeedMax = core.Pose2D(0.7,270,180) # by Ruohan 7/18/15
elif id == 55:
walk_param.bh_params_.speedMax = core.Pose2D(0.4,100,40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1,10,10)
else:
set = False
if not core.TOOL:
if set:
UTdebug.log(0,' BH Robot specific params found for',id)
else:
UTdebug.log(0,' ** No BH Robot specific params found for',id)
def run(self):
UTdebug.log(15, "Resetting keeper")
return pose.Sit()
def processFrame(self): self._frames += 1 if UTdebug.TIME: UTdebug.stimer(str(self)) self._startrun() if UTdebug.TIME: UTdebug.ttimer(str(self))
def initWalk():
walk_param.send_params_ = True
walk_param.bh_params_.set = True # DON'T FORGET THIS
# defaults
walk_param.bh_params_.speedMax = core.Pose2D(0.8, 120, 50)
walk_param.bh_params_.speedMaxBackwards = 50
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
walk_param.bh_params_.rsSpeedMax = core.Pose2D(0.87, 300,
200) # 0.87, 300, 200
walk_param.bh_params_.rsMaxChange = core.Pose2D(0.87, 50, 50) #0.87,50,50
walk_param.bh_params_.rs_turn_angle_offset = 0 #TODO 07/19/2015
# Default settings for walk speed control
walk_param.sagittal_prior_mean = 3
walk_param.sagittal_prior_stddev = 6.0
walk_param.coronal_prior_mean = 1.5
walk_param.coronal_prior_stddev = 6.0
# Default settings for forwards/backwards transition
walk_param.dir_change_wait_duration = 100
walk_param.turn_limit = 1.0
walk_param.walking_turn_limit = 1.0
# robot specific
id = core.CONFIG_ID
set = True
# Austin Villa IDs
if id == 35:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
elif id == 36:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
elif id == 41:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
elif id == 43:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by katie 1/23/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
elif id == 42:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by katie 1/25/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
elif id == 47:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by katie 1/25/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
elif id == 40:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 4/30/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 20, 10)
elif id == 46:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 10, 10)
elif id == 50:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 10, 10)
elif id == 51:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 10, 10)
elif id == 52:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 10, 10)
elif id == 53:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 10, 10)
elif id == 54:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 10, 10)
# carpet challenge
#walk_param.bh_params_.rsSpeedMax = core.Pose2D(0.7,270,180) # by Ruohan 7/18/15
elif id == 55:
walk_param.bh_params_.speedMax = core.Pose2D(0.4, 100,
40) # by Josiah 5/1/15
walk_param.bh_params_.speedMaxChange = core.Pose2D(0.1, 10, 10)
else:
set = False
if not core.TOOL:
if set:
UTdebug.log(0, ' BH Robot specific params found for', id)
else:
UTdebug.log(0, ' ** No BH Robot specific params found for', id)
def run(self):
print("dafuq")
pose.sit()
commands.setStiffness()
# pose.BlockCenter()
UTdebug.log(15, "Blocking center")
def run(self):
UTdebug.log(15, "Blocking left")
def run(self):
UTdebug.log(15, "Blocking left")
return pose.BlockLeft()
def run(self):
UTdebug.log(15, "Blocking center")
return #pose.Squat(time=3.0)
def run(self):
pose.sit()
commands.setStiffness(cfgstiff.Zero)
# pose.BlockCenter()
UTdebug.log(15, "Blocking center")
def run(self):
UTdebug.log(15, "Blocking right")
return pose.BlockRight()
def run(self):
UTdebug.log(15, "Blocking right")
def run(self):
UTdebug.log(15, "Moving to Blocking")
return #pose.Squat(time=3.0)
