The robot's operating system updates the states of all the effectors (servos, motors, LEDs, etc.) every few milliseconds. Each update is called a "frame", and can accommodate simultaneous changes to any number of effectors. On the AIBO, updates occur every 8 milliseconds and frames are buffered four at a time, so the application must have a new buffer available every 32 milliseconds; other robots may use different update intervals. In Tekkotsu these buffers of frames are produced by the MotionManager, whose job is to execute a collection of simultaneously active MotionCommands (MCs) of various types every few milliseconds. The results of these MotionCommands are assembled into a buffer that is passed to the operating system (Aperios for the AIBO, or Linux for other robots).

Suppose we want the robot to blink its LEDs on and off at a rate of once per second. What we need is a MotionCommand that will calculate new states for the LEDs each time the MotionManager asks for an update. LedMC, a subclass of both MotionCommand and LedEngine, performs this service. If we create an instance of LedMC, tell it the frequency at which to blink the LEDs, and add it to the MotionManager's list of active MCs, then it will do all the work for us. There's just one catch: our application is running in the Main process, while the MotionManager runs in a separate Motion process. This is necessary to assure that potentially lengthy computations taking place in Main don't prevent Motion from running every few milliseconds. So how can we communicate with our MotionCommand while at the same time making it available to the MotionManager?

The solution is to construct MotionCommands in a memory region that is shared by both processes. Because we have continuous access to the MotionCommand, we can change its parameters even while it's active, to tell it to do different things. But it's dangerous to modify a MotionCommand while the MotionManager is in the midst of invoking it. Therefore, Tekkotsu provides a mutual exclusion mechanism called an MMAccessor that temporarily locks out the MotionManager when we need to invoke a MotionCommand's member functions from within Main. Whenever we want to call such functions, we must lock down the MotionCommand by creating an MMAccessor first. Destroying the MMAccessor unlocks the MotionCommand.

There is one remaining wrinkle to the story. When a MotionCommand is passed to the MotionManager, it is assigned a unique ID called an MC_ID that identifies it within the MotionManager's active list. To lock the MotionCommand, we must pass this MC_ID value to the MMAccessor constructor. The MC_ID is also used when we tell the MotionManager to remove this MotionCommand from its active list. So the MC_ID must be saved somewhere. Normally it is kept in a protected data member within the Behavior instance so it can be shared by the doStart, doStop, and doEvent methods.

To summarize: MotionCommands must be instantiated in shared memory. An MC_ID, which is typically stored locally in the Behavior (not in shared memory), uniquely identifies the MotionCommand within the MotionManager's active list. Certain member functions of the MotionCommand will be called repeatedly from within the Motion process, by the MotionManager, to compute updated effector states. An MMAccessor, created in Main using the MC_ID, must be used to lock down an active MotionCommand so we can safely call its member functions from within the Main process. This allows us to modify the MotionCommand's parameters on the fly.

Detecting Motion Completion

#include "Behaviors/StateMachine.h"

