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).
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 Install 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.
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).
# -*- encoding: UTF-8 -*-
''' Example showing how to use almath with python and send the results to
the robot by using a proxy to ALMotion '''
import argparse
import time
from naoqi import ALProxy
import almath
def main(robotIP, PORT=9559):
# Create a proxy to ALMotion.
try:
motionProxy = ALProxy("ALMotion", robotIP, PORT)
except Exception,e:
print "Could not create proxy to ALMotion"
print "Error was: ",e
# Create a proxy to ALRobotPosture.
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
except Exception,e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ",e
# WakeUp
motionProxy.wakeUp()
# Stand up.
postureProxy.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(
motionProxy.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
motionProxy.setTransforms(
chainName,
frame,
targetTransformList,
fractionOfMaxSpeed,
axisMask)
time.sleep(2.0)
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
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.
# -*- encoding: UTF-8 -*-
''' 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. '''
import argparse
import time
import math
from naoqi import ALProxy
import almath
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(motionProxy, postureProxy):
''' Inits NAO's position and stiffnesses to make the guiding possible.'''
motionProxy.wakeUp()
postureProxy.goToPosture("StandInit", 0.3)
motionProxy.moveInit()
time.sleep(1.0)
# Make left arm loose.
motionProxy.setAngles("LWristYaw", 0.0, 1.0)
motionProxy.setAngles("Head", [0.44, -0.44], 0.5)
motionProxy.setStiffnesses(armName, 0.0)
motionProxy.setStiffnesses("LWristYaw", 0.2)
# Disable arm moves while walking on left arm.
motionProxy.setMoveArmsEnabled(False, True)
time.sleep(1.0)
def interpretJointsPose(motionProxy, memoryProxy):
''' Translates the current left arm pose into a target position for NAO's
foot. '''
# Retrieve current arm position.
armPose = motionProxy.getAngles(armName, True)
targetX = 0.0
targetY = 0.0
targetTheta = 0.0
gaitConfig = motionProxy.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, motionProxy, 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.
motionProxy.setFootStepsWithSpeed(name, step, speed, False)
# Change current foot.
isLeftSupport = not isLeftSupport
def main(robotIP, PORT=9559):
# Init proxies.
try:
memoryProxy = ALProxy("ALMemory", robotIP, PORT)
except Exception, e:
print "Could not create proxy to ALMemory"
print "Error was: ", e
try:
motionProxy = ALProxy("ALMotion", robotIP, PORT)
except Exception, e:
print "Could not create proxy to ALMotion"
print "Error was: ", e
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
try:
ledsProxy = ALProxy("ALLeds", robotIP, PORT)
except Exception, e:
print "Could not create proxy to ALLeds"
print "Error was: ", e
motionProxy.setExternalCollisionProtectionEnabled("Move", False)
# Init robot position.
initRobotPosition(motionProxy, postureProxy)
# Wait for the user to press the front tactile sensor.
print "Please press head front tactile sensor to start."
while not memoryProxy.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
ledsProxy.fadeRGB("FaceLeds", 255, 0.1)
while not memoryProxy.getData("RearTactilTouched"):
targetPose = interpretJointsPose(motionProxy, memoryProxy)
# 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, motionProxy, isLeftSupport)
isLeftSupport = not isLeftSupport
isMoving = True
# Set LEDs to green.
ledsProxy.fadeRGB("FaceLeds", 256 * 255, 0.1)
elif isMoving:
# Stop the robot.
motionProxy.stopMove()
isMoving = False
# Set LEDs to blue.
ledsProxy.fadeRGB("FaceLeds", 255, 0.1)
print "Stopping..."
# Set LEDs to white.
ledsProxy.fadeRGB("FaceLeds", 256 * 256 * 255 + 256 * 255 + 255, 0.1)
# Crouch.
motionProxy.rest()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parser.add_argument("--port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
main(args.ip, args.port)
The code for the foot clipping is the following:
# -*- encoding: UTF-8 -*-
''' 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.
'''
import almath
# 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.Position2D(0.11, 0.038), # rFootBoxFL
almath.Position2D(0.11, -0.050), # rFootBoxFR
almath.Position2D(-0.047, -0.050), # rFootBoxRR
almath.Position2D(-0.047, 0.038)] # rFootBoxRL
lFootBox = [almath.Position2D(0.11, 0.050), # lFootBoxFL
almath.Position2D(0.11, -0.038), # lFootBoxFR
almath.Position2D(-0.047, -0.038), # lFootBoxRR
almath.Position2D(-0.047, 0.050)] # 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)