Aldebaran documentation What's new in NAOqi 2.4.3?

ALMath

<< return to Python examples

This section contains some examples showing how to use libalmath Python wrapping. The C++ documentation of this library can be found here. The wrapping allows to use all functions contained in this library, which is very useful for computations related with motions (effectors positions for example).

Python wrapping

libalmath is wrapped in Python. This makes it possible for example to use this library from Choregraphe, or from any Python script.

To import almath, use the following line (after having correctly configured the Python SDK path if you are coding outside Choregraphe, see Python SDK - Installation Guide ):

import almath

You can now call any method from libalmath with the following line:

almath.methodName(arg0, arg1, ...)

Since libalmath is using particular types, you have to be careful to use them correctly. This can cause some difficulties when interfacing with other modules, such as ALMotion, which do not give the right format directly.

Using ALMath with ALMotion

This example shows how to retrieve transforms from ALMotion using the ALMotionProxy::getTransform method, and how to send transforms computed with ALMath back to ALMotion using the ALMotionProxy::setTransform for example (but the principle is still the same for other methods using transforms).

almath_transform.py

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

"""Example: Show how to use almath with python and send the results to
    the robot by using a proxy to ALMotion"""

import qi
import argparse
import sys
import time
import almath


def main(session):
    """
    Show how to use almath with python and send the results to
    the robot by using a proxy to ALMotion.
    """
    # Get the services AlMotion and ALRobotPosture.

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

    # WakeUp
    motion_service.wakeUp()

    # Stand up.
    posture_service.goToPosture("StandInit", 0.3)

    chainName = "RArm"
    frame = 1 # FRAME_WORLD
    useSensors = True

    ##############################################
    # Retrieve a transform matrix using ALMotion #
    ##############################################

    # Retrieve current transform from ALMotion.
    # Convert it to a transform matrix for ALMath.
    origTransform = almath.Transform(
        motion_service.getTransform(chainName, frame, useSensors))

    # Visualize the transform using overriden print from ALMath
    print "Original transform"
    print origTransform

    ##########################################################
    # Use almath to do some computations on transform matrix #
    ##########################################################

    # Compute a transform corresponding to the desired move
    # (here, move the chain for 5cm along the Z axis and the X axis).
    moveTransform = almath.Transform.fromPosition(0.05, 0.0, 0.05)

    # Compute the corresponding target transform.
    targetTransform = moveTransform * origTransform

    # Visualize it.
    print "Target transform"
    print targetTransform

    ##############################################
    # Send a transform to the robot via ALMotion #
    ##############################################

    # Convert it to a tuple.
    targetTransformList = list(targetTransform.toVector())

    # Send the target transform to NAO through ALMotion.
    fractionOfMaxSpeed = 0.5
    axisMask = almath.AXIS_MASK_VEL # Translation X, Y, Z
    motion_service.setTransforms(
        chainName,
        frame,
        targetTransformList,
        fractionOfMaxSpeed,
        axisMask)

    time.sleep(2.0)
    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)

Using ALMath to generate footsteps

ALMath is widely used inside ALMotion. Using ALMath, you can then reproduce some of the features included in ALMath, in particular concerning footsteps. If you send any footstep to ALMotion, you will get a lot of warnings because they are probably dangerous for the robot: they might cause foot collision or be too long for NAO. If you want to generate footsteps without any warnings, you have to clip them, using ALMath functionnalities.

The following example allows you to lead NAO by its arm. It generates footsteps according to the left arm position, and then clips them to make sure they are possible for NAO.

To use the example, launch the script giving your NAO’s IP as an argument. NAO will stand up. When you are ready, take NAO’s left arm and press the front tactil sensor. You can now guide NAO by inclining its arm forward and backwards, and make him turn by turning his left wrist. NAO’s eyes will turn green when its arm position gives him a target, and blue when the arm position is neutral. To end the example, press the rear tactile sensor: NAO will crouch and remove its stiffness.

The code for the robot guide is the following: almath_robot_guide.py. Do not forget to put the code for the foot clipping in the same folder: almath_foot_clip.py.

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

"""Example : Show how to guide NAO by the hand"""

import qi
import argparse
import sys
import almath
import time
import math
import almath_foot_clip

armName     = "LArm"
lFootOffset = almath.Pose2D(0.0, 0.09, 0.0)
rFootOffset = almath.Pose2D(0.0, -0.09, 0.0)
stepSpeed   = 1.0
stepLength  = 0.05