$nodeclass MoveBlink : StateNode : leds_id(MotionManager::invalid_MC_ID),   \
		  head_id(MotionManager::invalid_MC_ID) {

  MotionManager::MC_ID leds_id, head_id;

  ...

  virtual void doStart() {
    erouter->addListener(this,EventBase::buttonEGID);
    // set up LED motion command
    SharedObject<LedMC> leds_mc;
    leds_id = motman->addPersistentMotion(leds_mc);
    // set up head pointer motion command
    SharedObject<HeadPointerMC> head_mc;
    head_mc->setMaxSpeed(RobotInfo::TiltOffset,0.5);
    head_id = motman->addPersistentMotion(head_mc);
    erouter->addListener(this, EventBase::motmanEGID, head_id, EventBase::statusETID);
  }

  virtual void doStop() {
    motman->removeMotion(leds_id);
    motman->removeMotion(head_id);
  }

  virtual void doEvent() {

    switch ( event->getGeneratorID() ) {

    case EventBase::buttonEGID:
      std::cout << getName() << " got button event: " << event->getDescription() << std::endl;
      if (event->getTypeID() == EventBase::activateETID) {
	const float lowpos = RobotInfo::outputRanges[HeadOffset+TiltOffset][MinRange];
	const float highpos = RobotInfo::outputRanges[HeadOffset+TiltOffset][MaxRange];
	const float midpos = (lowpos+highpos) / 2;
	MMAccessor<HeadPointerMC> head_acc(head_id);
	double val = head_acc->getJointValue(RobotInfo::TiltOffset);
	head_acc->setJointValue(RobotInfo::TiltOffset, (val > midpos) ? lowpos : highpos);
      }
      break;

    case EventBase::motmanEGID:
      std::cout << getName() << " got motman event: " << event->getDescription() << std::endl;
      { MMAccessor<LedMC> leds_acc(leds_id);
        leds_acc->flash(RobotInfo::FaceLEDMask, 500);
      }
      break; 

    default:
      std::cout << "Unexpected event:" << event->getDescription() << std::endl;
    }
  }

} // end of $nodeclass

Explore more:

1. Suppose you push a button, the head starts to move, and you immediately push the button again. What will the head and LEDs do?

2. When you first activate this behavior, the head moves to a neutral position. Why does it do that?

3. Suppose you didn't want the head to move to a neutral postion on startup. To prevent this, try setting the motion command's initial target position equal to the head's current position. You can access the current tilt position using state->outputs[HeadOffset+TiltOffset]. You will need to include Shared/WordlState.h to do this.

Effector names

Here is how the numbering scheme works: Joints are grouped together in the table based on the robot's anatomy. Thus, all the leg joints have consecutive offsets, as do all the head joints, all the tail joints, and so on. The order of the legs is defined by RobotInfo::LegOrder_t, whose values are 0 to 3, defined symbolically as LFrLegOrder, RFrLegOrder, LBkLegOrder, and RBkLegOrder. The number of joints per leg is given by the constant RobotInfo::NumLegJoints, which is equal to 3 for current AIBO models. There are also symbols defined for each leg offset individually. So:

LFrLegOffset == LegOffset + LFrLegOrder * NumLegJoints
RFrLegOffset == LegOffset + RFrLegOrder * NumLegJoints
LBkLegOffset == LegOffset + LBkLegOrder * NumLegJoints
RBkLegOffset == LegOffset + RBkLegOrder * NumLegJoints


The order of joints within a leg is defined by RobotInfo::REKOffset_t, whose values are 0 to 2, defined symbolically as RotatorOffset, ElevatorOffset, and KneeOffset. (Note: the rotator and elevator joints are both located in the hip/shoulder; the rotator moves the leg forward or back, while the elevator moves it in or out.)

To refer to a specific leg joint, we start with the offset for the leg the joint is on, and add in the offset for the desired joint relative to that leg. So, for example, the offset for the right back knee is given by: RBkLegOffset + KneeOffset.

A similar convention applies to the head. The beginning of the head joints is specified by RobotInfo::HeadOffset. The joint order is defined by RobotInfo::TPROffset_t, which stands for Tilt, Pan, and Roll. (Note: the ERS-7 head does not roll. Instead, it has a second tilt joint called "nod". And the Qwerkbot has only tilt and pan.) The TPROffset values for the head joints are TiltOffset, PanOffset, and RollOffset (or NodOffset for the ERS-7.) So, for example, to access information about the head tilt joint we would use the index value HeadOffset+TiltOffset.

Joints are always referred to by their full indices, with one exception: the HeadPointerMC motion command, because it deals exclusively with the head, refers to joints by their TPROffset without adding in the value of HeadOffset. So if you want to set the head's pan angle using a HeadPointerMC, you would write something like:

head_acc->setJointValue(PanOffset, panvalue)

posture_acc->setOutputCmd(HeadOffset+PanOffset, panvalue)