Python ALProxy.wbEnableBalanceConstraint方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableBalanceConstraint [as 别名]

#.........这里部分代码省略.........
                ["MaxStepX", 0.07],
                #maxStepYRight,
                ["MaxStepTheta", 0.349],
                ["MaxStepFrequency", 1],
                ["StepHeight", 0.015],
                ["TorsoWx", 0.0],
                ["TorsoWy", 0.0]],
                [#RIGHT
                ["MaxStepX", 0.7],
                #maxStepYLeft,
                ["MaxStepTheta", 0.349],
                ["MaxStepFrequency", 1],
                ["StepHeight", 0.015],
                ["TorsoWx", 0.0], 
                ["TorsoWy", 0.0]])

    def killWalk(self):
        self.motionProxy.stopWalk()

    def leftKick(self, prime, execute, cool_down):

        # Activate Whole Body Balancer
        isEnabled  = True
        self.motionProxy.wbEnable(isEnabled)

        # Legs are constrained fixed
        stateName  = "Fixed"
        supportLeg = "Legs"
        self.motionProxy.wbFootState(stateName, supportLeg)

        # Constraint Balance Motion
        isEnable   = True
        supportLeg = "Legs"
        self.motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)

        # Com go to LLeg
        supportLeg = "LLeg"
        duration   = 2.0
        self.motionProxy.wbGoToBalance(supportLeg, duration)

        # RLeg is free
        stateName  = "Free"
        supportLeg = "RLeg"
        self.motionProxy.wbFootState(stateName, supportLeg)

        # RLeg is optimized
        effectorName = "RLeg"
        axisMask     = 63
        space        = motion.FRAME_ROBOT


        # Motion of the RLeg
        dx      = 0.05                 # translation axis X (meters)
        dz      = 0.05                 # translation axis Z (meters)
        dwy     = 5.0*math.pi/180.0    # rotation axis Y (radian)


        times   = [prime, execute, cool_down]
        isAbsolute = False

        targetList = [
          [-dx, 0.0, dz, 0.0, +dwy, 0.0],
          [+dx, 0.0, dz, 0.0, 0.0, 0.0],
          [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]

    

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableBalanceConstraint [as 别名]
 def run(self, robotIP):
     ''' Example of a whole body kick
      Warning: Needs a PoseInit before executing
              Whole body balancer must be inactivated at the end of the script
     '''
 
     motionProxy  = ALProxy("ALMotion", robotIP, 9559)
     postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
 
     # Wake up robot
     motionProxy.wakeUp()
 
     # Send robot to Stand Init
     postureProxy.goToPosture("StandInit", 0.5)
 
     # Activate Whole Body Balancer
     isEnabled  = True
     motionProxy.wbEnable(isEnabled)
 
     # Legs are constrained fixed
     stateName  = "Fixed"
     supportLeg = "Legs"
     motionProxy.wbFootState(stateName, supportLeg)
 
     # Constraint Balance Motion
     isEnable   = True
     supportLeg = "Legs"
     motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)
 
     # Com go to LLeg
     supportLeg = "LLeg"
     duration   = 2.0
     motionProxy.wbGoToBalance(supportLeg, duration)
 
     # RLeg is free
     stateName  = "Free"
     supportLeg = "RLeg"
     motionProxy.wbFootState(stateName, supportLeg)
 
     # RLeg is optimized
     effector = "RLeg"
     axisMask = 63
     frame    = motion.FRAME_WORLD
 
     # Motion of the RLeg
     times   = [2.0, 2.7, 4.5]
 
     path = Kick.computePath(self, motionProxy, effector, frame)
 
     motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
 
     # Example showing how to Enable Effector Control as an Optimization
     isActive     = False
     motionProxy.wbEnableEffectorOptimization(effector, isActive)
 
     # Com go to LLeg
     supportLeg = "RLeg"
     duration   = 2.0
     motionProxy.wbGoToBalance(supportLeg, duration)
 
     # RLeg is free
     stateName  = "Free"
     supportLeg = "LLeg"
     motionProxy.wbFootState(stateName, supportLeg)
 
     effector = "LLeg"
     path = Kick.computePath(self, motionProxy, effector, frame)
     motionProxy.transformInterpolations(effector, frame, path, axisMask, times)
 
     time.sleep(1.0)
 
     # Deactivate Head tracking
     isEnabled = False
     motionProxy.wbEnable(isEnabled)
 
     # send robot to Pose Init
     postureProxy.goToPosture("StandInit", 0.3)
 
     # Go to rest position
     motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableBalanceConstraint [as 别名]
                times   = [1.1, 1.4, 1.5]
                isAbsolute = False

                targetList = [
                  [0.0, -dy, dz, -dwx, 0.0, 0.0],
                  [dx, -dy, dz, -dwx, dwy, dwz],
                  [0.0, -0.07, 0.0, 0.0, 0.0, 0.0]]

                
                ##################################################
                ############## Left Leg Balance ##################
                ##################################################
                # Constraint Balance Motion
                isEnable   = True
                supportLeg = "Legs"
                motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)

                # Com go to LLeg
                supportLeg = "LLeg"
                duration   = 2.0
                motionProxy.wbGoToBalance(supportLeg, duration)

                # RLeg is free
                stateName  = "Free"
                supportLeg = "RLeg"
                motionProxy.wbFootState(stateName, supportLeg)

                # RLeg is optimized
                effectorName = "RLeg"
                axisMask     = 63
                space        = motion.FRAME_ROBOT

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableBalanceConstraint [as 别名]

