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

# Cartesian commands

<< return to Python examples

This section contains examples showing cartesian commands on NAO’s body.

## Arms

Arm trajectories examples.

### Trajectory 1

`almotion_cartesianArm1.py`

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

    """Example: Use transformInterpolations Method on Arm"""

    import qi
    import argparse
    import sys
    import motion
    import almath

    def main(session):
        """
        Use case of transformInterpolations API.
        """
        # 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)

        effector   = "LArm"
        frame      = motion.FRAME_TORSO
        axisMask   = almath.AXIS_MASK_VEL # just control position
        useSensorValues = False

        path = []
        currentTf = motion_service.getTransform(effector, frame, useSensorValues)
        targetTf  = almath.Transform(currentTf)
        targetTf.r1_c4 += 0.03 # x
        targetTf.r2_c4 += 0.03 # y

        path.append(list(targetTf.toVector()))
        path.append(currentTf)

        # Go to the target and back again
        times      = [2.0, 4.0] # seconds

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

        # 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)

### Trajectory 2

`almotion_cartesianArm2.py`

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

    """Example: Use transformInterpolations Method on Arm"""

    import qi
    import argparse
    import sys
    import motion
    import almath

    def main(session):
        """
        Use transformInterpolations Method on Arm
        """
        # 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)

        effector   = "LArm"
        frame      = motion.FRAME_TORSO
        axisMask   = almath.AXIS_MASK_VEL    # just control position
        useSensorValues = False

        path = []
        currentTf = motion_service.getTransform(effector, frame, useSensorValues)
        # point 1
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 -= 0.05 # y
        path.append(list(targetTf.toVector()))

        # point 2
        targetTf  = almath.Transform(currentTf)
        targetTf.r3_c4 += 0.04 # z
        path.append(list(targetTf.toVector()))

        # point 3
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 += 0.04 # y
        path.append(list(targetTf.toVector()))

        # point 4
        targetTf  = almath.Transform(currentTf)
        targetTf.r3_c4 -= 0.02 # z
        path.append(list(targetTf.toVector()))

        # point 5
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 -= 0.05 # y
        path.append(list(targetTf.toVector()))

        # point 6
        path.append(currentTf)

        times = [0.5, 1.0, 2.0, 3.0, 4.0, 4.5] # seconds

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

        # 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)

## Feet

Control NAO’s left foot.

`almotion_cartesianFoot.py`

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

    """Example: Use transformInterpolations Method on Arm on Foot"""

    import qi
    import argparse
    import sys
    import almath
    import motion

    def main(session):
        """
        Use transformInterpolations Method on Foot.
        """
        # 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)

        frame      = motion.FRAME_WORLD
        axisMask   = almath.AXIS_MASK_ALL   # full control
        useSensorValues = False

        # Lower the Torso and move to the side
        effector = "Torso"
        initTf   = almath.Transform(
            motion_service.getTransform(effector, frame, useSensorValues))
        deltaTf  = almath.Transform(0.0, -0.06, -0.03) # x, y, z
        targetTf = initTf*deltaTf
        path     = list(targetTf.toVector())
        times    = 2.0 # seconds
        motion_service.transformInterpolations(effector, frame, path, axisMask, times)

        # LLeg motion
        effector = "LLeg"
        initTf = almath.Transform()

        try:
            initTf = almath.Transform(motion_service.getTransform(effector, frame, useSensorValues))
        except Exception, errorMsg:
            print str(errorMsg)
            print "This example is not allowed on this robot."
            exit()

        # rotation Z
        deltaTf  = almath.Transform(0.0, 0.04, 0.0)*almath.Transform().fromRotZ(45.0*almath.TO_RAD)
        targetTf = initTf*deltaTf
        path     = list(targetTf.toVector())
        times    = 2.0 # seconds

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

        # 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)

## Torso

Make NAO’s torso take different positions.

### Trajectory

