SoftBank Robotics documentation What's new in NAOqi 2.8?

Whole Body control API

NAOqi Motion - Overview | API | Tutorial

nao NAO only

Warning

Do not use on Pepper.

Method list

class ALMotionProxy
void ALMotionProxy::wbEnable(const bool& isEnabled)

Enables Whole Body Balancer. It’s a Generalized Inverse Kinematics which deals with Cartesian control, balance, redundancy and task priority. The main goal is to generate and stabilized consistent motions without precomputed trajectories and adapt the behavior of the robot to the situation. The generalized inverse kinematic problem takes in account equality constraints (keep foot fix), inequality constraints (joint limits, balance, ...) and quadratic minimization (Cartesian / articular desired trajectories). We solve each step a quadratic programming on the robot.

Parameters:
  • isEnabled – Activate / Deactivate Whole Body Balancer.

almotion_wbenable.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_wbenable robotIp "
              << "(optional default \"127.0.0.1\")."<< std::endl;
  }
  else {
    robotIp = argv[1];
  }

  AL::ALMotionProxy motion(robotIp);

  // Example showing how to active Whole Body Balancer.
  bool isEnabled = true;
  motion.wbEnable(isEnabled);
  std::cout << "Whole body enabled." << std::endl;

  isEnabled = false;
  motion.wbEnable(isEnabled);
  std::cout << "Whole body disabled." << std::endl;

  return 0;
}

almotion_wbEnable.py

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

"""Example: Use wbEnable Method"""

import qi
import argparse
import sys
import time


def main(session):
    """
    This example uses the wbEnable method.
    """
    # Get the services ALMotion & ALRobotPosture.

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

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    # Example showing how to enable Whole Body Balancer.
    isEnabled = True
    motion_service.wbEnable(isEnabled)

    time.sleep(2.0)

    # Example showing how to disable Whole Body Balancer.
    isEnabled = False
    motion_service.wbEnable(isEnabled)

    # Go to rest position
    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)
void ALMotionProxy::wbFootState(const std::string& stateName, const std::string& supportLeg)

Sets the state of the foot (or feet): fixed foot, constrained in a plane or free.

Parameters:
  • stateName – Name of the foot state. “Fixed” set the foot fixed. “Plane” constrained the foot in the plane. “Free” set the foot free.
  • supportLeg – Name of the foot. “LLeg”, “RLeg” or “Legs”.

almotion_wbfootstate.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_wbfootstate robotIp "
              << "(optional default \"127.0.0.1\")."<< std::endl;
  }
  else {
    robotIp = argv[1];
  }

  AL::ALMotionProxy motion(robotIp);

  std::vector<float> stiffnesses = motion.getStiffnesses("Body");
  if (stiffnesses[0] < 0.5f) {
    std::cerr << "Warning: this example will have no effect. "
              << "Robot must be stiff and standing." << std::endl;
    return 1;
  }

  bool isEnabled = true;
  motion.wbEnable(isEnabled);

  // Example showing how to fix the feet.
  std::string stateName = "Fixed";
  std::string supportLeg = "Legs";
  motion.wbFootState(stateName, supportLeg);
  std::cout << supportLeg << " " << stateName << std::endl;

  // Example showing how to fix the left leg and constrained in a plane the right leg.
  motion.wbFootState("Fixed", "LLeg");
  motion.wbFootState("Plane", "RLeg");
  std::cout << "LLeg Fixed, RLeg Plane" << std::endl;

  // Example showing how to fix the left leg and keep free the right leg.
  motion.wbFootState("Fixed", "LLeg");
  motion.wbFootState("Free", "RLeg");
  std::cout << "LLeg Fixed, RLeg Free" << std::endl;

  isEnabled = false;
  motion.wbEnable(isEnabled);

  return 0;
}

almotion_wbFootState.py

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

"""Example: Use wbFootState Method"""

import qi
import argparse
import sys
import time


def main(session):
    """
    This example uses the wbFootState method.
    """
    # Get the services ALMotion & ALRobotPosture.

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

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    motion_service.wbEnable(True)

    # Example showing how to fix the feet.
    print "Feet fixed."
    stateName = "Fixed"
    supportLeg = "Legs"
    motion_service.wbFootState(stateName, supportLeg)

    # Example showing how to fix the left leg and constrained in a plane the right leg.
    print "Left leg fixed, right leg in a plane."
    motion_service.wbFootState("Fixed", "LLeg")
    motion_service.wbFootState("Plane", "RLeg")

    # Example showing how to fix the left leg and keep free the right leg.
    print "Left leg fixed, right leg free"
    motion_service.wbFootState("Fixed", "LLeg")
    motion_service.wbFootState("Free", "RLeg")

    time.sleep(2.0)
    motion_service.wbEnable(False)

    # Go to rest position
    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)