def initRobotPosition(motion_service, posture_service):
    """
    Inits NAO's position and stiffnesses to make the guiding possible.
    """

    motion_service.wakeUp()
    posture_service.goToPosture("StandInit", 0.3)
    motion_service.moveInit()
    time.sleep(1.0)
    # Make left arm loose.
    motion_service.setAngles("LWristYaw", 0.0, 1.0)
    motion_service.setAngles("Head", [0.44, -0.44], 0.5)
    motion_service.setStiffnesses(armName, 0.0)
    motion_service.setStiffnesses("LWristYaw", 0.2)

    # Disable arm moves while walking on left arm.
    motion_service.setMoveArmsEnabled(False, True)
    time.sleep(1.0)


def interpretJointsPose(motion_service, memory_service):
    """
    Translates the current left arm pose into a target position
    for NAO's foot.
    """

    # Retrieve current arm position.
    armPose = motion_service.getAngles(armName, True)

    targetX     = 0.0
    targetY     = 0.0
    targetTheta = 0.0
    gaitConfig = motion_service.getMoveConfig("Default")

    # Filter Shoulder Pitch.
    if (armPose[0] > - 0.9 and armPose[0] < -0.20):
        targetX = stepLength
    elif (armPose[0] > -2.5 and armPose[0] < -1.5):
        targetX = - stepLength - 0.02

    # Filter Wrist Yaw.
    if armPose[4] > 0.2:
        targetTheta = gaitConfig[2][1]
    elif armPose[4] < -0.2:
        targetTheta = - gaitConfig[2][1]

    # Return corresponding pose.
    return almath.Pose2D(targetX, targetY, targetTheta)


def moveToTargetPose(targetPose, motion_service, isLeftSupport):
    """Move to the desired target with the current foot."""

    name = ""
    targetTf = almath.transformFromPose2D(targetPose)

    # Compute foot position with the offset in NAOSpace.
    if isLeftSupport:
        footTargetTf = targetTf * almath.transformFromPose2D(rFootOffset)
        footTargetPose = almath.pose2DFromTransform(footTargetTf)
        name = ["RLeg"]
    else:
        footTargetTf = targetTf * almath.transformFromPose2D(lFootOffset)
        footTargetPose = almath.pose2DFromTransform(footTargetTf)
        name = ["LLeg"]

    # Clip the footstep to avoid collisions and too wide steps.
    almath_foot_clip.clipFootStep(footTargetPose, isLeftSupport)

    step = [[footTargetPose.x, footTargetPose.y, footTargetPose.theta]]
    speed = [stepSpeed]

    # Send the footstep to NAO.
    motion_service.setFootStepsWithSpeed(name, step, speed, False)

    # Change current foot.
    isLeftSupport = not isLeftSupport


def main(session):
    """
    This example shows how to guide NAO by the hand, while computing his
    moves with only footsteps, using ALMath library. Footstep clipping is
    described in almath_foot_clip.py.
    """
    # Get the services ALMemory, ALMotion, ALRobotPosture and ALLeds.

    memory_service = session.service("ALMemory")
    motion_service = session.service("ALMotion")
    posture_service = session.service("ALRobotPosture")
    leds_service = session.service("ALLeds")

    motion_service.setExternalCollisionProtectionEnabled("Move", False)

    # Init robot position.
    initRobotPosition(motion_service, posture_service)

    # Wait for the user to press the front tactile sensor.
    print "Please press head front tactile sensor to start."
    while not memory_service.getData("FrontTactilTouched"):
        pass

    print "To guide the robot use the robot left arm."
    print "Move LShoulderPitch to set x, y target and move LWristYaw to set wz target."
    print "When the robot eyes are green, the robot is ready to move."

    print "Starting..."
    print "Please press head rear tactile sensor to stop."

    # Start by moving left foot.
    isLeftSupport = False
    isMoving = False
    leds_service.fadeRGB("FaceLeds", 255, 0.1)

    while not memory_service.getData("RearTactilTouched"):
        targetPose = interpretJointsPose(motion_service, memory_service)
        # Filter the pose to avoid too small steps.
        if (math.fabs(targetPose.x) > 0.01) or \
           (math.fabs(targetPose.y) > 0.01) or \
           (math.fabs(targetPose.theta) > 0.08):

            moveToTargetPose(targetPose, motion_service, isLeftSupport)
            isLeftSupport = not isLeftSupport
            isMoving = True
            # Set LEDs to green.
            leds_service.fadeRGB("FaceLeds", 256 * 255, 0.1)

        elif isMoving:
            # Stop the robot.
            motion_service.stopMove()
            isMoving = False
            # Set LEDs to blue.
            leds_service.fadeRGB("FaceLeds", 255, 0.1)

    print "Stopping..."
    # Set LEDs to white.
    leds_service.fadeRGB("FaceLeds", 256 * 256 * 255 + 256 * 255 + 255, 0.1)

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