`almotion_cartesianTorso.py`

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

    """Example: Use transformInterpolations Method on Torso"""

    import qi
    import argparse
    import sys
    import almath
    import motion

    def main(session):
        """
        Use transformInterpolations Method on Torso.
        """
        # 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)

        effector   = "Torso"
        frame      =  motion.FRAME_WORLD
        axisMask   = almath.AXIS_MASK_ALL # full control
        useSensorValues = False

        # Define the changes relative to the current position
        dx         = 0.045 # translation axis X (meter)
        dy         = 0.050 # translation axis Y (meter)

        path = []
        currentTf = motion_service.getTransform(effector, frame, useSensorValues)

        # point 1
        targetTf  = almath.Transform(currentTf)
        targetTf.r1_c4 += dx
        path.append(list(targetTf.toVector()))

        # point 2
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 -= dy
        path.append(list(targetTf.toVector()))

        # point 3
        targetTf  = almath.Transform(currentTf)
        targetTf.r1_c4 -= dx
        path.append(list(targetTf.toVector()))

        # point 4
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 += dy
        path.append(list(targetTf.toVector()))

        # point 5
        targetTf  = almath.Transform(currentTf)
        targetTf.r1_c4 += dx
        path.append(list(targetTf.toVector()))

        # point 6
        path.append(currentTf)

        times = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # seconds

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

        # 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)

### Hula Hoop

`almotion_hulaHoop.py`

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

    """Example: Use transformInterpolations Method to play short animation"""

    import qi
    import argparse
    import sys
    import almath
    import motion

    def main(session):
        """
        Use transformInterpolations Method to play short animation.
        This example will only work on Nao.
        """
        # Get the services ALMotion & ALRobotPosture.

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

        # end initialize proxy, begin go to Stand Init

        # Wake up robot
        motion_service.wakeUp()

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

        # end go to Stand Init, begin define control point
        effector        = "Torso"
        frame           =  motion.FRAME_ROBOT
        axisMask        = almath.AXIS_MASK_ALL
        useSensorValues = False

        currentTf = almath.Transform(motion_service.getTransform(effector, frame, useSensorValues))

        # end define control point, begin define target

        # Define the changes relative to the current position
        dx         = 0.03                    # translation axis X (meter)
        dy         = 0.03                    # translation axis Y (meter)
        dwx        = 8.0*almath.TO_RAD       # rotation axis X (rad)
        dwy        = 8.0*almath.TO_RAD       # rotation axis Y (rad)

        # point 01 : forward  / bend backward
        target1Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
        target1Tf *= almath.Transform(dx, 0.0, 0.0)
        target1Tf *= almath.Transform().fromRotY(-dwy)

        # point 02 : right    / bend left
        target2Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
        target2Tf *= almath.Transform(0.0, -dy, 0.0)
        target2Tf *= almath.Transform().fromRotX(-dwx)

        # point 03 : backward / bend forward
        target3Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
        target3Tf *= almath.Transform(-dx, 0.0, 0.0)
        target3Tf *= almath.Transform().fromRotY(dwy)

        # point 04 : left     / bend right
        target4Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
        target4Tf *= almath.Transform(0.0, dy, 0.0)
        target4Tf *= almath.Transform().fromRotX(dwx)

        path = []
        path.append(list(target1Tf.toVector()))
        path.append(list(target2Tf.toVector()))
        path.append(list(target3Tf.toVector()))
        path.append(list(target4Tf.toVector()))

        path.append(list(target1Tf.toVector()))
        path.append(list(target2Tf.toVector()))
        path.append(list(target3Tf.toVector()))
        path.append(list(target4Tf.toVector()))

        path.append(list(target1Tf.toVector()))
        path.append(list(currentTf.toVector()))

        timeOneMove  = 0.5 #seconds
        times = []
        for i in range(len(path)):
            times.append((i+1)*timeOneMove)

        # end define target, begin call motion api

        # call the cartesian control API

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

        # 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 and torso

Control multiple effectors with cartesian commands.

### Trajectory 1

