# SoftBank Robotics documentation What's new in NAOqi 2.8?

# Whole body motion

<< return to Python examples

This section contains examples showing various possibilities of Whole Body Motion.

## Effector control

Control NAO’s effectors, separately or simultaneously.

### Head

`almotion_wbEffectorControlHead.py`

    #! /usr/bin/env python
    # -*- encoding: UTF-8 -*-

    """Example: Whole Body Motion - Head orientation control"""

    import qi
    import argparse
    import sys
    import time
    import math

    def main(session):
        """
        Example of a whole body head orientation control.
        Warning: Needs a PoseInit before executing.
                Whole body balancer must be deactivated at the end of the script.
        This example is only compatible with NAO.
        """
        # Get the services ALMotion & ALRobotPosture.

        motion_service = session.service("ALMotion")
        posture_service = session.service("ALRobotPosture")

        # Wake up robot
        motion_service.wakeUp()

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

        effectorName = "Head"

        # Active Head tracking
        isEnabled    = True
        motion_service.wbEnableEffectorControl(effectorName, isEnabled)

        # Example showing how to set orientation target for Head tracking
        # The 3 coordinates are absolute head orientation in NAO_SPACE
        # Rotation in RAD in x, y and z axis

        # X Axis Head Orientation feasible movement = [-20.0, +20.0] degree
        # Y Axis Head Orientation feasible movement = [-75.0, +70.0] degree
        # Z Axis Head Orientation feasible movement = [-30.0, +30.0] degree

        targetCoordinateList = [
        [+20.0,  00.0,  00.0], # target 0
        [-20.0,  00.0,  00.0], # target 1
        [ 00.0, +70.0,  00.0], # target 2
        [ 00.0, +70.0, +30.0], # target 3
        [ 00.0, +70.0, -30.0], # target 4
        [ 00.0, -75.0,  00.0], # target 5
        [ 00.0, -75.0, +30.0], # target 6
        [ 00.0, -75.0, -30.0], # target 7
        [ 00.0,  00.0,  00.0], # target 8
        ]

        # wbSetEffectorControl is a non blocking function
        # time.sleep allow head go to his target
        # The recommended minimum period between two successives set commands is
        # 0.2 s.
        for targetCoordinate in targetCoordinateList:
            targetCoordinate = [target*math.pi/180.0 for target in targetCoordinate]
            motion_service.wbSetEffectorControl(effectorName, targetCoordinate)
            time.sleep(3.0)

        # Deactivate Head tracking
        isEnabled = False
        motion_service.wbEnableEffectorControl(effectorName, isEnabled)

        # Go to rest position
        motion_service.rest()

    if __name__ == "__main__":
        parser = argparse.ArgumentParser()
        parser.add_argument("--ip", type=str, default="127.0.0.1",
                            help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
        parser.add_argument("--port", type=int, default=9559,
                            help="Naoqi port number")

        args = parser.parse_args()
        session = qi.Session()
        try:
            session.connect("tcp://" + args.ip + ":" + str(args.port))
        except RuntimeError:
            print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
                   "Please check your script arguments. Run with -h option for help.")
            sys.exit(1)
        main(session)

### Arms

