Beispiel #1
0
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
Beispiel #2
0
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
Beispiel #3
0
def doEyePower(eye=ledsC.allBottomLeftEye):
    battery = sensors.getValue(core.battery)
    if (battery > 0.95):
        eye(1, 1, 1)
        return False
    elif (battery > 0.8):
        eye(1, 1, 0)
        return False
    elif (battery > 0.65):
        eye(1, .5, 1)
        return False
    else:
        eye(1, 0, 0)
        return True
    def run(self):
      global start_frame, droll_off, dpitch_off, dlarmroll_off, dlarmpitch_off, drarmroll_off, drarmpitch_off
      if(start_frame == -1):
        start_frame = vision_frame_info.frame_id

      traj_idx = vision_frame_info.frame_id - start_frame
      if(traj_idx >= len(trajectory)):
        start_frame = -1
        self.finish()
        print "done!"
        return
        # traj_idx = len(trajectory) - 1

      # print "Kick is " + str(float(traj_idx)/float(len(trajectory)-1.)*100.) + " percent complete"

      inverted_joints = [1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1]
      # inverted_joints[core.LShoulderPitch] = -1.
      # inverted_joints[core.RShoulderPitch] = -1.
      # inverted_joints[core.RShoulderRoll] = -1.
      # inverted_joints[core.RElbowRoll] = -1.
  
      pose = trajectory[traj_idx]
      for i in range(2, core.NUM_JOINTS):
        joint_commands.setJointCommand(i, pose[i]*inverted_joints[i])

      # tweak left ankle commands to handle cop dynamics
      fl = sensors.getValue(core.fsrLFL)
      fr = sensors.getValue(core.fsrLFR)
      rl = sensors.getValue(core.fsrLRL)
      rr = sensors.getValue(core.fsrLRR)
      dx = (fl + fr)/2. - (rl + rr)/2. + 0.
      dy = (fl + rl)/2. - (fr + rr)/2. - 0.05
      # print "dx: " + str(dx)
      # print "dy: " + str(dy)

      clamp = 0.2
      # droll_off += -dy * 0.001
      # dpitch_off += -dx * 0.0 
      dlarmroll_off = numpy.clip(dlarmroll_off - dy * 0.0025, -clamp, clamp)
      # dlarmpitch_off += dx * 0.0 
      drarmroll_off = numpy.clip(drarmroll_off - dy * 0.0025, -clamp, clamp)
      # drarmpitch_off += dx * 0.0 

      droll = dy * 0.01 + droll_off
      dpitch = -dx * 0.0  + dpitch_off
      dlarmroll = -dy * 0.6 + dlarmroll_off
      dlarmpitch = dx * 0.0  + dlarmpitch_off
      dlhiproll = -dy * 0.025
      drarmroll = -dy * 0.6 + drarmroll_off
      drarmpitch = dx * 0.0  + drarmpitch_off



      # print "droll: " + str(droll)
      # print "dpitch: " + str(dpitch)
      # print "dlarmroll: " + str(dlarmroll)
      # print "dlarmpitch: " + str(dlarmpitch)
      # print "drarmroll: " + str(drarmroll)
      # print "drarmpitch: " + str(drarmpitch)

      # joint_commands.setJointCommand(core.LAnklePitch, (pose[core.LAnklePitch] + dpitch)*inverted_joints[core.LAnklePitch])
      # joint_commands.setJointCommand(core.LAnkleRoll, (pose[core.LAnkleRoll] + droll)*inverted_joints[core.LAnkleRoll])
      # joint_commands.setJointCommand(core.LShoulderPitch, (pose[core.LShoulderPitch] + dlarmpitch)*inverted_joints[core.LShoulderPitch])
      # joint_commands.setJointCommand(core.LShoulderRoll, numpy.clip(pose[core.LShoulderRoll] + dlarmroll, 0.3, 10)*inverted_joints[core.LShoulderRoll])
      # joint_commands.setJointCommand(core.LHipRoll, (pose[core.LHipRoll] + dlhiproll)*inverted_joints[core.LHipRoll])
      # joint_commands.setJointCommand(core.RShoulderPitch, (pose[core.RShoulderPitch] + drarmpitch)*inverted_joints[core.RShoulderPitch])
      # joint_commands.setJointCommand(core.RShoulderRoll, numpy.clip(pose[core.RShoulderRoll] + drarmroll, -0.3, -10)*inverted_joints[core.RShoulderRoll])

      #send commands and prevent other controllers from operating
      joint_commands.send_body_angles_ = True
      joint_commands.body_angle_time = 1# 1.0/30.0 * 1000.0#todo: figure out what units this is
      walk_request.noWalk()
      kick_request.setNoKick()
