NAOqi Motion - Overview | API | Tutorial
Depecated Methods
Deprecated since version 1.16: use ALMotionProxy::positionInterpolations() instead.
NAO only
Moves an end-effector to the given position and orientation over time. This is a blocking call.
Parameters: |
|
---|
NAO only
There are two overloads of this function:
Moves end-effectors to the given positions and orientations over time. This is a blocking call.
Parameters: |
|
---|
almotion_positionInterpolations.py
# -*- encoding: UTF-8 -*-
import almath
import motion
import argparse
from naoqi import ALProxy
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to use positionInterpolations
frame = motion.FRAME_ROBOT
useSensorValues = False
dx = 0.03 # translation axis X (meters)
dy = 0.04 # translation axis Y (meters)
# Motion of Arms with block process
effectorList = []
pathList = []
axisMaskList = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
timeList = [[1.0], [1.0]] # seconds
effectorList.append("LArm")
currentPos = motionProxy.getPosition("LArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y -= dy
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
currentPos = motionProxy.getPosition("RArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y += dy
pathList.append(list(targetPos.toVector()))
motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
# Motion of Arms and Torso with block process
axisMaskList = [motion.AXIS_MASK_VEL,
motion.AXIS_MASK_VEL,
motion.AXIS_MASK_ALL]
timeList = [[4.0],
[4.0],
[1.0, 2.0, 3.0, 4.0]] # seconds
effectorList = []
pathList = []
effectorList.append("LArm")
pathList.append([motionProxy.getPosition("LArm", frame, useSensorValues)])
effectorList.append("RArm")
pathList.append([motionProxy.getPosition("RArm", frame, useSensorValues)])
effectorList.append("Torso")
torsoList = []
currentPos = motionProxy.getPosition("Torso", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y += dy
torsoList.append(list(targetPos.toVector()))
targetPos = almath.Position6D(currentPos)
targetPos.x -= dx
torsoList.append(list(targetPos.toVector()))
targetPos = almath.Position6D(currentPos)
targetPos.y -= dy
torsoList.append(list(targetPos.toVector()))
targetPos = almath.Position6D(currentPos)
torsoList.append(list(targetPos.toVector()))
pathList.append(torsoList)
motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
# Go to rest position
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)
Deprecated since version 1.16: use ALMotionProxy::positionInterpolations() instead.
NAO only
Moves end-effectors to the given positions and orientations over time. This is a blocking call.
Parameters: |
|
---|
NAO only
Moves an end-effector to the given position and orientation. This is a non-blocking call.
Parameters: |
|
---|
#include <iostream>
#include <alproxies/almotionproxy.h>
#include <alproxies/alrobotpostureproxy.h>
#include <qi/os.hpp>
int main(int argc, char **argv)
{
std::string robotIp = "127.0.0.1";
if (argc < 2) {
std::cerr << "Usage: almotion_setpositions robotIp "
<< "(optional default \"127.0.0.1\")."<< std::endl;
}
else {
robotIp = argv[1];
}
AL::ALMotionProxy motion(robotIp);
AL::ALRobotPostureProxy robotPosture(robotIp);
robotPosture.goToPosture("StandInit", 0.5f);
// Example showing how to set Torso Position, using a fraction of max speed
std::string chainName = "Torso";
int space = 2;
std::vector<float> position(6, 0.0f); // Absolute Position
position[2] = 0.25f;
float fractionMaxSpeed = 0.2f;
int axisMask = 63;
motion.setPositions(chainName, space, position, fractionMaxSpeed, axisMask);
qi::os::sleep(2.0f);
return 0;
}
# -*- encoding: UTF-8 -*-
import time
import motion
import argparse
from naoqi import ALProxy
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to set LArm Position, using a fraction of max speed
chainName = "LArm"
frame = motion.FRAME_TORSO
useSensor = False
# Get the current position of the chainName in the same frame
current = motionProxy.getPosition(chainName, frame, useSensor)
target = [
current[0] + 0.05,
current[1] + 0.05,
current[2] + 0.05,
current[3] + 0.0,
current[4] + 0.0,
current[5] + 0.0]
fractionMaxSpeed = 0.5
axisMask = 7 # just control position
motionProxy.setPositions(chainName, frame, target, fractionMaxSpeed, axisMask)
time.sleep(1.0)
# Example showing how to set Torso Position, using a fraction of max speed
chainName = "Torso"
frame = motion.FRAME_ROBOT
position = [0.0, 0.0, 0.25, 0.0, 0.0, 0.0] # Absolute Position
fractionMaxSpeed = 0.2
axisMask = 63
motionProxy.setPositions(chainName, frame, position, fractionMaxSpeed, axisMask)
time.sleep(4.0)
# Go to rest position
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)
Deprecated since version 1.16: use ALMotionProxy::setPositions() instead.
NAO only
Moves an end-effector to the given position and orientation. This is a non-blocking call.
Parameters: |
|
---|
Deprecated since version 1.16: use ALMotionProxy::setPositions() instead.
NAO only
Creates a move of an end effector in Cartesian frame. This is a non-blocking call.
Parameters: |
|
---|
Gets a Position relative to the FRAME. Axis definition: the x axis is positive toward the robot’s front, the y from right to left and the z is vertical. The angle convention of Position6D is Rot_z(wz).Rot_y(wy).Rot_x(wx).
Parameters: |
|
---|---|
Returns: | Vector containing the Position6D using meters and radians (x, y, z, wx, wy, wz) |
#include <iostream>
#include <alproxies/almotionproxy.h>
int main(int argc, char **argv)
{
std::string robotIp = "127.0.0.1";
if (argc < 2) {
std::cerr << "Usage: almotion_getposition robotIp "
<< "(optional default \"127.0.0.1\")."<< std::endl;
}
else {
robotIp = argv[1];
}
AL::ALMotionProxy motion(robotIp);
// Example showing how to get the position of the top camera
std::string name = "CameraTop";
int space = 1;
bool useSensorValues = true;
std::vector<float> result = motion.getPosition(name, space, useSensorValues);
std::cout << name << ":" << std::endl;
std::cout << "Position (x, y, z): " << result.at(0) << ", " << result.at(1)
<< ", " << result.at(2) << std::endl;
std::cout << "Rotation (x, y, z): " << result.at(3) << ", " << result.at(4)
<< ", " << result.at(5) << std::endl;
return 0;
}
# -*- encoding: UTF-8 -*-
import motion
import argparse
from naoqi import ALProxy
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
# Example showing how to get the position of the top camera
name = "CameraTop"
frame = motion.FRAME_WORLD
useSensorValues = True
result = motionProxy.getPosition(name, frame, useSensorValues)
print "Position of", name, " in World is:"
print result
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)
Deprecated since version 1.16: use ALMotionProxy::transformInterpolations() instead.
NAO only
Moves an end-effector to the given position and orientation over time using homogeneous transforms to describe the positions or changes. This is a blocking call.
Parameters: |
|
---|
NAO only
There are two overloads of this function:
Moves end-effector to the given transforms over time. This is a blocking call.
Parameters: |
|
---|
almotion_transformInterpolations.py
# -*- encoding: UTF-8 -*-
import almath
import motion
import argparse
from naoqi import ALProxy
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to use transformInterpolations
frame = motion.FRAME_ROBOT
isAbsolute = False
useSensorValues = False
# Motion of Arms with block process
effectorList = ["LArm", "RArm"]
axisMaskList = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
timeList = [[1.0], [1.0]] # seconds
dy = 0.04
pathList = []
targetLArmTf = almath.Transform(motionProxy.getTransform("LArm", frame, useSensorValues))
targetLArmTf.r2_c4 -= dy
pathList.append(list(targetLArmTf.toVector()))
targetRArmTf = almath.Transform(motionProxy.getTransform("RArm", frame, useSensorValues))
targetRArmTf.r2_c4 += dy
pathList.append(list(targetRArmTf.toVector()))
motionProxy.transformInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
# Motion of Arms and Torso with block process
effectorList = ["LArm", "RArm", "Torso"]
axisMaskList = [motion.AXIS_MASK_VEL,
motion.AXIS_MASK_VEL,
motion.AXIS_MASK_ALL]
timeList = [[4.0],
[4.0],
[1.0, 2.0, 3.0, 4.0]] # seconds
dx = 0.03 # translation axis X (meters)
dy = 0.05 # translation axis Y (meters)
pathList = []
targetLArmTf = almath.Transform(motionProxy.getTransform("LArm", frame, useSensorValues))
pathList.append(list(targetLArmTf.toVector()))
targetRArmTf = almath.Transform(motionProxy.getTransform("RArm", frame, useSensorValues))
pathList.append(list(targetRArmTf.toVector()))
pathTorsoList = []
# point 1
initTorsoTf = almath.Transform(motionProxy.getTransform("Torso", frame, useSensorValues))
targetTorsoTf = initTorsoTf
targetTorsoTf.r2_c4 += dy
pathTorsoList.append(list(targetTorsoTf.toVector()))
# point 2
initTorsoTf = almath.Transform(motionProxy.getTransform("Torso", frame, useSensorValues))
targetTorsoTf = initTorsoTf
targetTorsoTf.r1_c4 += -dx
pathTorsoList.append(list(targetTorsoTf.toVector()))
# point 3
initTorsoTf = almath.Transform(motionProxy.getTransform("Torso", frame, useSensorValues))
targetTorsoTf = initTorsoTf
targetTorsoTf.r2_c4 += -dy
pathTorsoList.append(list(targetTorsoTf.toVector()))
# point 4
initTorsoTf = almath.Transform(motionProxy.getTransform("Torso", frame, useSensorValues))
pathTorsoList.append(list(initTorsoTf.toVector()))
pathList.append(pathTorsoList)
motionProxy.transformInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
# Go to rest position
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)
Deprecated since version 1.16: use ALMotionProxy::transformInterpolations() instead.
NAO only
Moves end-effector to the given transforms over time. This is a blocking call.
Parameters: |
|
---|
NAO only
Moves an end-effector to the given position and orientation transform. This is a non-blocking call.
Parameters: |
|
---|
#include <iostream>
#include <alproxies/almotionproxy.h>
#include <alproxies/alrobotpostureproxy.h>
#include <qi/os.hpp>
int main(int argc, char **argv)
{
std::string robotIp = "127.0.0.1";
if (argc < 2) {
std::cerr << "Usage: almotion_settransforms robotIp "
<< "(optional default \"127.0.0.1\")."<< std::endl;
}
else {
robotIp = argv[1];
}
AL::ALMotionProxy motion(robotIp);
AL::ALRobotPostureProxy robotPosture(robotIp);
robotPosture.goToPosture("StandInit", 0.5f);
// Example showing how to set Torso Transform, using a fraction of max speed
std::string chainName = "Torso";
int space = 2;
std::vector<float> transform(12, 0.0f); // Absolute Position
transform[0] = 1.0f; // 1.0f, 0.0f, 0.0f, 0.00f
transform[5] = 1.0f; // 0.0f, 1.0f, 0.0f, 0.00f
transform[10] = 1.0f; // 0.0f, 0.0f, 1.0f, 0.25f
transform[11] = 0.25f;
float fractionMaxSpeed = 0.2f;
int axisMask = 63;
motion.setTransforms(chainName, space, transform, fractionMaxSpeed, axisMask);
qi::os::sleep(3.0f);
return 0;
}
# -*- encoding: UTF-8 -*-
import time
import motion
import argparse
from naoqi import ALProxy
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to set Torso Transform, using a fraction of max speed
chainName = "Torso"
frame = motion.FRAME_ROBOT
transform = [1.0, 0.0, 0.0, 0.00,
0.0, 1.0, 0.0, 0.00,
0.0, 0.0, 1.0, 0.25]
fractionMaxSpeed = 0.2
axisMask = 63
motionProxy.setTransforms(chainName, frame, transform, fractionMaxSpeed, axisMask)
time.sleep(4.0)
# Go to rest position
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)
Deprecated since version 1.16: use ALMotionProxy::setTransforms() instead.
NAO only
Moves an end-effector to the given position and orientation transform. This is a non-blocking call.
Parameters: |
|
---|
Deprecated since version 1.16: use ALMotionProxy::setTransforms() instead.
NAO only
Moves an end-effector to the given position and orientation transform. This is a non-blocking call.
Parameters: |
|
---|
Gets an Homogeneous Transform relative to the FRAME. Axis definition: the x axis is positive toward the robot’s front, the y from right to left and the z is vertical.
Parameters: |
|
---|---|
Returns: | Vector of 16 floats corresponding to the values of the matrix, line by line. |
#include <iostream>
#include <alproxies/almotionproxy.h>
int main(int argc, char **argv)
{
std::string robotIp = "127.0.0.1";
if (argc < 2) {
std::cerr << "Usage: almotion_gettransform robotIp "
<< "(optional default \"127.0.0.1\")."<< std::endl;
}
else {
robotIp = argv[1];
}
AL::ALMotionProxy motion(robotIp);
// Example showing how to get the end of the right arm as a transform
// represented in torso space.
std::string name = "RArm";
int space = 0; // TORSO_SPACE
bool useSensorValues = false;
std::vector<float> result = motion.getTransform(name, space, useSensorValues);
// R R R x
// R R R y
// R R R z
// 0 0 0 1
std::cout << "Transform of RArm" << std::endl;
std::cout << result.at(0) << " " << result.at(1) << " " << result.at(2) << " " << result.at(3) << std::endl;
std::cout << result.at(4) << " " << result.at(5) << " " << result.at(6) << " " << result.at(7) << std::endl;
std::cout << result.at(8) << " " << result.at(9) << " " << result.at(10) << " " << result.at(11) << std::endl;
return 0;
}
# -*- encoding: UTF-8 -*-
import motion
import argparse
from naoqi import ALProxy
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
# Example showing how to get the end of the right arm as a transform
# represented in torso space. The result is a 4 by 4 matrix composed
# of a 3*3 rotation matrix and a column vector of positions.
name = 'RArm'
frame = motion.FRAME_TORSO
useSensorValues = True
result = motionProxy.getTransform(name, frame, useSensorValues)
for i in range(0, 4):
for j in range(0, 4):
print result[4*i + j],
print ''
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)