void ALMotionProxy::wbEnableBalanceConstraint(const bool& isEnable, const std::string& supportLeg)

Enables to keep balance in support polygon.

Parameters:
  • isEnable – Enable the robot to keep balance.
  • supportLeg – Name of the support leg: “Legs”, “LLeg”, “RLeg”.

almotion_wbenablebalanceconstraint.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_wbenablebalanceconstraint robotIp "
              << "(optional default \"127.0.0.1\")."<< std::endl;
  }
  else {
    robotIp = argv[1];
  }

  AL::ALMotionProxy motion(robotIp);

  // Example showing how to Constraint Balance Motion.
  bool isEnable = true;
  std::string supportLeg  = "Legs";
  motion.wbEnableBalanceConstraint(isEnable, supportLeg);
  std::cout << "Enabled whole body balance constraint on " << supportLeg << std::endl;

  isEnable = false;
  motion.wbEnableBalanceConstraint(isEnable, supportLeg);
  std::cout << "Disabled whole body balance constraint on " << supportLeg << std::endl;

  return 0;
}

almotion_wbEnableBalanceConstraint.py

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

"""Example: Use wbEnableBalanceConstraint Method"""

import qi
import argparse
import sys
import time


def main(session):
    """
    This example uses the wbEnableBalanceConstraint method.
    """
    # Get the services ALMotion & ALRobotPosture.

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

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    motion_service.wbEnable(True)

    # Example showing how to Constraint Balance Motion.
    isEnable   = True
    supportLeg = "Legs"
    motion_service.wbEnableBalanceConstraint(isEnable, supportLeg)

    time.sleep(2.0)
    motion_service.wbEnable(False)

    # Go to rest position
    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)
void ALMotionProxy::wbGoToBalance(const std::string& supportLeg, const float& duration)

Advanced Whole Body API: Com go to a desired support polygon. This is a blocking call. Use with caution: make sure the starting pose is consistent with the targeted pose. Starting form Init pose is the safer way to use it.

Parameters:
  • supportLeg – Name of the support leg: “Legs”, “LLeg”, “RLeg”.
  • duration – Time in seconds.

almotion_wbgotobalance.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_wbgotobalance 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);

  bool isEnabled = true;
  motion.wbEnable(isEnabled);

  // Example showing how to com go to LLeg.
  std::string supportLeg = "LLeg";
  float duration         = 2.0f;
  motion.wbGoToBalance(supportLeg, duration);

  isEnabled = false;
  motion.wbEnable(isEnabled);

  return 0;
}

almotion_wbGoToBalance.py

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

"""Example: Use wbGoToBalance Method"""

import qi
import argparse
import sys


def main(session):
    """
    This example uses the wbGoToBalance method.
    """
    # Get the services ALMotion & ALRobotPosture.

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

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    motion_service.wbEnable(True)

    # Example showing how to com go to LLeg.
    supportLeg = "LLeg"
    duration   = 2.0
    motion_service.wbGoToBalance(supportLeg, duration)

    motion_service.wbEnable(False)

    # Go to rest position
    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)
bool ALMotionProxy::wbGoToBalanceWithSpeed(const std::string& supportLeg, const float& fractionMaxSpeed)

Advanced Whole Body API: Com go to a desired support polygon. This is a blocking call. Use with caution: make sure the starting pose is consistent with the targeted pose. Starting form Init pose is the safer way to use it.

Parameters:
  • supportLeg – Name of the support leg: “Legs”, “LLeg”, “RLeg”.
  • fractionOfMaxSpeed – The fraction of maximum speed to use.
Returns:

Return a boolean of the success of the go to balance with speed.
void ALMotionProxy::wbEnableEffectorControl(const std::string& effectorName, const bool& isEnabled)

Enables whole body Cartesian control of an effector.

Parameters:
  • effectorName – Name of the effector : “Head”, “LArm” or “RArm”. The robot goes to posture init. He manages his balance and keep foot fix. “Head” is controlled in rotation. “LArm” and “RArm” are controlled in position.
  • isEnabled – Activate / Deactivate Effector Control.