Beispiel #5
0
        def run(self):
            global start_frame, droll_off, dpitch_off, dlarmroll_off, dlarmpitch_off, drarmroll_off, drarmpitch_off
            if (start_frame == -1):
                start_frame = vision_frame_info.frame_id

            traj_idx = vision_frame_info.frame_id - start_frame
            if (traj_idx >= len(trajectory)):
                start_frame = -1
                self.finish()
                print "done!"
                return
                # traj_idx = len(trajectory) - 1

            # print "Kick is " + str(float(traj_idx)/float(len(trajectory)-1.)*100.) + " percent complete"

            inverted_joints = [
                1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, -1, 1, 1, 1, -1,
                -1, -1, -1
            ]
            # inverted_joints[core.LShoulderPitch] = -1.
            # inverted_joints[core.RShoulderPitch] = -1.
            # inverted_joints[core.RShoulderRoll] = -1.
            # inverted_joints[core.RElbowRoll] = -1.

            pose = trajectory[traj_idx]
            for i in range(2, core.NUM_JOINTS):
                joint_commands.setJointCommand(i, pose[i] * inverted_joints[i])

            # tweak left ankle commands to handle cop dynamics
            fl = sensors.getValue(core.fsrLFL)
            fr = sensors.getValue(core.fsrLFR)
            rl = sensors.getValue(core.fsrLRL)
            rr = sensors.getValue(core.fsrLRR)
            dx = (fl + fr) / 2. - (rl + rr) / 2. + 0.
            dy = (fl + rl) / 2. - (fr + rr) / 2. - 0.05
            # print "dx: " + str(dx)
            # print "dy: " + str(dy)

            clamp = 0.2
            # droll_off += -dy * 0.001
            # dpitch_off += -dx * 0.0
            dlarmroll_off = numpy.clip(dlarmroll_off - dy * 0.0025, -clamp,
                                       clamp)
            # dlarmpitch_off += dx * 0.0
            drarmroll_off = numpy.clip(drarmroll_off - dy * 0.0025, -clamp,
                                       clamp)
            # drarmpitch_off += dx * 0.0

            droll = dy * 0.01 + droll_off
            dpitch = -dx * 0.0 + dpitch_off
            dlarmroll = -dy * 0.6 + dlarmroll_off
            dlarmpitch = dx * 0.0 + dlarmpitch_off
            dlhiproll = -dy * 0.025
            drarmroll = -dy * 0.6 + drarmroll_off
            drarmpitch = dx * 0.0 + drarmpitch_off

            # print "droll: " + str(droll)
            # print "dpitch: " + str(dpitch)
            # print "dlarmroll: " + str(dlarmroll)
            # print "dlarmpitch: " + str(dlarmpitch)
            # print "drarmroll: " + str(drarmroll)
            # print "drarmpitch: " + str(drarmpitch)

            # joint_commands.setJointCommand(core.LAnklePitch, (pose[core.LAnklePitch] + dpitch)*inverted_joints[core.LAnklePitch])
            # joint_commands.setJointCommand(core.LAnkleRoll, (pose[core.LAnkleRoll] + droll)*inverted_joints[core.LAnkleRoll])
            # joint_commands.setJointCommand(core.LShoulderPitch, (pose[core.LShoulderPitch] + dlarmpitch)*inverted_joints[core.LShoulderPitch])
            # joint_commands.setJointCommand(core.LShoulderRoll, numpy.clip(pose[core.LShoulderRoll] + dlarmroll, 0.3, 10)*inverted_joints[core.LShoulderRoll])
            # joint_commands.setJointCommand(core.LHipRoll, (pose[core.LHipRoll] + dlhiproll)*inverted_joints[core.LHipRoll])
            # joint_commands.setJointCommand(core.RShoulderPitch, (pose[core.RShoulderPitch] + drarmpitch)*inverted_joints[core.RShoulderPitch])
            # joint_commands.setJointCommand(core.RShoulderRoll, numpy.clip(pose[core.RShoulderRoll] + drarmroll, -0.3, -10)*inverted_joints[core.RShoulderRoll])

            #send commands and prevent other controllers from operating
            joint_commands.send_body_angles_ = True
            joint_commands.body_angle_time = 1  # 1.0/30.0 * 1000.0#todo: figure out what units this is
            walk_request.noWalk()
            kick_request.setNoKick()