`almotion_wbEffectorControlLArm.py`

    #! /usr/bin/env python
    # -*- encoding: UTF-8 -*-

    """Example: Whole Body Motion - Left or Right Arm position control"""

    import qi
    import argparse
    import sys
    import motion
    import time

    def main(session, chainName):
        """
        Example of a whole body Left or Right Arm position control
        Warning: Needs a PoseInit before executing
                Whole body balancer must be inactivated at the end of the script
        This example is only compatible with NAO
        """
        # Get the service ALMotion.

        motion_service = session.service("ALMotion")

        # Wake up robot
        motion_service.wakeUp()

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

        frame     = motion.FRAME_ROBOT
        useSensor = False

        effectorInit = motion_service.getPosition(chainName, frame, useSensor)

        # Active LArm tracking
        isEnabled = True
        motion_service.wbEnableEffectorControl(chainName, isEnabled)

        # Example showing how to set position target for LArm
        # The 3 coordinates are absolute LArm position in FRAME_ROBOT
        # Position in meter in x, y and z axis.

        # X Axis LArm Position feasible movement = [ +0.00, +0.12] meter
        # Y Axis LArm Position feasible movement = [ -0.05, +0.10] meter
        # Y Axis RArm Position feasible movement = [ -0.10, +0.05] meter
        # Z Axis LArm Position feasible movement = [ -0.10, +0.10] meter

        coef = 1.0
        if chainName == "LArm":
            coef = +1.0
        elif chainName == "RArm":
            coef = -1.0

        targetCoordinateList = [
        [ +0.12, +0.00*coef, +0.00], # target 0
        [ +0.12, +0.00*coef, -0.10], # target 1
        [ +0.12, +0.05*coef, -0.10], # target 1
        [ +0.12, +0.05*coef, +0.10], # target 2
        [ +0.12, -0.10*coef, +0.10], # target 3
        [ +0.12, -0.10*coef, -0.10], # target 4
        [ +0.12, +0.00*coef, -0.10], # target 5
        [ +0.12, +0.00*coef, +0.00], # target 6
        [ +0.00, +0.00*coef, +0.00], # target 7
        ]

        # wbSetEffectorControl is a non blocking function
        # time.sleep allow head go to his target
        # The recommended minimum period between two successives set commands is
        # 0.2 s.
        for targetCoordinate in targetCoordinateList:
            targetCoordinate = [targetCoordinate[i] + effectorInit[i] for i in range(3)]
            motion_service.wbSetEffectorControl(chainName, targetCoordinate)
            time.sleep(4.0)

        # Deactivate Head tracking
        isEnabled    = False
        motion_service.wbEnableEffectorControl(chainName, isEnabled)

        # Go to rest position
        motion_service.rest()

    if __name__ == "__main__":
        parser = argparse.ArgumentParser()
        parser.add_argument("--ip", type=str, default="127.0.0.1",
                            help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
        parser.add_argument("--port", type=int, default=9559,
                            help="Naoqi port number")
        parser.add_argument("--chain", type=str, default="LArm",
                            choices=["LArm", "RArm"], help="Chain name")

        args = parser.parse_args()
        session = qi.Session()
        try:
            session.connect("tcp://" + args.ip + ":" + str(args.port))
        except RuntimeError:
            print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
                   "Please check your script arguments. Run with -h option for help.")
            sys.exit(1)
        main(session, args.chain)

### Multiple Effectors

Control arms and torso simultaneously.

`almotion_wbMultipleEffectors.py`

    #! /usr/bin/env python
    # -*- encoding: UTF-8 -*-

    """Example: Whole Body Motion - Multiple Effectors control """

    import qi
    import argparse
    import sys
    import almath
    import motion

    def main(session):
        """
            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.
        """
        # Get the services ALMotion & ALRobotPosture.

        motion_service = session.service("ALMotion")
        posture_service = session.service("ALRobotPosture")

        # Wake up robot
        motion_service.wakeUp()

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

        # end go to Stand Init, begin initialize whole body

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

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

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

        # end initialize whole body, define arms motions

        useSensorValues = False

        # Arms motion
        effectorList = ["LArm", "RArm"]

        frame        = motion.FRAME_ROBOT

        # pathLArm
        pathLArm = []
        currentTf = motion_service.getTransform("LArm", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += 0.08 # y
        target1Tf.r3_c4 += 0.14 # z

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= 0.05 # y
        target2Tf.r3_c4 -= 0.07 # z

        pathLArm.append(list(target1Tf.toVector()))
        pathLArm.append(list(target2Tf.toVector()))
        pathLArm.append(list(target1Tf.toVector()))
        pathLArm.append(list(target2Tf.toVector()))
        pathLArm.append(list(target1Tf.toVector()))

        # pathRArm
        pathRArm = []
        currentTf = motion_service.getTransform("RArm", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += 0.05 # y
        target1Tf.r3_c4 -= 0.07 # z

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= 0.08 # y
        target2Tf.r3_c4 += 0.14 # z

        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))
        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))
        pathRArm.append(list(target1Tf.toVector()))
        pathRArm.append(list(target2Tf.toVector()))

        pathList = [pathLArm, pathRArm]

        axisMaskList = [almath.AXIS_MASK_VEL, # for "LArm"
                        almath.AXIS_MASK_VEL] # for "RArm"

        coef       = 1.5
        timesList  = [ [coef*(i+1) for i in range(5)],  # for "LArm" in seconds
                       [coef*(i+1) for i in range(6)] ] # for "RArm" in seconds

        # called cartesian interpolation
        motion_service.transformInterpolations(effectorList, frame, pathList, axisMaskList, timesList)

        # end define arms motions, define torso motion

        # Torso Motion
        effectorList = ["Torso", "LArm", "RArm"]

        dy = 0.06
        dz = 0.06

        # pathTorso
        currentTf = motion_service.getTransform("Torso", frame, useSensorValues)
        # 1
        target1Tf  = almath.Transform(currentTf)
        target1Tf.r2_c4 += dy
        target1Tf.r3_c4 -= dz

        # 2
        target2Tf  = almath.Transform(currentTf)
        target2Tf.r2_c4 -= dy
        target2Tf.r3_c4 -= dz

        pathTorso = []
        for i in range(3):
            pathTorso.append(list(target1Tf.toVector()))
            pathTorso.append(currentTf)
            pathTorso.append(list(target2Tf.toVector()))
            pathTorso.append(currentTf)

        pathLArm = [motion_service.getTransform("LArm", frame, useSensorValues)]
        pathRArm = [motion_service.getTransform("RArm", frame, useSensorValues)]

        pathList = [pathTorso, pathLArm, pathRArm]

        axisMaskList = [almath.AXIS_MASK_ALL, # for "Torso"
                        almath.AXIS_MASK_VEL, # for "LArm"
                        almath.AXIS_MASK_VEL] # for "RArm"

        coef       = 0.5
        timesList  = [
                      [coef*(i+1) for i in range(12)], # for "Torso" in seconds
                      [coef*12],                       # for "LArm" in seconds
                      [coef*12]                        # for "RArm" in seconds
                     ]

        motion_service.transformInterpolations(
            effectorList, frame, pathList, axisMaskList, timesList)

        # end define torso motion, disable whole body

        # Deactivate whole body
        isEnabled    = False
        motion_service.wbEnable(isEnabled)

        # Send robot to Pose Init
        posture_service.goToPosture("StandInit", 0.3)

        # Go to rest position
        motion_service.rest()

        # end script

    if __name__ == "__main__":
        parser = argparse.ArgumentParser()
        parser.add_argument("--ip", type=str, default="127.0.0.1",
                            help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
        parser.add_argument("--port", type=int, default=9559,
                            help="Naoqi port number")

        args = parser.parse_args()
        session = qi.Session()
        try:
            session.connect("tcp://" + args.ip + ":" + str(args.port))
        except RuntimeError:
            print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
                   "Please check your script arguments. Run with -h option for help.")
            sys.exit(1)
        main(session)

