ALMath¶
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).
#! /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)