`almotion_cartesianTorsoArm1.py`

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

    """Example: Use transformInterpolations Method on Arm and Torso"""

    import qi
    import argparse
    import sys
    import almath
    import motion

    def main(session):
        """
        Use transformInterpolations Method on Arm and Torso
        """
        # 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)

        frame      = motion.FRAME_WORLD
        coef       = 0.5                   # motion speed
        times      = [coef, 2.0*coef, 3.0*coef, 4.0*coef]
        useSensorValues = False

        # Relative movement between current and desired positions
        dy         = +0.03                 # translation axis Y (meters)
        dz         = -0.03                 # translation axis Z (meters)
        dwx        = +8.0*almath.TO_RAD   # rotation axis X (radians)

        # Motion of Torso with _async process
        effector   = "Torso"

        path = []
        initTf = almath.Transform(motion_service.getTransform(effector, frame, useSensorValues))
        # point 1
        deltaTf  = almath.Transform(0.0, -dy, dz)*almath.Transform().fromRotX(-dwx)
        targetTf = initTf*deltaTf
        path.append(list(targetTf.toVector()))

        # point 2
        path.append(list(initTf.toVector()))

        # point 3
        deltaTf  = almath.Transform(0.0, dy, dz)*almath.Transform().fromRotX(dwx)
        targetTf = initTf*deltaTf
        path.append(list(targetTf.toVector()))

        # point 4
        path.append(list(initTf.toVector()))

        axisMask   = almath.AXIS_MASK_ALL  # control all the effector axes
        motion_service.transformInterpolations(effector, frame, path,
                                               axisMask, times, _async=True)

        # Motion of Arms with block process
        frame     = motion.FRAME_TORSO
        axisMask  = almath.AXIS_MASK_VEL  # control just the position
        times     = [1.0*coef, 2.0*coef]  # seconds

        # Motion of Right Arm during the first half of the Torso motion
        effector  = "RArm"

        path = []
        currentTf = motion_service.getTransform(effector, frame, useSensorValues)
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 -= 0.04 # y
        path.append(list(targetTf.toVector()))
        path.append(currentTf)

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

        # Motion of Left Arm during the last half of the Torso motion
        effector   = "LArm"

        path = []
        currentTf = motion_service.getTransform(effector, frame, useSensorValues)
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 += 0.04 # y
        path.append(list(targetTf.toVector()))
        path.append(currentTf)

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

        # 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)

### Trajectory 2

`almotion_cartesianTorsoArm2.py`

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

    """Example: Use transformInterpolations Method on Arm and Torso"""

    import qi
    import argparse
    import sys
    import motion
    import almath

    def main(session):
        """
        Use transformInterpolations Method on Arm and Torso.
        """
        # 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)

        frame      = motion.FRAME_ROBOT
        useSensorValues = False

        effectorList = ["LArm", "RArm"]

        # Motion of Arms with block process
        axisMaskList = [almath.AXIS_MASK_VEL, almath.AXIS_MASK_VEL]

        timesList    = [[1.0], [1.0]] # seconds

        # LArm path
        pathLArm = []
        targetTf  = almath.Transform(motion_service.getTransform("LArm", frame, useSensorValues))
        targetTf.r2_c4 -= 0.04 # y
        pathLArm.append(list(targetTf.toVector()))

        # RArm path
        pathRArm = []
        targetTf  = almath.Transform(motion_service.getTransform("RArm", frame, useSensorValues))
        targetTf.r2_c4 += 0.04 # y
        pathRArm.append(list(targetTf.toVector()))

        pathList = []
        pathList.append(pathLArm)
        pathList.append(pathRArm)

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

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

        # Motion of Arms and Torso with block process
        axisMaskList = [almath.AXIS_MASK_VEL,
                        almath.AXIS_MASK_VEL,
                        almath.AXIS_MASK_ALL]

        timesList    = [[4.0],                  # LArm  in seconds
                        [4.0],                  # RArm  in seconds
                        [1.0, 2.0, 3.0, 4.0]]   # Torso in seconds

        # LArm path
        pathLArm = []
        pathLArm.append(motion_service.getTransform("LArm", frame, useSensorValues))

        # RArm path
        pathRArm = []
        pathRArm.append(motion_service.getTransform("RArm", frame, useSensorValues))

        # Torso path
        pathTorso = []
        currentTf = motion_service.getTransform("Torso", frame, useSensorValues)

        # 1
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 += 0.04 # y
        pathTorso.append(list(targetTf.toVector()))

        # 2
        targetTf  = almath.Transform(currentTf)
        targetTf.r1_c4 -= 0.03 # x
        pathTorso.append(list(targetTf.toVector()))

        # 3
        targetTf  = almath.Transform(currentTf)
        targetTf.r2_c4 -= 0.04 # y
        pathTorso.append(list(targetTf.toVector()))

        # 4
        pathTorso.append(currentTf)

        pathList = []
        pathList.append(pathLArm)
        pathList.append(pathRArm)
        pathList.append(pathTorso)

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

        # 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