Whole Body control API¶

NAOqi Motion - Overview | API | Tutorial

nao NAO only - Do not use on juju Pepper

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

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

import time
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 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)

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

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 time
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)

    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)

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

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

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

import time
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)

    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)

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

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

    motionProxy.wbEnable(True)

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

    motionProxy.wbEnable(False)

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

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

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

import time
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)

    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)

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

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

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

import time
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)

    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.20, 0.12, 0.30]
    motionProxy.wbSetEffectorControl(effectorName, targetCoordinate)

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

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

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

Aldebaran documentation

Whole Body control API¶

Method list¶

Table Of Contents

On this page

Aldebaran documentation

Whole Body control API¶

Method list¶

Quick search

Table Of Contents

On this page