Cartesian control API

NAOqi Motion - Overview | API | Tutorial

Method list

class ALMotionProxy
void ALMotionProxy::positionInterpolation(const std::string& chainName, const int& space, const AL::ALValue& path, const int& axisMask, const AL::ALValue& durations, const bool& isAbsolute)

Moves an end-effector to the given position and orientation over time. This is a blocking call.

Parameters:
  • chainName – Name of the chain. Could be: “Head”, “LArm”, “RArm”, “LLeg”, “RLeg”, “Torso”
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • path – Vector of 6D position arrays (x,y,z,wx,wy,wz) in meters and radians
  • axisMask – Axis mask. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both
  • durations – Vector of times in seconds corresponding to the path points
  • isAbsolute – If true, the movement is absolute else relative

almotion_positioninterpolation.cpp

#include <iostream>
#include <alproxies/almotionproxy.h>
#include <alproxies/alrobotpostureproxy.h>

int main(int argc, char **argv)
{
  std::string robotIp = "127.0.0.1";

  if (argc < 2) {
    std::cerr << "Usage: almotion_positioninterpolation 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 of a cartesian foot trajectory
  // Warning: Needs a PoseInit before executing
  int space       =  2; // FRAME_ROBOT
  int axisMask    = 63; // control all the effector's axes
  bool isAbsolute = false;

  // Lower the Torso and move to the side
  std::string effector = "Torso";
  AL::ALValue path     = AL::ALValue::array(0.0f, -0.07f, -0.03f, 0.0f, 0.0f, 0.0f);
  AL::ALValue timeList = 2.0f; // seconds
  motion.positionInterpolation(effector, space, path,
                              axisMask, timeList, isAbsolute);

  // LLeg motion
  effector   = "LLeg";
  path       = AL::ALValue::array(0.0f,  0.06f,  0.00f, 0.0f, 0.0f, 0.8f);
  timeList   = 2.0f; // seconds
  motion.positionInterpolation(effector, space, path,
                              axisMask, timeList, isAbsolute);

  return 0;
}

almotion_positioninterpolation.py

# -*- encoding: UTF-8 -*-

import sys
from naoqi import ALProxy
from naoqi import motion

def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example of a cartesian foot trajectory
    # Warning: Needs a PoseInit before executing
    # Example available: path/to/aldebaran-sdk/examples/
    #                    python/motion_cartesianFoot.py

    space      =  motion.FRAME_ROBOT
    axisMask   = 63                     # control all the effector's axes
    isAbsolute = False

    # Lower the Torso and move to the side
    effector = "Torso"
    path     = [0.0, -0.07, -0.03, 0.0, 0.0, 0.0]
    timeList = 2.0 # seconds
    motionProxy.positionInterpolation(effector, space, path,
                                axisMask, timeList, isAbsolute)

    # LLeg motion
    effector = "LLeg"
    path     = [0.0,  0.06,  0.00, 0.0, 0.0, 0.8]
    timeList = 2.0 # seconds
    motionProxy.positionInterpolation(effector, space, path,
                                axisMask, timeList, isAbsolute)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_positioninterpolation.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
void ALMotionProxy::positionInterpolations(const std::vector<std::string>& effectorNames, const int& taskSpaceForAllPaths, const AL::ALValue& paths, const AL::ALValue& axisMasks, const AL::ALValue& relativeTimes, const bool& isAbsolute)

Moves end-effectors to the given positions and orientations over time. This is a blocking call.

Parameters:
  • effectorNames – Vector of chain names. Could be: “Head”, “LArm”, “RArm”, “LLeg”, “RLeg”, “Torso”
  • taskSpaceForAllPaths – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • paths – Vector of 6D position arrays (x,y,z,wx,wy,wz) in meters and radians
  • axisMasks – Vector of Axis Masks. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both
  • relativeTimes – Vector of times in seconds corresponding to the path points
  • isAbsolute – If true, the movement is absolute else relative

almotion_positioninterpolations.py

# -*- encoding: UTF-8 -*-

import sys
from naoqi import ALProxy
import motion

def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to use positionInterpolations
    space        = motion.FRAME_ROBOT
    isAbsolute   = 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
    pathList     = [[[0.0, -0.04, 0.0, 0.0, 0.0, 0.0]],
                    [[0.0,  0.04, 0.0, 0.0, 0.0, 0.0]]]
    motionProxy.positionInterpolations(effectorList, space, pathList,
                                 axisMaskList, timeList, isAbsolute)

    # 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.04                   # translation axis Y (meters)
    pathList     = [[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],
                    [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],
                    [[0.0, +dy, 0.0, 0.0, 0.0, 0.0], # point 1
                     [-dx, 0.0, 0.0, 0.0, 0.0, 0.0], # point 2
                     [0.0, -dy, 0.0, 0.0, 0.0, 0.0], # point 3
                     [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] # point 4
                   ]
    motionProxy.positionInterpolations(effectorList, space, pathList,
                                 axisMaskList, timeList, isAbsolute)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_positioninterpolations.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
void ALMotionProxy::setPosition(const std::string& chainName, const int& space, const std::vector<float>& position, const float& fractionMaxSpeed, const int& axisMask)

Moves an end-effector to the given position and orientation. This is a non-blocking call.

Parameters:
  • chainName – Name of the chain. Could be: “Head”, “LArm”,”RArm”, “LLeg”, “RLeg”, “Torso”
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • position – 6D position array (x,y,z,wx,wy,wz) in meters and radians
  • fractionMaxSpeed – The fraction of maximum speed to use
  • axisMask – Axis mask. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both

almotion_setposition.cpp

#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_setposition 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.setPosition(chainName, space, position, fractionMaxSpeed, axisMask);
  qi::os::sleep(2.0f);

  return 0;
}

almotion_setposition.py

# -*- encoding: UTF-8 -*-

import sys
import time
from naoqi import ALProxy
from naoqi import motion

def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to set LArm Position, using a fraction of max speed
    chainName = "LArm"
    space     = motion.FRAME_TORSO
    useSensor = False

    # Get the current position of the chainName in the same space
    current = motionProxy.getPosition(chainName, space, useSensor)

    target  = [
        current[0] + 0.1,
        current[1] + 0.1,
        current[2] + 0.1,
        current[3] + 0.0,
        current[4] + 0.0,
    current[5] + 0.0]

    fractionMaxSpeed = 0.5
    axisMask         = 7 # just control position

    motionProxy.setPosition(chainName, space, target, fractionMaxSpeed, axisMask)

    time.sleep(1.0)

    # Example showing how to set Torso Position, using a fraction of max speed
    chainName        = "Torso"
    space            = 2
    position         = [0.0, 0.0, 0.25, 0.0, 0.0, 0.0] # Absolute Position
    fractionMaxSpeed = 0.2
    axisMask         = 63
    motionProxy.setPosition(chainName, space, position, fractionMaxSpeed, axisMask)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_setposition.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
void ALMotionProxy::changePosition(const std::string& effectorName, const int& space, const std::vector<float>& positionChange, const float& fractionMaxSpeed, const int& axisMask)

Creates a move of an end effector in Cartesian space. This is a non-blocking call.

Parameters:
  • effectorName – Name of the effector.
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • positionChange – 6D position change array (xd, yd, zd, wxd, wyd, wzd) in meters and radians
  • fractionMaxSpeed – The fraction of maximum speed to use
  • axisMask – Axis mask. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both

almotion_changeposition.cpp

#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_changeposition 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 move forward (3cm) "LArm".
  std::string effectorName  = "LArm";
  int space                 = 2; // FRAME_ROBOT
  std::vector<float> positionChange(6, 0.0f);
  positionChange[0] = 0.03f;
  float fractionMaxSpeed    = 0.3f;
  int axisMask              = 7;

  motion.changePosition(effectorName, space, positionChange, fractionMaxSpeed, axisMask);

  return 0;
}