## Balance

Balance constraints in whole body motion.

### Balance constraint

Enable a balance constraint.

`almotion_wbEnableBalanceConstraint.py`

    #! /usr/bin/env python
    # -*- encoding: UTF-8 -*-

    """Example: Whole Body Motion - Enable Balance Constraint"""

    import qi
    import argparse
    import sys
    import math

    def main(session):
        """
        Example of a whole body Enable Balance Constraint.
        Warning: Needs a PoseInit before executing.
            Whole body balancer must be deactivated at the end of the script.
        This example is only compatible with NAO.
        """
        # Get the services ALMotion & ALRobotPosture.

        motion_service = session.service("ALMotion")
        posture_service = session.service("ALRobotPosture")

        # Wake up robot
        motion_service.wakeUp()

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

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

        # Legs are constrained in a plane
        stateName  = "Fixed"
        supportLeg = "Legs"
        motion_service.wbFootState(stateName, supportLeg)

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

        # KneePitch angleInterpolation
        # Without Whole Body balancer, foot will fall down
        names      = ["LKneePitch", "RKneePitch"]
        angleLists = [ [0.0, 40.0*math.pi/180.0], [0.0, 40.0*math.pi/180.0]]
        timeLists  = [ [5.0, 10.0], [5.0, 10.0]]
        isAbsolute = True
        try:
            motion_service.angleInterpolation(names, angleLists, timeLists, isAbsolute)
        except Exception, errorMsg:
            print str(errorMsg)
            print "This example is not allowed on this robot."
            exit()

        # Deactivate Whole Body Balancer
        isEnabled  = False
        motion_service.wbEnable(isEnabled)

        # Go to rest position
        motion_service.rest()

    if __name__ == "__main__":
        parser = argparse.ArgumentParser()
        parser.add_argument("--ip", type=str, default="127.0.0.1",
                            help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
        parser.add_argument("--port", type=int, default=9559,
                            help="Naoqi port number")

        args = parser.parse_args()
        session = qi.Session()
        try:
            session.connect("tcp://" + args.ip + ":" + str(args.port))
        except RuntimeError:
            print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
                   "Please check your script arguments. Run with -h option for help.")
            sys.exit(1)
        main(session)

### Kick

Make NAO kick the air with its foot.