#.........这里部分代码省略.........
        keys.append(
            [4.19617e-05, -0.167164, -0.164096, -0.164096, -0.164096, -0.164096, -0.159494, -0.159494, -0.159494,
             -0.161028, -0.161028, -0.164096])

        namess.append("RKneePitch")
        times.append([1.6, 3.2, 4.84, 6.6, 8, 9.4, 11, 12.6, 14.2, 15.8, 17.4, 19])
        keys.append(
            [0.852946, -0.0889301, -0.0812599, -0.0843279, -0.0812599, -0.0890141, -0.0889301, -0.0889301, -0.0889301,
             -0.0873961, -0.0873961, -0.0923279])

        namess.append("RShoulderPitch")
        times.append([1.6, 3.2, 4.84, 6.6, 8, 9.4, 11, 12.6, 14.2, 15.8, 17.4, 19])
        keys.append(
            [1.42053, 1.30087, 1.22724, 1.21037, 1.22724, 1.21037, -0.262272, -0.0383082, 1.126, 0.828401, 0.564555,
             0.590632])

        namess.append("RShoulderRoll")
        times.append([1.6, 3.2, 4.84, 6.6, 8, 9.4, 11, 12.6, 14.2, 15.8, 17.4, 19])
        keys.append([-0.253151, -0.392746, -0.289967, -0.306841, -0.289967, -0.306841, 0.124212, -0.179519, -0.128898,
                     -0.142704, -0.0782759, -0.211735])

        namess.append("RWristYaw")
        times.append([1.6, 3.2, 4.84, 6.6, 8, 9.4, 11, 12.6, 14.2, 15.8, 17.4, 19])
        keys.append(
            [0.176367, -0.619779, -0.066004, -0.04913, -0.066004, -0.04913, -0.107422, -0.377407, -1.04623, -0.197927,
             -0.306841, -0.345191])


        nplay = MusicPlay(self.IPOne)
        nplay.start()

        isEnable = True
        supportLeg = "Legs"
        motionProxyOne.wbEnableBalanceConstraint(isEnable, supportLeg)

        motionProxyOne.setStiffnesses("Head", 1)

#        motionProxyOne.setAnges("HeadYaw", 0.0, 0.2)

        n = 0
        while n < 2:  # 手部动作
            n = n + 1
            motionProxyOne.setAngles(names, angles, 0.06)
            time.sleep(0.6)
            motionProxyOne.setAngles(names, angles1, 0.06)
            time.sleep(0.6)
            motionProxyOne.setAngles(names, angles2, 0.06)
            time.sleep(0.9)
            motionProxyOne.setAngles(names, angles3, 0.06)
            time.sleep(1.6)
#        motionProxyOne.setAngles(names, angles, 0.06)
#        time.sleep(0.6)
#        motionProxyOne.setAngles(names, angles1, 0.06)
#        time.sleep(0.6)
#        motionProxyOne.setAngles(names, angles2, 0.06)
#        time.sleep(1.6)

        while n < 5:  # 扭腰
            n = n + 1
            motionProxyOne.setAngles(names, angles4, 0.1)  # 扭腰
            time.sleep(0.7)
            motionProxyOne.setAngles(names, angles5, 0.1)
            time.sleep(0.7)
            motionProxyOne.setAngles(names, angles6, 0.1)
            time.sleep(1.2)
        motionProxyOne.setAngles(names, angles4, 0.1)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableBalanceConstraint [as 别名]

