Whole Body control API¶

NAOqi Motion - Overview | API | Tutorial

Method list¶

class ALMotionProxy¶

ALMotionProxy::wbEnable()
ALMotionProxy::wbFootState()
ALMotionProxy::wbEnableBalanceConstraint()
ALMotionProxy::wbGoToBalance()
ALMotionProxy::wbEnableEffectorControl()
ALMotionProxy::wbSetEffectorControl()
ALMotionProxy::wbEnableEffectorOptimization()

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 NAO’s behavior 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

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

import sys
from naoqi import ALProxy
import time

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 enable Whole Body Balancer.
    isEnabled = True
    motionProxy.wbEnable(isEnabled)

    time.sleep(2.0)

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


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::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

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

import sys
from naoqi import ALProxy
import time

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)

    motionProxy.wbEnable(True)

    # Example showing how to fix the feet.
    print "Feet fixed."
    stateName = "Fixed"
    supportLeg = "Legs"
    motionProxy.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."
    motionProxy.wbFootState("Fixed", "LLeg")
    motionProxy.wbFootState("Plane", "RLeg")

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

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


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

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

    main(robotIp)

void ALMotionProxy::wbEnableBalanceConstraint(const bool& isEnable, const std::string& supportLeg)¶

Enables to keep balance in support polygon.

Parameters:	isEnable – Enable NAO 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

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

import sys
from naoqi import ALProxy
import time

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)

    motionProxy.wbEnable(True)

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

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


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

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

    main(robotIp)

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

# -*- 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)

    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)

    motionProxy.setStiffnesses("Body", 1.0)
    motionProxy.moveInit()
    motionProxy.wbEnable(True)

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

    motionProxy.wbEnable(False)


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

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

    main(robotIp)

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”. NAO 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

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

import sys
from naoqi import ALProxy
import time

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)

    motionProxy.wbEnable(True)

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

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


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

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

    main(robotIp)

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”. NAO 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

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

import sys
from naoqi import ALProxy
import time

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)

    motionProxy.wbEnable(True)

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

    motionProxy.wbEnableEffectorControl(effectorName, True)
    time.sleep(2.0)
    targetCoordinate = [0.1, 0.0, 0.0]
    motionProxy.wbSetEffectorControl(effectorName, targetCoordinate)

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


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

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

    main(robotIp)

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

NAO Software 1.14.5 documentation

Whole Body control API¶

Method list¶

Table Of Contents

On this page

NAO Software 1.14.5 documentation

Whole Body control API¶

Method list¶

Quick search

Table Of Contents

On this page