almotion_changeposition.py

# -*- encoding: UTF-8 -*-

import sys
import time
from naoqi import ALProxy
import motion

def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to move forward (4cm) "LArm".
    effectorName     = "LArm"
    space            = motion.FRAME_ROBOT
    positionChange   = [0.04, 0.0, 0.0, 0.0, 0.0, 0.0]
    fractionMaxSpeed = 0.5
    axisMask         = 7
    motionProxy.changePosition(effectorName, space, positionChange, fractionMaxSpeed, axisMask)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_changeposition.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
std::vector<float> ALMotionProxy::getPosition(const std::string& name, const int& space, const bool& useSensorValues)

Gets a Position relative to the TASK_SPACE. Axis definition: the x axis is positive toward NAO’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:
  • name – Name of the item. Could be: Head, LArm, RArm, LLeg, RLeg, Torso, CameraTop, CameraBottom, MicroFront, MicroRear, MicroLeft, MicroRight, Accelerometer, Gyrometer, Laser, LFsrFR, LFsrFL, LFsrRR, LFsrRL, RFsrFR, RFsrFL, RFsrRR, RFsrRL, USSensor1, USSensor2, USSensor3, USSensor4. Use getSensorNames for the list of sensors supported on your robot.
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • useSensorValues – If true, the sensor values will be used to determine the position.
Returns:

Vector containing the Position6D using meters and radians (x, y, z, wx, wy, wz)

almotion_getposition.cpp

#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;
}

almotion_getposition.py

# -*- encoding: UTF-8 -*-

import sys
from naoqi import ALProxy
import motion


def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    # Example showing how to get the position of the top camera
    name            = "CameraTop"
    space           = motion.FRAME_WORLD
    useSensorValues = True
    result          = motionProxy.getPosition(name, space, useSensorValues)
    print "Position of", name, " in World is:"
    print result


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_advancedcreaterotation.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
void ALMotionProxy::transformInterpolation(const std::string& chainName, const int& space, const AL::ALValue& path, const int& axisMask, const AL::ALValue& duration, const bool& isAbsolute)

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:
  • chainName – Name of the chain. Could be: “Head”, “LArm”,”RArm”, “LLeg”, “RLeg”, “Torso”
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • path – Vector of Transform arrays
  • axisMask – Axis mask. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both
  • duration – Vector of times in seconds corresponding to the path points
  • isAbsolute – If true, the movement is absolute else relative

almotion_transforminterpolation.py

import sys
# -*- encoding: UTF-8 -*-

import time
from naoqi import ALProxy
import motion
import math

def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example moving the left hand up a little using transforms
    # Note that this is easier to do with positionInterpolation
    chainName  = 'LArm'
    # Defined in 'Torso' space
    space  = motion.FRAME_TORSO
    # We will use a single transform
    # | R R R x |
    # | R R R y |
    # | R R R z |
    # | 0 0 0 1 |
    # Get the current transform, in 'Torso' space using
    # the command angles.
    useSensorValue = False
    initialTransform = motionProxy.getTransform('LArm', space, useSensorValue)
    # Copy the current transform
    targetTransform = initialTransform
    # add 2cm to the z axis, making the arm move upwards
    targetTransform[11] = initialTransform[11] + 0.02
    # construct a path with only one transform
    path  = [targetTransform]
    # The arm does not have enough degees of freedom
    # to be able to constrain all the axes of movement,
    # so here, we will choose an axis mask of 7, which
    # will contrain position only
    # x = 1, y = 2, z = 4, wx = 8, wy = 16, wz = 32
    # 1 + 2 + 4 = 7
    axisMask  = 7
    # Allow three seconds for the movement
    duration  = [3.0]
    isAbsolute  = False
    motionProxy.transformInterpolation(chainName, space, path,
    axisMask, duration, isAbsolute)
    finalTransform = motionProxy.getTransform('LArm', motion.FRAME_TORSO, False)
    print 'Initial', initialTransform
    print 'Target', targetTransform
    print 'Final', finalTransform

    time.sleep(1.0)

    # Example moving the left hand up a little using transforms
    # Note that this is easier to do with positionInterpolation

    space      = motion.FRAME_ROBOT
    axisMask   = motion.AXIS_MASK_ALL   # full control
    isAbsolute = False
    # Lower the Torso and move to the side
    effector   = "Torso"
    path       = [1.0, 0.0, 0.0, 0.0,
                  0.0, 1.0, 0.0, -0.07,
                  0.0, 0.0, 1.0, -0.03]
    duration   = 2.0                    # seconds
    motionProxy.transformInterpolation(effector, space, path,
                                 axisMask, duration, isAbsolute)
    # LLeg motion
    effector   = "LLeg"
    cs = math.cos(45.0*math.pi/180.0)
    ss = math.cos(45.0*math.pi/180.0)
    path       = [ cs, -ss, 0.0, 0.0,
                   ss,  cs, 0.0, 0.06,
                  0.0, 0.0, 1.0, 0.0]
    duration   = 2.0                    # seconds
    motionProxy.transformInterpolation(effector, space, path,
                                 axisMask, duration, isAbsolute)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_transforminterpolation.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