#.........这里部分代码省略.........
            # Compute the position of the ball relative to the robot
            self.ball[0] = camera_pos[2] * tan(camera_pos[4] + ball_info[1] + ball_info[2]) + camera_pos[0]
            self.ball[1] = camera_pos[2] * tan(camera_pos[3]) + camera_pos[1]

            # Compute the distance between the robot and the ball
            dist = camera_pos[2] * tan(camera_pos[4] + ball_info[1])
            # Compare to the euclidean distance
            dist_eucl = sqrt(self.ball[0]**2 + self.ball[1]**2)
            print("Distance difference: "+str(dist - dist_eucl))

            # If the robot is near the ball enough (set to 2 centimeters here)
            if dist < 0.02:
                # Stop the tracker
                self.tracker.stopTracker()
                # TODO: Request the robot to look for the landmark

        # Warn that we found a red ball
        print("RED BALL DETECTED " + str(self.ball))

    def on_landmark_detected(self):
        """Compute the position of the landmark relative to the robot"""

        # If the tracker is stopped or not following the landmark
        if not self.tracker.isActive() or self.tracker.getActiveTarget() != "LandMark":
            # Stop the tracker
            self.tracker.stopTracker()

            # Ask the robot to track the target by moving only its body
            self.tracker.setMode("WholeBody")
            # Start to track the landmark
            self.tracker.track("LandMark")
        else:
            # Get the data from the memory
            data = memory.getData("LandmarkDetected")
            # Check that we have the right mark
            if data[1][0][1] == mark_id:
                # Extract information regarding the mark
                mark_info = data[1][0][0]
                camera_pos = data[3]
                # Compute the position of the landmark relative to the robot
                self.goal[0] = camera_pos[2] * tan(camera_pos[4] + mark_info[2] + mark_info[3]) + camera_pos[0]
                self.goal[1] = camera_pos[2] * tan(camera_pos[3]) + camera_pos[1]
                # Turn the robot toward the goal
                rotation = atan((self.goal[0] - self.ball[0]) / (self.goal[1] - self.ball[1]))

                # If the robot is facing the landmark with a 2 degree precision
                if -2 * almath.TO_RAD < rotation < 2 * almath.TO_RAD:
                    # Stop the tracker
                    self.tracker.stopTracker()
                    # TODO: Request the robot to kick

                # Warn that we found a landmark
                print("LANDMARK DETECTED " + str(self.goal))

    def on_fall(self):
        """Set the stiffness back and request the robot to get up"""

        # Set the body stiffness
        self.motion.setStiffnesses("Body", 1)
        # Ask the robot to stand up
        self.posture.goToPosture("StandInit", 0.5)

    def on_bumper_pressed(self):
        """Have the robot turn around and walk away"""

        # Stop all movements
        self.motion.stopMove()
        # Turn around
        self.motion.moveTo(0, 0, pi / 2)

    def kick(self):
        """Have the robot execute a kick"""

        # Request robot to go to stand init posture
        self.posture.goToPosture("StandInit", 0.5)

        # Activate whole body balancer
        self.motion.wbEnable(True)

        # Constraint both legs as fixed
        self.motion.wbFootState("Fixed", "Legs")

        # Request legs joints to move to keep robot's balance
        self.motion.wbEnableBalanceConstraint(True, "Legs")

        # Balance the robot on the left leg
        self.motion.wbGoToBalance("LLeg", 2.5)

        # Remove fixed constraint from the right leg
        self.motion.wbFootState("Free", "RLeg")

        # Compute the path for moving the right leg back and forth
        path = compute_path(self.motion, "RLeg", motion.FRAME_WORLD)

        # Request the right leg to move following the computed path
        times = [1.2, 1.6, 2.3]
        self.motion.transformInterpolations("RLeg", motion.FRAME_WORLD, path, 63, times)

        # Request the robot to stand in initial position
        self.posture.goToPosture("StandInit", 0.2)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableBalanceConstraint [as 别名]