almotion_wbenableeffectorcontrol.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_wbenableeffectorcontrol robotIp "
              << "(optional default \"127.0.0.1\")."<< std::endl;
  }
  else {
    robotIp = argv[1];
  }

  AL::ALMotionProxy motion(robotIp);

  // Example showing how to active Head tracking.
  std::string effectorName = "Head";
  bool isEnabled = true;
  motion.wbEnableEffectorControl(effectorName, isEnabled);
  std::cout << "Enabled whole body effector " << effectorName << " control"
            << std::endl;

  isEnabled = false;
  motion.wbEnableEffectorControl(effectorName, isEnabled);
  std::cout << "Disabled whole body effector " << effectorName << " control"
            << std::endl;

  return 0;
}

almotion_wbEnableEffectorControl.py

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

"""Example: Use wbEnableEffectorControl Method"""

import qi
import argparse
import sys
import time


def main(session):
    """
    This example uses the wbEnableEffectorControl method.
    """
    # Get the services ALMotion & ALRobotPosture.

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

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    motion_service.wbEnable(True)

    # Example showing how to active Head tracking.
    effectorName = 'Head'
    isEnabled    = True
    motion_service.wbEnableEffectorControl(effectorName, isEnabled)
    print "Enabled head effector control"

    time.sleep(2.0)
    motion_service.wbEnable(False)

    # Go to rest position
    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)
void ALMotionProxy::wbSetEffectorControl(const std::string& effectorName, const AL::ALValue& targetCoordinate)

Sets a new target for controlled effector. This is a non-blocking call.

Parameters:
  • effectorName – Name of the effector : “Head”, “LArm” or “RArm”. The robot goes to posture init. He manages his balance and keep foot fix. “Head” is controlled in rotation. “LArm” and “RArm” are controlled in position.
  • targetCoordinate – “Head” is controlled in rotation (WX, WY, WZ). “LArm” and “RArm” are controlled in position (X, Y, Z). TargetCoordinate must be absolute and expressed in FRAME_ROBOT. If the desired position/orientation is unfeasible, target is resize to the nearest feasible motion.

almotion_wbseteffectorcontrol.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_wbseteffectorcontrol 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);

  bool isEnabled = true;
  motion.wbEnable(isEnabled);

  // Example showing how to set orientation target for Head tracking.
  std::string effectorName = "Head";
  AL::ALValue targetCoordinate = AL::ALValue::array(0.1f, 0.0f, 0.0f);
  motion.wbSetEffectorControl(effectorName, targetCoordinate);

  isEnabled = false;
  motion.wbEnable(isEnabled);

  return 0;
}

almotion_wbSetEffectorControl.py

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

"""Example: Use wbSetEffectorControl Method"""

import qi
import argparse
import sys
import time


def main(session):
    """
    This example uses the wbSetEffectorControl method.
    """
    # Get the services ALMotion & ALRobotPosture.

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

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    motion_service.wbEnable(True)

    # Example showing how to set orientation target for LArm tracking.
    effectorName = "LArm"

    motion_service.wbEnableEffectorControl(effectorName, True)
    time.sleep(2.0)
    targetCoordinate = [0.20, 0.12, 0.30]
    motion_service.wbSetEffectorControl(effectorName, targetCoordinate)

    time.sleep(4.0)
    motion_service.wbEnable(False)

    # Go to rest position
    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)
void ALMotionProxy::wbEnableEffectorOptimization(const std::string& effectorName, const bool& isEnabled)

Enables whole body Cartesian optimization of an effector.

Parameters:
  • effectorName – Name of the effector.
  • isEnabled – Activate / Deactivate Effector Optimization.

almotion_wbenableeffectoroptimization.cpp

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

int main(int argc, char **argv)
{
  if (argc < 2) {
    std::cerr << "Usage: motion_wbenableeffectoroptimization pIp"
              << std::endl;
    return 1;
  }
  const std::string pIp = argv[1];

  AL::ALMotionProxy motion(pIp);

  // Example showing how to Enable Effector Control as an Optimization.
  std::string effectorName = "RArm";
  bool isActive = true;
  motion.wbEnableEffectorOptimization(effectorName, isActive);
  std::cout << "Enabled whole body effector " << effectorName << " optimization"
          << std::endl;

  isActive = false;
  motion.wbEnableEffectorOptimization(effectorName, isActive);
  std::cout << "Disabled whole body effector " << effectorName << " optimization"
          << std::endl;

  return 0;
}