def __init__(self, pose, time = 0.50, reverse=False):
   Task.__init__(self)
   self.pose = pose
   self.time = time
   self.reverse = reverse
   walk_request.noWalk()
   kick_request.setNoKick()
Ejemplo n.º 2
0
 def __init__(self):
   Task.__init__(self)
   kick_request.setNoKick()
   walk_request.noWalk()
   kick_request.kick_running_ = False
   behavior_mem.keeperDiving = core.Dive.NONE
   self.state = state_machine.SimpleStateMachine('stop', 'checkarms', 'movearms', 'sit', 'relaxknee', 'relaxbody', 'finish')
   self.skippedState = False
   self.lower_time = 0
Ejemplo n.º 3
0
 def __init__(self):
   Task.__init__(self)
   kick_request.setNoKick()
   walk_request.noWalk()
   kick_request.kick_running_ = False
   behavior_mem.keeperDiving = core.Dive.NONE
   self.state = state_machine.SimpleStateMachine('stop', 'checkarms', 'movearms', 'sit', 'relaxknee', 'relaxbody', 'finish')
   self.skippedState = False
   self.lower_time = 0
Ejemplo n.º 4
0
    def run(self):
        finished = walk_response.finished_standing_
        received = walk_response.received_
        kick_request.setNoKick()
        walk_request.standStraight()
        if received and self.state.inState('requested'):
            self.state.transition('running')
        elif self.state.inState('running') and finished:
            self.finish()

        if self.getTime() > 2:
            self.finish()
Ejemplo n.º 5
0
  def run(self):
    finished = walk_response.finished_standing_
    received = walk_response.received_
    kick_request.setNoKick()
    walk_request.standStraight()
    if received and self.state.inState('requested'):
      self.state.transition('running')
    elif self.state.inState('running') and finished:
      self.finish()

    if self.getTime() > 2:
      self.finish()
Ejemplo n.º 6
0
    def run(self):
        for i in range(2, core.NUM_JOINTS):
            val = util.getPoseJoint(i, self.pose, self.reverse)
            if val != None:
                joint_commands.setJointCommand(i, val * core.DEG_T_RAD)

        joint_commands.send_body_angles_ = True
        joint_commands.body_angle_time = self.time * 1000.0
        walk_request.noWalk()
        kick_request.setNoKick()

        if self.getTime() > self.time:
            self.finish()
Ejemplo n.º 7
0
  def run(self):
    if self.first:
      for i in range(2, core.NUM_JOINTS):
        val = util.getPoseJoint(i, self.pose, self.reverse)
        if val != None:
          joint_commands.setJointCommand(i, val * core.DEG_T_RAD)

      joint_commands.send_body_angles_ = True
      joint_commands.body_angle_time_ = self.time * self.interpolate #1000.0
      walk_request.noWalk()
      kick_request.setNoKick()
      self.first = False

    if self.getTime() > self.time:
      self.finish()
Ejemplo n.º 8
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, 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()
Ejemplo n.º 9
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()
    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()
Ejemplo n.º 11
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()