Python ALProxy.wbEnable方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnable [as 别名]
class TestModule(ALModule):
    """A module used for testing Nao's functionality"""

    def __init__(self, var_name):
        """Define the module's attributes and initialize their value"""

        # Request the parent module to initialize and register the module
        ALModule.__init__(self, var_name)

        # Declare the memory
        global memory
        memory = ALProxy("ALMemory")

        # Initialize a motion module
        self.motion = ALProxy("ALMotion")
        # Initialize a posture module
        self.posture = ALProxy("ALRobotPosture")

    def initialize(self):
        """Initialize the inner modules of the module"""

        # Wake the robot up
        self.motion.wakeUp()
        # Enable the whole body motion
        self.motion.wbEnable(True)

    def shutdown(self):
        """Shut the module down gracefully"""

        # Disable the whole body motion
        self.motion.wbEnable(False)
        # Put the robot to rest
        self.motion.rest()

    def kick(self):
        """Define the procedure to follow to accomplish a kick"""

        # Go to stand init posture
        self.posture.goToPosture("StandInit", 0.5)

        # Define the joints we are interested in
        hip_joints = ["RHipRoll", "LHipRoll"]

        # Get the angles for HipRolls
        angles = self.motion.getAngles(hip_joints, True)

        # Modify the value of the hip rolls
        angles[0] -= 10 * almath.TO_RAD
        angles[1] += 10 * almath.TO_RAD

        # Set the angles to have Nao stand on one foot
        self.motion.setAngles(hip_joints, angles, 0.2)

        # Wait for some time
        sleep(5)

        # Go back to stand init posture
        self.posture.goToPosture("StandInit", 0.5)

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

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

    def sidestep(self, value):
        self.motionProxy.setWalkTargetVelocity(0.0,value,0.0,1.0,
                [#LEFT
                ["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)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnable [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 wbEnable [as 别名]
names  = ["RHipPitch","RKneePitch","RHipRoll","RAnklePitch","RAnkleRoll","LAnkleRoll","LHipRoll","LKneePitch","LAnklePitch"]
angles = [0*almath.TO_RAD,-5*almath.TO_RAD,5*almath.TO_RAD,0.05,0.1,-0.13,0.1,10*almath.TO_RAD,-0.09]
fractionMaxSpeed  = 0.1
motionProxy.angleInterpolationWithSpeed(names, angles, fractionMaxSpeed)
lowerReference()
'''


robot_ip = '192.168.1.100'
robot_port = 9559
motionProxy=ALProxy("ALMotion",robot_ip,robot_port)
postureProxy = ALProxy("ALRobotPosture", robot_ip,robot_port)
motionProxy.wakeUp()
time.sleep(1)
lowerReference()
motionProxy.wbEnable(True)
supportLeg = "RLeg"
duration   = 1.0
motionProxy.wbGoToBalance(supportLeg, duration)
motionProxy.wbEnable(False)
'''
names  = ["LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll","RHipPitch","RHipRoll","RKneePitch","RAnklePitch","RAnkleRoll"]
angles = [0.2,0.13,-0.09,0.09,-0.23,0.1,0.13,-0.09,0.09,-0.2]
fractionMaxSpeed  = 0.1
motionProxy.angleInterpolationWithSpeed(names, angles, fractionMaxSpeed)
'''

names  = ["LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll","RHipPitch","RHipRoll","RKneePitch","RAnklePitch","RAnkleRoll"]
angles = [0.3,0.23,0.23,-0.23,-0.3,0.1,0.13,-0.09,0,-0.23]
fractionMaxSpeed  = 0.1
motionProxy.angleInterpolationWithSpeed(names, angles, fractionMaxSpeed)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnable [as 别名]
            ############## Setup for side kick ###############
            ##################################################
            if(i < 10):
                i += 1
                motionProxy.setWalkTargetVelocity(1, 0, -.2, 0.2)
            elif (i == 10):
                print "side kick"
                motionProxy.setWalkTargetVelocity(0, 0, 0, 0)
                # Get to a stable posture first
                postureProxy.goToPosture("StandInit", 0.8)
                    #wait a second to gain composure
                time2.sleep(1)
                #then kick
                # Activate Whole Body Balancer
                isEnabled  = True
                motionProxy.wbEnable(isEnabled)

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

                # Motion of the RLeg and LLeg
                # Motion of the RLeg and LLeg
                dx      = 0.0                 # translation axis X (meters)
                dy      = 0.25                   # translation axis Y (meters)
                dz      = 0.01                 # translation axis Z (meters)

                dwx     = 10.0*math.pi/180.0     # rotation axis X (radian)
                dwy     = 0.0                   # rotation axis Y (radian) Original: 5.0*math.pi/180.0
                dwz     = 0.0

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnable [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)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnable [as 别名]
class SoccerModule(ALModule):
    """A simple module allowing Nao to play soccer"""

    def __init__(self, var_name):
        """Define and initialize the class' attributes"""

        # Request the parent module to initialize and register our module
        ALModule.__init__(self, var_name)

        # Initialize the memory
        global memory
        memory = ALProxy("ALMemory")
        # Initialize the motion module
        self.motion = ALProxy("ALMotion")
        # Initialize the posture module
        self.posture = ALProxy("ALRobotPosture")
        # Initialize the tracker module
        self.tracker = ALProxy("ALTracker")

        # Initialize the goal position
        self.goal = [0, 0]
        # Initialize the ball position
        self.ball = [0, 0]

    def initialize(self):
        """Subscribe to the required events and enable the different sub-modules"""

        # Enable the fall manager
        self.motion.setFallManagerEnabled(True)
        # Enable the body balancer
        self.motion.wbEnable(True)

        # Wake the robot up
        self.motion.wakeUp()
        # Request the robot to stand
        self.posture.goToPosture("StandInit", 0.5)

        # Wait for some time
        sleep(1)

        # Subscribe to the redBallDetected event
        memory.subscribeToEvent("redBallDetected", self.getName(), "on_red_ball")
        # Subscribe to the robot has fallen event
        memory.subscribeToEvent("robotHasFallen", self.getName(), "on_fall")
        # Subscribe to the landmarkDetected event
        memory.subscribeToEvent("LandmarkDetected", self.getName(), "on_landmark_detected")

        # Register the red ball and landmark as targets for the tracker module
        self.tracker.registerTarget("RedBall", 0.5)  # The second parameter is the distance to keep with the ball
        self.tracker.registerTarget("LandMark", [0.2, [85]])  # The second parameter is the target's size, followed by the mark id

        # Fix the tracker's limits
        self.tracker.setMaximumDistanceDetection(0.5)  # Objects more than 50 centimeters away are not considered
        self.tracker.setTimeOut(2000)  # Objects not detected for more than 2 sec are lost

        # Have the robot look for the targets if they are lost
        self.tracker.toggleSearch(True)

        # Have the robot track the red ball
        self.tracker.track("RedBall")

    def shutdown(self):
        """Define the procedure to follow upon shutting down"""

        # If the tracker is still running
        if self.tracker.isActive():
            # Shut the tracker down
            self.tracker.stopTracker()

        # Unregister all tracker's targets
        self.tracker.unregisterAllTargets()
        # Request the robot to go in resting position
        self.motion.rest()
        # Set the body stiffness to 0
        self.motion.setStiffnesses("Body", 0)

    def on_red_ball(self):
        """Compute the position of the ball relative to the robot and move towards it"""

        # Check if the tracker is stopped or not tracking the red ball
        if not self.tracker.isActive() or self.tracker.getActiveTarget() != "RedBall":
            # Stop the tracker
            self.tracker.stopTracker()

            # Ask the robot to track the target by moving towards it
            self.tracker.setMode("Move")
            # Start to track the red ball
            self.tracker.track("RedBall")
        else:
            # Get the data from the memory
            # Apparently the robot blocks all calls here because the data query is really slow.
            data = memory.getData("redBallDetected")
            ball_info = data[1]
            camera_pos = data[3]

            # 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
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import wbEnable [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