void ALMotionProxy::transformInterpolations(const std::vector<std::string>& effectorNames, const int& taskSpaceForAllPaths, const AL::ALValue& paths, const AL::ALValue& axisMasks, const AL::ALValue& relativeTimes, const bool& isAbsolute)

Moves end-effector to the given transforms over time. This is a blocking call.

Parameters:
  • effectorNames – Vector of chain names. Could be: “Head”, “LArm”, “RArm”, “LLeg”, “RLeg”, “Torso”
  • taskSpaceForAllPaths – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • paths – Vector of transforms arrays.
  • axisMasks – Vector of Axis Masks. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both
  • relativeTimes – Vector of times in seconds corresponding to the path points
  • isAbsolute – If true, the movement is absolute else relative

almotion_transforminterpolations.py

# -*- encoding: UTF-8 -*-

import sys
from naoqi import ALProxy
from naoqi import motion


def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to use transformInterpolations
    space        = motion.FRAME_ROBOT
    isAbsolute   = 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
    pathList     = [[[1.0, 0.0, 0.0, 0.0,
                      0.0, 1.0, 0.0, -0.04,
                      0.0, 0.0, 1.0, 0.0]],
                    [[1.0, 0.0, 0.0, 0.0,
                      0.0, 1.0, 0.0, 0.04,
                      0.0, 0.0, 1.0, 0.0]]]
    motionProxy.transformInterpolations(effectorList, space, pathList,
                                 axisMaskList, timeList, isAbsolute)

    # 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.04                   # translation axis Y (meters)

    pathList     = [[[1.0, 0.0, 0.0, 0.0,
                      0.0, 1.0, 0.0, 0.0,
                      0.0, 0.0, 1.0, 0.0]],
                    [[1.0, 0.0, 0.0, 0.0,
                      0.0, 1.0, 0.0, 0.0,
                      0.0, 0.0, 1.0, 0.0]],
                    [[1.0, 0.0, 0.0, 0.0, # point 1
                      0.0, 1.0, 0.0, +dy,
                      0.0, 0.0, 1.0, 0.0],
                     [1.0, 0.0, 0.0, -dx, # point 2
                      0.0, 1.0, 0.0, 0.0,
                      0.0, 0.0, 1.0, 0.0],
                     [1.0, 0.0, 0.0, 0.0, # point 3
                      0.0, 1.0, 0.0, -dy,
                      0.0, 0.0, 1.0, 0.0],
                     [1.0, 0.0, 0.0, 0.0, # point 4
                      0.0, 1.0, 0.0, 0.0,
                      0.0, 0.0, 1.0, 0.0]]
                   ]

    motionProxy.transformInterpolations(effectorList, space, pathList,
                                  axisMaskList, timeList, isAbsolute)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_transforminterpolations.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
void ALMotionProxy::setTransform(const std::string& chainName, const int& space, const std::vector<float>& transform, const float& fractionMaxSpeed, const int& axisMask)

Moves an end-effector to the given position and orientation transform. This is a non-blocking call.

Parameters:
  • chainName – Name of the chain. Could be: “Head”, “LArm”,”RArm”, “LLeg”, “RLeg”, “Torso”
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • transform – Transform arrays
  • fractionMaxSpeed – The fraction of maximum speed to use
  • axisMask – Axis mask. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both

almotion_settransform.cpp

#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_settransform 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.setTransform(chainName, space, transform, fractionMaxSpeed, axisMask);
  qi::os::sleep(3.0f);

  return 0;
}

almotion_settransform.py

# -*- encoding: UTF-8 -*-