def kick():
    motionProxy  = ALProxy("ALMotion", nao_host, nao_port)
    postureProxy = ALProxy("ALRobotPosture", nao_host, nao_port)
    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Activate Whole Body Balancer
    isEnabled  = True
    motionProxy.wbEnable(isEnabled)

    # Legs are constrained fixed
    stateName  = "Fixed"
    supportLeg = "Legs"
    motionProxy.wbFootState(stateName, supportLeg)

    # Constraint Balance Motion
    isEnable   = True
    supportLeg = "Legs"
    motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)

    # Com go to LLeg
    supportLeg = "LLeg"
    duration   = 2.0
    motionProxy.wbGoToBalance(supportLeg, duration)

    # RLeg is free
    stateName  = "Free"
    supportLeg = "RLeg"
    motionProxy.wbFootState(stateName, supportLeg)

    # RLeg is optimized
    effector = "RLeg"
    axisMask = 63
    frame    = motion.FRAME_WORLD

    # Motion of the RLeg
    times   = [2.0, 2.7, 4.5]

    path = computePath(motionProxy, effector, frame)

    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    # Example showing how to Enable Effector Control as an Optimization
    isActive     = False
    motionProxy.wbEnableEffectorOptimization(effector, isActive)

    # Com go to LLeg
    supportLeg = "RLeg"
    duration   = 2.0
    motionProxy.wbGoToBalance(supportLeg, duration)

    # RLeg is free
    stateName  = "Free"
    supportLeg = "LLeg"
    motionProxy.wbFootState(stateName, supportLeg)

    effector = "LLeg"
    path = computePath(motionProxy, effector, frame)
    motionProxy.transformInterpolations(effector, frame, path, axisMask, times)

    time.sleep(1.0)

    # Deactivate Head tracking
    isEnabled = False
    motionProxy.wbEnable(isEnabled)

    # send robot to Pose Init
    postureProxy.goToPosture("StandInit", 0.3)

    # Go to rest position
    motionProxy.rest()
    return "ZzzZzzZzzZ", 200

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnableBalanceConstraint [as 别名]
def main(robotIP, PORT=9559):
    '''
        Example of a whole body multiple effectors control "LArm", "RArm" and "Torso"
        Warning: Needs a PoseInit before executing
                 Whole body balancer must be inactivated at the end of the script
    '''

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # end initialize proxy, begin go to Stand Init

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    id = postureProxy.post.goToPosture("Stand", 0.5)
    postureProxy.wait(id, 0)

    # end go to Stand Init, begin initialize whole body

    # Enable Whole Body Balancer
    isEnabled  = True
    motionProxy.wbEnable(isEnabled)

    # Legs are constrained fixed
    stateName  = "Fixed"
    supportLeg = "Legs"
    motionProxy.wbFootState(stateName, supportLeg)

    # Constraint Balance Motion
    isEnable   = True
    supportLeg = "Legs"
    motionProxy.wbEnableBalanceConstraint(isEnable, supportLeg)

    # end initialize whole body, define arms motions

    useSensorValues = False

    # ***************************************************************

    # effectorList = ["LArm"]
    # arm = "LArm"
    # axisDirection = 1    # +1 for LArm, -1 for RArm
    # frame = motion.FRAME_WORLD
    # currentTf = motionProxy.getTransform(arm, frame, useSensorValues)

    for i in range(5):



        print i

        # Arms motion
        effectorList = ["LArm"]
        arm = "LArm"
        axisDirection = 1    # +1 for LArm, -1 for RArm

        frame = motion.FRAME_WORLD
        pathArm = []
        
        currentTf = motionProxy.getTransform(arm, frame, useSensorValues)

        # 1 - arm ready out front
        target1Tf = almath.Transform(currentTf)
        target1Tf.r1_c4 += 0.05 # x
        target1Tf.r2_c4 += 0.00 * axisDirection # y
        target1Tf.r3_c4 += 0.00 # z

        # 2 - arm back
        target2Tf = almath.Transform(currentTf)
        target2Tf.r1_c4 += 0.00
        target2Tf.r2_c4 += 0.15
        target2Tf.r3_c4 += 0.15

        # 3 - arm to ball using ball.y
        target3Tf = almath.Transform(currentTf)
        target3Tf.r1_c4 += 0.05
        target3Tf.r2_c4 += 0.00 * axisDirection
        target3Tf.r3_c4 += 0.10

        pathArm.append(list(target1Tf.toVector()))
        pathArm.append(list(target2Tf.toVector()))
        pathArm.append(list(target3Tf.toVector()))

        pathList = [pathArm]

        axisMaskList = [almath.AXIS_MASK_VEL]

        coef = 1.5
        timesList = [coef * (i + 1) for i in range(len(pathArm))]

        # And move!
        id = motionProxy.post.transformInterpolations(effectorList, frame, pathArm, axisMaskList, timesList)
        motionProxy.wait(id, 0)

        # It is necessary to return the robot to the start position so the next target
        # positions are not added to the last move position.
        # id = postureProxy.post.goToPosture("Stand", 0.75)
        # postureProxy.wait(id, 0)
#.........这里部分代码省略.........