def run(self):
commands.setStiffness()
commands.setsetWalkVelocity(.5, 0, 0.0)
commands.setHeadPan(math.pi/4.0, 3)
commands.setHeadPan(-math.pi/4.0, 3)
if self.getTime() > 15.0:
self.finish()
def processFrame():
global currentState, lastState, currentTask
commands.setHeadTilt()
lastState = currentState
currentState = game_state.state()
if currentState == core.PLAYING and lastState != currentState and lastState != core.PENALISED:
behavior_mem.timePlayingStarted = vision_frame_info.seconds_since_start
if util.currentFrame() % 30 == 0:
util.checkTemperatures()
if areDistinct(currentState, lastState):
if currentTask:
currentTask.finish()
currentTask = createStateTask(currentState)
if util.checkFallen(
) and robot_state.WO_SELF != core.WO_TEAM_COACH and not game_state.isPenaltyKick:
commands.setStiffness(cfgstiff.Zero)
kick_request.abortKick()
walk_request.noWalk()
return
currentTask.processFrame()
def run(self):
global carrot, direction, seen_ball, last_ball_heading, last_ball_time, alpha, last_carrot
ball = memory.world_objects.getObjPtr(core.WO_BALL)
commands.setStiffness()
feedback_rate = 0.2
velocity = 2.0
if(ball.seen):
memory.speech.say("found ball")
carrot = ball.visionBearing
if(seen_ball):
dt = self.getTime() - last_ball_time
ball_vel = (ball.visionBearing - last_ball_heading) / dt
direction = numpy.sign(ball_vel)
else:
last_carrot = carrot
seen_ball = True
last_ball_time = self.getTime()
last_ball_heading = ball.visionBearing
else:
# memory.speech.say("searching")
carrot += 0.01*direction
if(carrot > (math.pi/2.0-0.1) and direction > 0.0):
direction = -1.0
elif(carrot < -(math.pi/2.0-0.1) and direction < 0.0):
direction = 1.0
last_carrot = alpha * last_carrot + (1-alpha) * carrot
delta = abs(last_carrot - core.joint_values[core.HeadYaw])
dt = numpy.maximum(0.1, delta / velocity)
commands.setHeadPan(last_carrot, dt)
def run(self):
commands.setStiffness()
commands.setWalkVelocity(0.0, 0.0, 0.0)
commands.setHeadPanTilt(core.DEG_T_RAD * self.pan, self.tilt,
self.time)
if self.getTime() > (self.time + 0.5):
self.finish()
def run(self):
global direction
global angle
global speed
ball = memory.world_objects.getObjPtr(core.WO_BALL)
angle = core.joint_values[core.HeadPan]
speed = 8
normalSpeed = 0.5
commands.setStiffness()
if ball.seen:
commands.setHeadPan(ball.visionBearing, (ball.visionBearing-angle)/speed)
self.finish()
else:
if direction == 1:
commands.setHeadPan(1, (1-angle)/normalSpeed)
print "positive direction"
print "angle %f!" % angle
if angle > 0.9:
print "change to negative direciton"
direction = -1
elif direction == -1:
commands.setHeadPan(-1, (1+angle)/normalSpeed)
print "positive direction"
print "angle %f!" % angle
if angle < -0.9:
print "change to positive direciton"
direction = 1
else:
direction = 1;
def run(self):
global carrot, direction, seen_ball, last_ball_heading, last_ball_time, alpha, last_carrot
ball = memory.world_objects.getObjPtr(core.WO_BALL)
commands.setStiffness()
feedback_rate = 0.2
velocity = 2.0
if (ball.seen):
memory.speech.say("found ball")
carrot = ball.visionBearing
if (seen_ball):
dt = self.getTime() - last_ball_time
ball_vel = (ball.visionBearing - last_ball_heading) / dt
direction = numpy.sign(ball_vel)
else:
last_carrot = carrot
seen_ball = True
last_ball_time = self.getTime()
last_ball_heading = ball.visionBearing
else:
# memory.speech.say("searching")
carrot += 0.01 * direction
if (carrot > (math.pi / 2.0 - 0.1) and direction > 0.0):
direction = -1.0
elif (carrot < -(math.pi / 2.0 - 0.1) and direction < 0.0):
direction = 1.0
last_carrot = alpha * last_carrot + (1 - alpha) * carrot
delta = abs(last_carrot - core.joint_values[core.HeadYaw])
dt = numpy.maximum(0.1, delta / velocity)
commands.setHeadPan(last_carrot, dt)
def run(self):
ball = mem_objects.world_objects[core.WO_BALL]
commands.setStiffness()
if ball.seen:
# commands.setHeadPanTilt(ball.visionBearing, ball.visionElevation, 0.5)
commands.setHeadPan(ball.visionBearing, time_delay)
print(ball.visionBearing, ball.visionElevation)
print(ball.seen, ball.imageCenterX, ball.imageCenterY)
def run(self):
commands.setStiffness()
ball = mem_objects.world_objects[core.WO_BALL]
robot = mem_objects.world_objects[robot_state.WO_SELF]
rob_x = robot.loc.x
rob_y = robot.loc.y
ball_pos = localization_mem.getBallPosition()
x_pos, y_pos = ball_pos.x, ball_pos.y
ball_vel = localization_mem.getBallVel()
print()
print("=== python prints")
print("ball vel: ", ball.absVel.x, ", ", ball.absVel.y)
print("ball pos: ", ball.loc.x, ", ", ball.loc.y)
x_vel, y_vel = ball_vel.x, ball_vel.y
#if ball.seen:
# angle = np.arctan((y_pos - rob_y)/(x_pos - rob_x + 1e-5))
# commands.setHeadPan(angle, 0.1)
#else:
# commands.setHeadPan(0, 0.1)
predicted_x = [
x_pos + x_vel * time_delay * n
for n in np.arange(0.0, predict_secs, 0.1)
]
predicted_y = [
y_pos + y_vel * time_delay * n
for n in np.arange(0.0, predict_secs, 0.1)
]
print("predicted x: ", predicted_x[0], ", ", predicted_x[-1])
print("predicted y: ", predicted_y[0], ", ", predicted_y[-1])
print("robot x, y: ", rob_x, ", ", rob_y)
print("velocity x, y: ", x_vel, ", ", y_vel)
if ball.seen and any(x <= rob_x - x_thresh for x in predicted_x):
possible_goal_frames = [
i for i, x in enumerate(predicted_x) if x <= rob_x - x_thresh
]
if any(
abs(predicted_y[i] - rob_y) < y_thresh
for i in possible_goal_frames):
y_pred = predicted_y[possible_goal_frames[0]]
print('num_frames', possible_goal_frames[0])
if abs(y_pred - rob_y) <= center_region:
choice = "center"
elif y_pred > 0:
choice = "left"
elif y_pred < 0:
choice = "right"
print(choice)
print()
self.postSignal(choice)
def run(self):
#ball = mem_objects.world_objects[core.WO_BALL]
#if ball.seen:
# self.finish()
#else:
commands.setStiffness()
commands.setHeadPan(math.pi / 3, time_delay)
if self.getTime() > time_delay:
self.finish()
def setup(self):
blocker = Blocker()
blocks = {
"left": BlockLeft(),
"right": BlockRight(),
"center": BlockCenter()
}
commands.setStiffness()
for name in blocks:
b = blocks[name]
self.trans(self.Stand(), C, blocker, S(name), b, T(3), blocker)
def run(self):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
print "I am tracking"
if ball.seen:
print "I see the ball"
commands.setStiffness()
#commands.setHeadPan(ball.visionBearing, 0.1)
if ball.visionBearing > angle:
commands.setHeadPan(ball.visionBearing, (ball.visionBearing-angle)/speed)
else:
commands.setHeadPan(ball.visionBearing, -(ball.visionBearing-angle)/speed)
def run(self):
commands.setStiffness()
commands.stand()
# TODO: Check this
#commands.setHeadTilt(-15)
commands.setHeadTilt(0)
#try:
# os.remove("beacon_data_sim.txt")
#except OSError:
# pass
if self.getTime() > 3.0:
self.finish()
def run(self):
#commands.setWalkVelocity(0.2, 0, 0)
#commands.setHeadPan(1, 1.0)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
#memory.speech.say('Yes Yes Yes Yes')
#commands.setHeadPan(1, 1.0)
commands.setStiffness()
print "ball.visionBearing is %f!" % ball.visionBearing
commands.setHeadPan(ball.visionBearing, 0.1)
print "Yes Yes Yes Yes"
else:
#memory.speech.say('NO')
print "NO"
def run(self):
commands.setStiffness(cfgstiff.Zero)
if self.getTime() > 5.0:
print('=============> Joint Sensors <=============')
for joint, index in joint_sensors.items():
print('{}: {}'.format(joint, core.joint_values[index]))
print
print('=============> Pressure Sensors <=============')
for pressure_sensor, index in pressure_sensors.items():
print('{}: {}'.format(pressure_sensor, core.sensor_values[index]))
self.resetTime()
def run(self):
commands.setStiffness(cfgstiff.Zero)
if self.getTime() > 5.0:
print('=============> Joint Sensors <=============')
for joint, index in joint_sensors.items():
print('{}: {}'.format(joint, core.joint_values[index]))
print
print('=============> Pressure Sensors <=============')
for pressure_sensor, index in pressure_sensors.items():
print('{}: {}'.format(pressure_sensor,
core.sensor_values[index]))
self.resetTime()
def run(self):
# get predicted location and velocity
ball = mem_objects.world_objects[core.WO_BALL]
commands.setStiffness()
# pose.SittingPose()
# import pdb; pdb.set_trace()
distance = ball.distance
x = ball.loc.x
y = ball.loc.y
vx = ball.absVel.x
vy = ball.absVel.y
v = math.sqrt(vx * vx + vy * vy)
print(x, y, vx, vy)
commands.setHeadPan(ball.bearing, 0.1)
if vx < 0 and ball.seen:
norm_vx = vx / v
norm_vy = vy / v
end_x = x + distance * norm_vx
end_y = y + distance * norm_vy
print("Distance: ", distance)
print("Predict velocity: ", vx, vy)
print("Predict end y: ", end_y)
# May need moving average for y. Fluctuating v can cause problems
# if v > V_THRESHOLD and vx < 0 and ball.seen and abs(end_y) < GOAL_SIDE:
if end_y < -CENTER_THRESHOLD:
choice = "right"
elif end_y > CENTER_THRESHOLD:
choice = "left"
else:
choice = "center"
self.postSignal(choice)
print("\n\n\n\n\n*******************************************\n",
choice, "\n\n\n\n\n\n")
# else:
# norm_vx = 0.0
# norm_vy = 0.0
# bearing = math.atan2(x, y)
else:
print("\n\n\n\n\n&&&&&&&&&&&&&&&&&&&&&&\n", 'notnotnotnotnot_seen',
'v is', v, 'vx is', vx, "\n\n\n\n\n\n")
self.postSignal('not_seen')
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 run(self):
if switch:
commands.setStiffness(cfgstiff.Zero)
else:
readings = core.joint_values
# TODO: Set joint values
# commands.setStiffness()
print("Set stiffness")
for i, value in enumerate(readings):
joint_commands.setJointCommand(i, value)
# commands.setStiffness()
if self.getTime() > 0.5:
self.finish()
def run(self):
commands.setStiffness()
# commands.stand()
ball = mem_objects.world_objects[core.WO_BALL]
selfRobot = mem_objects.world_objects[core.WO_TEAM5]
relBall = ball.loc.globalToRelative(selfRobot.loc,
selfRobot.orientation)
if ball.seen:
commands.setHeadPan(ball.bearing, 0.1)
self.lastSeenCt = 0
else:
self.lastSeenCt += 1
if self.lastSeenCt > 120:
commands.setHeadPan(0, 0.1)
eta = float('inf')
print "Ball: {0}, {1} Velocity: {2}, {3} Vision: {4} {5}".format(
relBall.x, relBall.y, ball.absVel.x, ball.absVel.y,
ball.visionDistance, ball.visionBearing * core.RAD_T_DEG)
if ball.absVel.x < 0:
# eta = -1.0 * (ball.loc.x + 1000) / ball.absVel.x
eta = -1.0 * relBall.x / ball.absVel.x
#if abs(ball.loc.x / ball.relVel.x) < 3.0 and ball.relVel.x < 0: # Ball will reach us in 3 seconds
if eta < 10 and relBall.x < 1000 and eta > 3:
# intercept = ball.loc.y + (ball.absVel.y * eta)
intercept = relBall.y + ball.absVel.y * eta
print eta
print ball.loc
print relBall
print ball.absVel
# print ball.relVel
print intercept
if intercept < 500 and intercept > -500:
UTdebug.log(15, "Ball is close, blocking!")
if intercept > 120:
choice = "left"
elif intercept < -120:
choice = "right"
else:
choice = "center"
self.postSignal(choice)
def run(self):
print "Search: {0}, {1}".format(self.direction, self.seenCount)
commands.setStiffness(cfgstiff.One)
commands.stand()
current_angle = core.joint_values[core.HeadYaw]
commands.setHeadPan(self.direction * 55 * DEG_T_RAD, 1.0)
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
self.seenCount = self.seenCount + 1
else:
self.seenCount = max(0,self.seenCount - 1)
if self.direction == 1 and current_angle > 45 * DEG_T_RAD:
self.direction = -1
if self.direction == -1 and current_angle < -45 * DEG_T_RAD:
self.direction = 1
if self.seenCount > 3:
self.seenCount = 0
self.finish()
print self.seenCount
def run(self):
commands.setStiffness()
commands.setHeadTilt(-15)
beacon1 = mem_objects.world_objects[core.WO_BEACON_BLUE_YELLOW]
if beacon1.seen:
if len(self.data) < 100:
self.data.append(
[beacon1.beacon_height, beacon1.visionBearing])
mat = np.array(self.data)
print(mat.shape)
if len(self.data) > 2:
print(np.mean(self.data, axis=0))
print(np.sqrt(np.var(self.data, axis=0)))
print()
state = game_state
if state.isPenaltyKick:
self.data = []
state.isPenaltyKick = False
def run(self):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
print "aaaaaaaaa"
if ball.seen:
#print "ball.visionBearing is %f!" % ball.visionBearing
print "I have seen the blue goal"
commands.setStiffness()
if ball.visionBearing > 0.20:
commands.setWalkVelocity(0, 0, 0.3)
elif ball.visionBearing < -0.20:
commands.setWalkVelocity(0, 0, -0.3)
else:
print "ball.visionDistance is %f!" % ball.visionDistance
if ball.visionDistance > 2200:
commands.setWalkVelocity(0.6, 0, 0)
elif ball.visionDistance < 1500:
commands.setWalkVelocity(0, 0, 0)
else:
commands.setWalkVelocity(0, 0, 0)
else:
print "I don't see anything"
def run(self):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
print "aaaaaaaaa"
if ball.seen:
#print "ball.visionBearing is %f!" % ball.visionBearing
print "I have seen the blue goal"
commands.setStiffness()
if ball.visionBearing > 0.20:
commands.setWalkVelocity(0, 0, 0.3)
elif ball.visionBearing < -0.20:
commands.setWalkVelocity(0, 0, -0.3)
else:
print "ball.visionDistance is %f!" % ball.visionDistance
if ball.visionDistance > 2200:
commands.setWalkVelocity(0.6, 0, 0)
elif ball.visionDistance < 1500:
commands.setWalkVelocity(0, 0, 0)
else:
commands.setWalkVelocity(0, 0, 0)
else:
print "I don't see anything"
def run(self):
commands.setStiffness()
# commands.stand()
ball = mem_objects.world_objects[core.WO_BALL]
selfRobot = mem_objects.world_objects[core.WO_TEAM5]
relBall = ball.loc.globalToRelative(selfRobot.loc, selfRobot.orientation)
if ball.seen:
commands.setHeadPan(ball.bearing, 0.1)
self.lastSeenCt = 0
else:
self.lastSeenCt += 1
if self.lastSeenCt > 120:
commands.setHeadPan(0,0.1)
eta = float('inf')
print "Ball: {0}, {1} Velocity: {2}, {3} Vision: {4} {5}".format(relBall.x, relBall.y, ball.absVel.x, ball.absVel.y, ball.visionDistance, ball.visionBearing*core.RAD_T_DEG)
if ball.absVel.x < 0:
# eta = -1.0 * (ball.loc.x + 1000) / ball.absVel.x
eta = -1.0 * relBall.x / ball.absVel.x
#if abs(ball.loc.x / ball.relVel.x) < 3.0 and ball.relVel.x < 0: # Ball will reach us in 3 seconds
if eta < 10 and relBall.x < 1000 and eta > 3:
# intercept = ball.loc.y + (ball.absVel.y * eta)
intercept = relBall.y + ball.absVel.y * eta
print eta
print ball.loc
print relBall
print ball.absVel
# print ball.relVel
print intercept
if intercept < 500 and intercept > -500:
UTdebug.log(15, "Ball is close, blocking!")
if intercept > 120:
choice = "left"
elif intercept < -120:
choice = "right"
else:
choice = "center"
self.postSignal(choice)
def processFrame():
global currentState, lastState, currentTask
commands.setHeadTilt()
lastState = currentState
currentState = game_state.state()
if currentState == core.PLAYING and lastState != currentState and lastState != core.PENALISED:
behavior_mem.timePlayingStarted = vision_frame_info.seconds_since_start
if util.currentFrame() % 30 == 0: util.checkTemperatures()
if areDistinct(currentState, lastState):
if currentTask: currentTask.finish()
currentTask = createStateTask(currentState)
if util.checkFallen() and robot_state.WO_SELF != core.WO_TEAM_COACH and not game_state.isPenaltyKick:
commands.setStiffness(cfgstiff.Zero)
kick_request.abortKick()
walk_request.noWalk()
return
currentTask.processFrame()
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 toPose(self, pose, time):
global pose_sent, pose_start_time, num_times_sent
commands.setStiffness()
times_to_send = 1
if num_times_sent < times_to_send:
for i in range(2, core.NUM_JOINTS):
val = util.getPoseJoint(i, pose, False)
if val != None:
joint_commands.setJointCommand(i, val * core.DEG_T_RAD)
joint_commands.send_body_angles_ = True
joint_commands.body_angle_time_ = time * 1000.0
walk_request.noWalk()
kick_request.setNoKick()
num_times_sent = num_times_sent+1
pose_start_time = self.getTime()
#print "Sending pose at time " + str(pose_start_time) + "! #" + str(num_times_sent)
if num_times_sent >= times_to_send and ((self.getTime() - pose_start_time) > time):
#print "DONE!"
return True
else:
return False
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 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
velX = behavior_mem.test_odom_fwd
velY = behavior_mem.test_odom_side
velTheta = 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(velX, velY, velTheta)
def toPose(self, pose, time):
global pose_sent, pose_start_time, num_times_sent
commands.setStiffness()
times_to_send = 1
if num_times_sent < times_to_send:
for i in range(2, core.NUM_JOINTS):
val = util.getPoseJoint(i, pose, False)
if val != None:
joint_commands.setJointCommand(i, val * core.DEG_T_RAD)
joint_commands.send_body_angles_ = True
joint_commands.body_angle_time_ = time * 1000.0
walk_request.noWalk()
kick_request.setNoKick()
num_times_sent = num_times_sent + 1
pose_start_time = self.getTime()
#print "Sending pose at time " + str(pose_start_time) + "! #" + str(num_times_sent)
if num_times_sent >= times_to_send and (
(self.getTime() - pose_start_time) > time):
#print "DONE!"
return True
else:
return False
def run(self):
beacon_pink = memory.world_objects.getObjPtr(core.WO_BEACON_BLUE_PINK)
if beacon_pink.seen:
#print "ball.visionBearing is %f!" % ball.visionBearing
print "I see"
commands.setStiffness()
if beacon_pink.visionBearing > 0.20:
commands.setWalkVelocity(0, 0, 0.3)
elif beacon_pink.visionBearing < -0.20:
commands.setWalkVelocity(0, 0, -0.3)
else:
#commands.setWalkVelocity(0, 0, 0)
pose.Sit()
commands.setStiffness(cfgstiff.Zero)
else:
commands.setStiffness()
commands.setWalkVelocity(0, 0, -0.3)
print "I don't see anything"
def run(self):
if self.getTime() < 2.0:
core.walk_request.noWalk()
core.kick_request.setNoKick()
commands.setStiffness(cfgstiff.One, 0.3)
return
st = self.state
if st.inState(st.stop):
st.transition(st.checkarms)
if st.inState(st.checkarms):
shoulderCutoff = core.DEG_T_RAD * -90
if percepts.joint_angles[core.LShoulderPitch] > shoulderCutoff and percepts.joint_angles[core.RShoulderPitch] > shoulderCutoff:
st.transition(st.sit)
else:
st.transition(st.movearms)
elif st.inState(st.movearms):
pose = util.deepcopy(cfgpose.sittingPoseV3)
for joint, val in cfgpose.armSidePose.items():
pose[joint] = val
self.subtask = skills.ToPoseMoveHead(tilt = 0.0, pose = pose)
self.subtask.start()
st.transition(st.sit)
elif st.inState(st.sit) and (not self.subtask or self.subtask.finished()):
self.skippedState = False
self.subtask = skills.ToPoseMoveHead(pose = cfgpose.sittingPoseV3, time = 1.0)
self.subtask.start()
st.transition(st.relaxknee)
elif st.inState(st.relaxknee) and self.subtask.finished():
self.lower_time = self.getTime()
commands.setStiffness(cfgstiff.ZeroKneeAnklePitch, 0.3)
st.transition(st.relaxbody)
elif st.inState(st.relaxbody) and st.timeSinceTransition() > 0.7:
commands.setStiffness(cfgstiff.Zero, 0.3)
st.transition(st.finish)
elif st.inState(st.finish):
self.finish()
def run(self):
if self.getTime() < 2.0:
walk_request.noWalk()
kick_request.setNoKick()
commands.setStiffness(cfgstiff.One, 0.3)
return
st = self.state
if st.inState(st.stop):
st.transition(st.checkarms)
if st.inState(st.checkarms):
shoulderCutoff = core.DEG_T_RAD * -90
lpitch = core.joint_values[core.LShoulderPitch]
rpitch = core.joint_values[core.RShoulderPitch]
if lpitch > shoulderCutoff and rpitch > shoulderCutoff:
st.transition(st.sit)
else:
st.transition(st.movearms)
elif st.inState(st.movearms):
pose = util.deepcopy(cfgpose.sittingPoseV3)
for joint, val in cfgpose.armSidePose.items():
pose[joint] = val
st.transition(st.sit)
return ToPoseMoveHead(tilt = 0.0, pose = pose)
elif st.inState(st.sit):
self.skippedState = False
st.transition(st.relaxknee)
return ToPoseMoveHead(pose = cfgpose.sittingPoseV3)
elif st.inState(st.relaxknee):
self.lower_time = self.getTime()
commands.setStiffness(cfgstiff.ZeroKneeAnklePitch, 0.3)
st.transition(st.relaxbody)
elif st.inState(st.relaxbody) and st.timeSinceTransition() > 0.7:
commands.setStiffness(cfgstiff.Zero, 0.3)
st.transition(st.finish)
elif st.inState(st.finish):
self.finish()
def run(self):
if self.getTime() < 2.0:
walk_request.noWalk()
kick_request.setNoKick()
commands.setStiffness(cfgstiff.One, 0.3)
return
st = self.state
if st.inState(st.stop):
st.transition(st.checkarms)
if st.inState(st.checkarms):
shoulderCutoff = core.DEG_T_RAD * -90
lpitch = core.joint_values[core.LShoulderPitch]
rpitch = core.joint_values[core.RShoulderPitch]
if lpitch > shoulderCutoff and rpitch > shoulderCutoff:
st.transition(st.sit)
else:
st.transition(st.movearms)
elif st.inState(st.movearms):
pose = util.deepcopy(cfgpose.sittingPoseV3)
for joint, val in cfgpose.armSidePose.items():
pose[joint] = val
st.transition(st.sit)
return ToPoseMoveHead(tilt=0.0, pose=pose)
elif st.inState(st.sit):
self.skippedState = False
st.transition(st.relaxknee)
return ToPoseMoveHead(pose=cfgpose.sittingPoseV3, time=1.0)
elif st.inState(st.relaxknee):
self.lower_time = self.getTime()
commands.setStiffness(cfgstiff.ZeroKneeAnklePitch, 0.3)
st.transition(st.relaxbody)
elif st.inState(st.relaxbody) and st.timeSinceTransition() > 0.7:
commands.setStiffness(cfgstiff.Zero, 0.3)
st.transition(st.finish)
elif st.inState(st.finish):
self.finish()
def run(self):
pose.sit()
commands.setStiffness(cfgstiff.Zero)
# pose.BlockCenter()
UTdebug.log(15, "Blocking center")
def run(self):
commands.setStiffness(cfgstiff.Zero)
if self.getTime() > 2.0:
memory.speech.say("turned off stiffness")
self.finish()
def run(self):
commands.setStiffness(cfgstiff.Zero)
if self.getTime() > 2.0:
self.finish()
def run(self):
commands.setStiffness(cfgstiff.Zero)
if self.getTime() > 3.0:
memory.speech.say("Off")
self.finish()
def run(self):
woself = core.world_objects.getObjPtr(core.robot_state.WO_SELF)
if inDuration(0.0, 2.0, self): ##START UP
commands.setStiffness()
commands.stand()
commands.setHeadTilt(-21)
# print('X(' + str(woself.loc.x) + ') Y(' + str(woself.loc.y) + ') ORIENT(' + str(woself.orientation) + ')\n' )
elif inDuration(0.0, self.walkEnd, self):
commands.setHeadPan(0.0, 1.0, False)
# print('IN DURATION ' + str(self.walkEnd) + ' < ' + str(self.getTime()) + '\n')
# actualDegrees = (woself.orientation * 180.0) / math.pi
# print('X(' + str(woself.loc.x) + ') Y(' + str(woself.loc.y) + ') ORIENT(' + str(woself.orientation) + ') DEGREES(' + str(actualDegrees) + ')\n' )
# commands.setWalkVelocity(0.3, 0.0, 0.0)
centAngle = math.atan(math.fabs(woself.loc.x)/math.fabs(woself.loc.y))
print('CENT BEFORE: ' + str(centAngle * (180/math.pi)))
if(woself.loc.x >= 0 and woself.loc.y >= 0):
centAngle = -(centAngle + (math.pi/2.0))
elif(woself.loc.x < 0 and woself.loc.y >= 0):
centAngle = -centAngle
elif(woself.loc.x > 0 and woself.loc.y < 0):
centAngle = centAngle + (math.pi/2.0)
else:
centAngle = centAngle
# print(' CENT ANGLE: ' + str(centAngle*(180.0/math.pi)) + ' TO THETA ' + str(woself.orientation*(180.0/math.pi)) + '\n')
# angleDiff = centAngle-self.safeOrientation
angleDiff = centAngle - woself.orientation#self.safeOrientation
if(angleDiff > math.pi):
angleDiff -= 2.0*math.pi
elif(angleDiff < -math.pi):
angleDiff += 2.0*math.pi
walkVelocity = 0.2
turnVelocity = 0.0
# print('FABS ' + str(math.fabs(angleDiff-math.pi)) + ' ' + str(math.fabs(angleDiff+math.pi)) + '\n')
# if(math.fabs(angleDiff-math.pi) < math.pi/8 or math.fabs(angleDiff+math.pi) < math.pi/8):
# turnVelocity = 0.0
# walkVelocity = -0.5
if(angleDiff < math.pi/8 and angleDiff > -math.pi/8):
turnVelocity = 0.0
elif(angleDiff > 0):
turnVelocity = -0.2
else:
turnVelocity = 0.2
if( woself.loc.x < 100 and woself.loc.x > -100 and woself.loc.y < 100 and woself.loc.y > -100):
commands.stand()
else:
commands.setWalkVelocity(walkVelocity, 0.0, turnVelocity)
else:
commands.stand()
numSteps = self.period / head.DiscreteScan.stepTime + 1
stepSize = (2 * self.maxPan / numSteps) * 1.05
st = self.state
self.orientationSamples.append(woself.orientation)
if len(self.orientationSamples)+1 >= 30:
self.orientationSamples.pop()
if self.sweepCounter > self.numSweeps:
self.walkEnd = self.getTime() + 6.0
self.sweepCounter = 0
self.safeOrientation = 0
for i in self.orientationSamples:
self.safeOrientation += i
self.safeOrientation = self.safeOrientation / len(self.orientationSamples)
self.orientationSamples = []
self.safeX = woself.loc.x
self.safeY = woself.loc.y
return
if st.inState(st.firstScan):
self.intDirection = self.direction
self.subtask = head.DiscreteScan(dest = self.direction * self.maxPan, stepSize = stepSize)
st.transition(st.nextScans)
return
if st.inState(st.nextScans) and self.subtask.finished():
self.sweepCounter += 1
self.intDirection *= -1
core.behavior_mem.completeBallSearchTime = core.vision_frame_info.seconds_since_start
self.subtask = head.DiscreteScan(dest = self.intDirection * self.maxPan, stepSize = stepSize, skipFirstPause = True)
return
def run(self):
commands.setStiffness(cfgstiff.kickStand)
def run(self):
commands.setStiffness(cfgstiff.Zero)
print ("My battery value is :", core.sensor_values[core.battery])
print ("My head yaw value is :", core.joint_values[core.HeadYaw])
def run(self):
commands.setStiffness()
self.finish()
def run(self):
commands.setStiffness()
commands.setWalkVelocity(.5, .2, 0.0)
if self.getTime() > 5.0:
self.subtask = pose.Sit()
self.finish()
def run(self): commands.setStiffness(cfgstiff.kickStand)
def run(self):
commands.stand()
commands.setStiffness(ChangeStiff.OneLegSoft)
if self.getTime() > 3.0:
memory.speech.say("I am ready")
self.finish()
def start(self): commands.setStiffness() super(PoseSequence, self).start()
def run(self):
commands.setStiffness(cfgstiff.Zero)
self.finish()
def start(self): commands.setStiffness() super(PoseSequence, self).start()
def run(self): commands.setStiffness() HeadBodyTask.run(self)
def run(self):
commands.setStiffness(cfgstiff.Zero)
if self.getTime() > 2.0:
self.finish()
def run(self):
commands.setStiffness(ChangeStiff.OneLegSoft)
if self.getTime() > 20.0:
self.finish()
def run(self):
commands.setStiffness(cfgstiff.Zero)
if self.getTime() > 2.0:
memory.speech.say("turned off stiffness")
self.finish()
def run(self): commands.setStiffness() commands.setWalkVelocity(0, 0, -0.3) '''beacon_bp = memory.world_objects.getObjPtr(core.WO_BEACON_BLUE_PINK)