`almotion_wbKick.py`

    #! /usr/bin/env python
    # -*- encoding: UTF-8 -*-

    """Example: Whole Body Motion - kick"""

    import qi
    import argparse
    import sys
    import motion
    import time
    import almath

    def computePath(motion_service, effector, frame):
        dx      = 0.05                 # translation axis X (meters)
        dz      = 0.05                 # translation axis Z (meters)
        dwy     = 5.0*almath.TO_RAD    # rotation axis Y (radian)

        useSensorValues = False

        path = []
        currentTf = []
        try:
            currentTf = motion_service.getTransform(effector, frame, useSensorValues)
        except Exception, errorMsg:
            print str(errorMsg)
            print "This example is not allowed on this robot."
            exit()

        # 1
        targetTf  = almath.Transform(currentTf)
        targetTf *= almath.Transform(-dx, 0.0, dz)
        targetTf *= almath.Transform().fromRotY(dwy)
        path.append(list(targetTf.toVector()))

        # 2
        targetTf  = almath.Transform(currentTf)
        targetTf *= almath.Transform(dx, 0.0, dz)
        path.append(list(targetTf.toVector()))

        # 3
        path.append(currentTf)

        return path

    def main(session):
        """
        Example of a whole body kick.
        Warning: Needs a PoseInit before executing
                 Whole body balancer must be inactivated at the end of the script.
        This example is only compatible with NAO.
        """
        # Get the services ALMotion & ALRobotPosture.

        motion_service = session.service("ALMotion")
        posture_service = session.service("ALRobotPosture")

        # Wake up robot
        motion_service.wakeUp()

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

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

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

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

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

        # RLeg is free
        stateName  = "Free"
        supportLeg = "RLeg"
        motion_service.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(motion_service, effector, frame)

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

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

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

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

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

        time.sleep(1.0)

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

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

        # Go to rest position
        motion_service.rest()

    if __name__ == "__main__":
        parser = argparse.ArgumentParser()
        parser.add_argument("--ip", type=str, default="127.0.0.1",
                            help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
        parser.add_argument("--port", type=int, default=9559,
                            help="Naoqi port number")

        args = parser.parse_args()
        session = qi.Session()
        try:
            session.connect("tcp://" + args.ip + ":" + str(args.port))
        except RuntimeError:
            print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
                   "Please check your script arguments. Run with -h option for help.")
            sys.exit(1)
        main(session)

## Foot state

Constrain whole body balancer with foot state.

`almotion_wbFootState.py`

    #! /usr/bin/env python
    # -*- encoding: UTF-8 -*-

    """Example: Whole Body Motion - Foot State"""

    import qi
    import argparse
    import sys
    import math

    def main(session):
        """
        Example of a whole body FootState.
        Warning: Needs a PoseInit before executing.
            Whole body balancer must be inactivated at the end of the script.
        This example is only compatible with NAO.
        """
        # Get the services ALMotion & ALRobotPosture.

        motion_service = session.service("ALMotion")
        posture_service = session.service("ALRobotPosture")

        # Wake up robot
        motion_service.wakeUp()

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

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

        # Legs are constrained in a plane
        stateName  = "Plane"
        supportLeg = "Legs"
        motion_service.wbFootState(stateName, supportLeg)

        # HipYawPitch angleInterpolation
        # Without Whole Body balancer, foot will not be keeped plane.
        names      = "LHipYawPitch"
        angleLists = [-45.0, 10.0, 0.0]
        timeLists  = [3.0, 6.0, 9.0]
        isAbsolute = True
        angleLists = [angle*math.pi/180.0 for angle in angleLists]
        try:
            motion_service.angleInterpolation(names, angleLists, timeLists, isAbsolute)
        except Exception, errorMsg:
            print str(errorMsg)
            print "This example is not allowed on this robot."
            exit()

        # Deactivate Whole Body Balancer.
        isEnabled  = False
        motion_service.wbEnable(isEnabled)

        # Go to rest position
        motion_service.rest()

    if __name__ == "__main__":
        parser = argparse.ArgumentParser()
        parser.add_argument("--ip", type=str, default="127.0.0.1",
                            help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
        parser.add_argument("--port", type=int, default=9559,
                            help="Naoqi port number")

        args = parser.parse_args()
        session = qi.Session()
        try:
            session.connect("tcp://" + args.ip + ":" + str(args.port))
        except RuntimeError:
            print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
                   "Please check your script arguments. Run with -h option for help.")
            sys.exit(1)
        main(session)

Glossary Site map Index Support Contact Legal Notices