示例#1
0
 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()
示例#2
0
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)
示例#4
0
 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()
示例#5
0
 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;
示例#6
0
        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)
示例#7
0
 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)
示例#8
0
    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)
示例#11
0
 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)
示例#12
0
 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()
示例#13
0
  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"
示例#14
0
  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()
示例#15
0
    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()
示例#16
0
    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')
示例#17
0
    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()
示例#19
0
    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)
示例#20
0
 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
示例#22
0
文件: main.py 项目: dtbinh/robotics-4
 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"
示例#23
0
文件: main.py 项目: tianyizh/robotics
 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"
示例#24
0
  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)
示例#25
0
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()
示例#26
0
    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)
示例#27
0
    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
示例#28
0
    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)
示例#29
0
  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)
示例#30
0
        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
示例#31
0
 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"
示例#32
0
  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()
示例#33
0
  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()
示例#34
0
文件: pose.py 项目: dtbinh/robotics-4
    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")
示例#36
0
 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()
示例#38
0
 def run(self):
   commands.setStiffness(cfgstiff.Zero)
   if self.getTime() > 3.0:
     memory.speech.say("Off")
     self.finish()
示例#39
0
  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
示例#40
0
 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])
示例#42
0
 def run(self):
     commands.setStiffness()
     self.finish()
示例#43
0
 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)
示例#45
0
 def run(self):
   commands.stand()
   commands.setStiffness(ChangeStiff.OneLegSoft)
   if self.getTime() > 3.0:
     memory.speech.say("I am ready")
     self.finish()
示例#46
0
 def start(self):
   commands.setStiffness()
   super(PoseSequence, self).start()
示例#47
0
 def run(self):
     commands.setStiffness(cfgstiff.Zero)
     self.finish()
 def start(self):
   commands.setStiffness()
   super(PoseSequence, self).start()
示例#49
0
 def run(self):
   commands.setStiffness()
   HeadBodyTask.run(self)
 def run(self):
     commands.setStiffness(cfgstiff.Zero)
     if self.getTime() > 2.0:
         self.finish()
示例#51
0
 def run(self):
   commands.setStiffness(ChangeStiff.OneLegSoft)
   if self.getTime() > 20.0:
     self.finish()
示例#52
0
 def run(self):
   commands.setStiffness(cfgstiff.Zero)
   if self.getTime() > 2.0:
     memory.speech.say("turned off stiffness")
     self.finish()
示例#53
0
 def run(self):
   commands.setStiffness()
   commands.setWalkVelocity(0, 0, -0.3)
   '''beacon_bp = memory.world_objects.getObjPtr(core.WO_BEACON_BLUE_PINK)