The code for the foot clipping is the following:

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

"""Example: Use ALMath"""

import almath

"""
This file shows how to use ALMath library in Python to clip
NAO footsteps to correct moves (avoiding too big steps and collision).
Use clipFootStep to clip your desired pose.
"""


# Various parameters describing step characteristics.
minFootSeparation = 0.088
minStepX = - 0.04
maxStepX = 0.08
maxStepY = 0.16
maxStepTheta = 0.35

def clipFootStepOnGaitConfig(footMove, isLeftSupport):
    ''' Clip the foot move so that it does not exceed the maximum
        size of steps.
        footMove is an almath.Pose2D (x, y, theta position).
        isLeftSupport must be set to True if the move is on the right leg
        (the robot is supporting itself on the left leg).
    '''

    def clipFloat(minValue, maxValue, value):
        ''' Clip value between two extremes. '''
        clipped = value
        if (clipped < minValue):
            clipped = minValue
        if (clipped > maxValue):
            clipped = maxValue
        return clipped

    # Clip X.
    clippedX = clipFloat(minStepX, maxStepX, footMove.x)
    footMove.x = clippedX

    # Clip Y.
    if not isLeftSupport:
        clippedY = clipFloat(minFootSeparation, maxStepY, footMove.y)
    else:
        clippedY = clipFloat(-maxStepY, - minFootSeparation, footMove.y)
    footMove.y = clippedY

    # Clip Theta.
    clippedTheta = clipFloat(-maxStepTheta, maxStepTheta, footMove.theta)
    footMove.theta = clippedTheta


def clipFootStepWithEllipse(footMove):
    ''' Clip the foot move inside an ellipse defined by the foot's dimansions.
        footMove is an almath.Pose2D (x, y, theta position).
    '''

    # Apply an offset to have Y component of foot move centered on 0.
    if (footMove.y < -minFootSeparation):
        footMove.y = footMove.y + minFootSeparation
    elif (footMove.y > minFootSeparation):
        footMove.y = footMove.y - minFootSeparation
    else:
        return

    # Clip the foot move to an ellipse using ALMath method.
    if footMove.x >= 0:
        almath.clipFootWithEllipse(maxStepX, maxStepY - minFootSeparation, footMove)
    else:
        almath.clipFootWithEllipse(minStepX, maxStepY - minFootSeparation, footMove)

    # Correct the previous offset on Y component.
    if footMove.y >=0:
        footMove.y  = footMove.y + minFootSeparation
    else:
        footMove.y = footMove.y - minFootSeparation


def clipFootStepToAvoidCollision(footMove, isLeftSupport):
    """ Clip the foot move to avoid collision with the other foot.
        footMove is an almath.Pose2D (x, y, theta position).
        isLeftSupport must be set to True if the move is on the right leg
        (the robot is supporting itself on the left leg).
    """

    # Bounding boxes of NAO's feet.
    rFootBox = [almath.Pose2D(0.11, 0.038, 0.0),    # rFootBoxFL
                almath.Pose2D(0.11, -0.050, 0.0),   # rFootBoxFR
                almath.Pose2D(-0.047, -0.050, 0.0), # rFootBoxRR
                almath.Pose2D(-0.047, 0.038, 0.0)]  # rFootBoxRL
    lFootBox = [almath.Pose2D(0.11, 0.050, 0.0),    # lFootBoxFL
                almath.Pose2D(0.11, -0.038, 0.0),   # lFootBoxFR
                almath.Pose2D(-0.047, -0.038, 0.0), # lFootBoxRR
                almath.Pose2D(-0.047,  0.050, 0.0)] # lFootBoxRL

    # Use ALMath method.
    almath.avoidFootCollision(lFootBox, rFootBox, isLeftSupport, footMove)

def clipFootStep(footMove, isLeftSupport):
    ''' Clipping functions to avoid any warnings and undesired effects
        when sending the footsteps to ALMotion.
        footMove is an almath.Pose2D (x, y, theta position)
        isLeftSupport must be set to True if the move is on the right leg
        (the robot is supporting itself on the left leg).
    '''

    # First clip the foot move with gait config.
    clipFootStepOnGaitConfig(footMove, isLeftSupport)
    # Then clip it on an ellipse.
    clipFootStepWithEllipse(footMove)
    # Then make sure you do not have any collision.
    clipFootStepToAvoidCollision(footMove, isLeftSupport)