import sys
from naoqi import ALProxy
import motion


def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to set Torso Transform, using a fraction of max speed
    chainName        = "Torso"
    space            = 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.setTransform(chainName, space, transform, fractionMaxSpeed, axisMask)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_settransform.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
void ALMotionProxy::changeTransform(const std::string& chainName, const int& space, const std::vector<float>& transform, const float& fractionMaxSpeed, const int& axisMask)

Moves an end-effector to the given position and orientation transform. This is a non-blocking call.

Parameters:
  • chainName – Name of the chain. Could be: “Head”, “LArm”,”RArm”, “LLeg”, “RLeg”, “Torso”
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • transform – Transform arrays
  • fractionMaxSpeed – The fraction of maximum speed to use
  • axisMask – Axis mask. True for axes that you wish to control. e.g. 7 for position only, 56 for rotation only and 63 for both

almotion_changetransform.cpp

#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_changetransform 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; // FRAME_ROBOT
  std::vector<float> transform(12, 0.0f);
  transform[0]  = 1.0f;  // 1.0f, 0.0f, 0.0f, 0.05f
  transform[3]  = 0.05f; // 0.0f, 1.0f, 0.0f, 0.0f
  transform[5]  = 1.0f;  // 0.0f, 0.0f, 1.0f, 0.0f
  transform[10] = 1.0f;
  float fractionMaxSpeed = 0.2f;
  int axisMask  = 63;
  motion.changeTransform(chainName, space, transform, fractionMaxSpeed, axisMask);

  qi::os::sleep(2.0f);

  return 0;
}

almotion_changetransform.py

# -*- encoding: UTF-8 -*-

import sys
from naoqi import ALProxy
import motion


def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    try:
        postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALRobotPosture"
        print "Error was: ", e

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to set Torso Transform, using a fraction of max speed
    chainName = "Torso"
    space     = motion.FRAME_ROBOT
    transform = [1.0, 0.0, 0.0, 0.04,
                 0.0, 1.0, 0.0, 0.00,
                 0.0, 0.0, 1.0, 0.00] # Relative Transform
    fractionMaxSpeed = 0.2
    axisMask         = 63 # all axis
    motionProxy.changeTransform(chainName, space, transform, fractionMaxSpeed, axisMask)


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_changeTransform.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
std::vector<float> ALMotionProxy::getTransform(const std::string& name, const int& space, const bool& useSensorValues)

Gets an Homogeneous Transform relative to the TASK_SPACE. Axis definition: the x axis is positive toward NAO’s front, the y from right to left and the z is vertical.

Parameters:
  • name – Name of the item. Could be: any joint or chain or sensor (Head, LArm, RArm, LLeg, RLeg, Torso, HeadYaw, ..., CameraTop, CameraBottom, MicroFront, MicroRear, MicroLeft, MicroRight, Accelerometer, Gyrometer, Laser, LFsrFR, LFsrFL, LFsrRR, LFsrRL, RFsrFR, RFsrFL, RFsrRR, RFsrRL, USSensor1, USSensor2, USSensor3, USSensor4. Use getSensorNames for the list of sensors supported on your robot.
  • space – Task space {FRAME_TORSO = 0, FRAME_WORLD = 1, FRAME_ROBOT = 2}.
  • useSensorValues – If true, the sensor values will be used to determine the position.
Returns:

Vector of 16 floats corresponding to the values of the matrix, line by line.

almotion_gettransform.cpp

#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;
}

almotion_gettransform.py

# -*- encoding: UTF-8 -*-

import sys
from naoqi import ALProxy

def main(robotIP):
    PORT = 9559

    try:
        motionProxy = ALProxy("ALMotion", robotIP, PORT)
    except Exception,e:
        print "Could not create proxy to ALMotion"
        print "Error was: ",e
        sys.exit(1)

    # 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'
    space  = 0
    useSensorValues  = True
    result = motionProxy.getTransform(name, space, useSensorValues)
    for i in range(0, 4):
        for j in range(0, 4):
            print result[4*i + j],
        print ''


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_gettransform.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
« Cartesian control | Cartesian control | Cartesian control Tutorial: The Hula-Hoop motion »
« previous | up | next »