Manipulators are constructed from a series of links, each connected by an actuator. A series of links is also called a chain, with one end defined as the base, and the other called the end effector. Actuators are commonly either revolute joints which cause rotation about an axis, or prismatic joints, which cause translation along an axis. All of the AIBO's and Chiara's joints are revolute, but some types of robot arms include prismatic joints..

The global variable kine holds a Kinematics instance whose joint angles are determined from current world state values, udpated automatically from the robot's sensors. We can use this instance to compute various reference frame transformations. Let's start with the simplest possible transformation:

cout << "Transform Base to Base:" << endl;
cout.precision(3);
cout << setw(7) << kine->jointToBase(BaseFrameOffset) << endl;

Since we are asking for a transformation from the base frame to itself, this should print out the identity matrix:

Transform Base to Base:
  1.000   0.000   0.000   0.000
  0.000   1.000   0.000   0.000
  0.000   0.000   1.000   0.000
  0.000   0.000   0.000   1.000

The rotational submatrix (shown in red) is a 3x3 identity matrix, the translational component (shown in blue) is zero, and the entire transformation matrix is a 4x4 identity matrix.

Now consider the transformation from the tilt to the base reference frame, i.e., from reference frame 1 to reference frame 0 on the head chain. The table at left gives the full definition of the head, mouth, and IR chains. The notation indicates that reference frame 1 (tilt) is defined with respect to reference frame 0 (base), with an x offset of 67.5 cm and z offset of 19.5 cm. The pan joint reference frame has the same origin as the tilt joint, so the x,y,z offsets from reference frame 2 to reference frame 1 are all zero. The nod joint (reference frame 3) is offset 80 mm along z2, the pan joint reference frame's z axis. The camera reference frame (cam) is defined with respect to reference frame 3 (nod), with an x offset of 81.06 cm and y offset of -14.6 cm.

Exercise: Examining Transformation Matrices

The relationship between reference frame 1 (tilt) and the base frame is fixed, because the base frame is a non-moving joint. So the transformation matrix from tilt to base should be the same no matter what posture the head is in. Let's see:

#ifndef INCLUDED_DstBehavior_h_
#define INCLUDED_DstBehavior_h_
 
#include <iostream>
#include <iomanip>

#include "Motion/Kinematics.h"
#include "Shared/newmat/newmatio.h"
#include "Shared/WorldState.h"

class DstBehavior : public BehaviorBase {

public:
  DstBehavior() : BehaviorBase("DstBehavior") {}
 
  virtual void DoStart() {
    BehaviorBase::DoStart();
    std::cout << getName() << " is starting up." << std::endl;
    cout.precision(3);
    
    const float headtilt = state->outputs[HeadOffset+TiltOffset];
    std::cout << "Head tilt is " << headtilt * 360/(2*M_PI) << " degrees." << std::endl;

    NEWMAT::Matrix Ttiltj(kine->jointToBase(HeadOffset+TiltOffset));
    NEWMAT::Matrix Ttiltl(kine->linkToBase (HeadOffset+TiltOffset));
    std::cout << "tilt jointToBase= " << std::endl << setw(7) << Ttiltj << std::endl;
    std::cout << "tilt linkToBase=  " << std::endl << setw(7) << Ttiltl << std::endl;
    DoStop();
  }
 
  virtual void DoStop() {
    std::cout << getName() <<" is shutting down." << std::endl; 
    BehaviorBase::DoStop();
  }

protected:  // Dummy functions to satisfy the compiler
  DstBehavior(const DstBehavior&);
  DstBehavior& operator=(const DstBehavior&);

};

#endif

Let's look at the output from the above exercise. First, for a zero degree tilt angle we should see the following:

DstBehavior is starting up.
Head tilt is 1.25 degrees.
tilt jointToBase=
  1.000   0.000   0.000  67.500
  0.000   0.000  -1.000   0.000
  0.000   1.000   0.000  19.500
  0.000   0.000   0.000   1.000

tilt linkToBase=
  1.000  -0.022   0.000  67.500
  0.000   0.000  -1.000   0.000
  0.022   1.000   0.000  19.500
  0.000   0.000   0.000   1.000

DstBehavior is shutting down.

Since the angle of the tilt joint is close to zero, the linkToBase transformation matrix matches the jointToBase matrix. Any difference between their rotational components (green vs. red), e.g., the green values of +/-0.022 shown above, is due to the head tilt value not being precisely 0.000.

Now let's use the Posture Editor to tilt the head down by 30 degrees and see what happens:

DstBehavior is starting up.
Head tilt is -29.5 degrees.
tilt jointToBase=
  1.000   0.000   0.000  67.500
  0.000   0.000  -1.000   0.000
  0.000   1.000   0.000  19.500
  0.000   0.000   0.000   1.000

tilt linkToBase=
  0.871   0.492   0.000  67.500
  0.000   0.000  -1.000   0.000
 -0.492   0.871   0.000  19.500
  0.000   0.000   0.000   1.000

DstBehavior is shutting down.

Explore more:

Interest Points

The diagrams above show the interest points defined for the ERS-7 head. For example, interest point "HeadButton" is located at the top of the head, at point M on the diagram. It is defined in link reference frame 3. This indicates that the HeadButton interest point moves with the head tilt, pan, and nod, but not with jaw motion (reference frame 4). Points A-D in the diagram are located on the lower jaw, and do shift with jaw motion; they are defined in reference frame 4.

To retrieve the current location of the head button in base frame coordinates, you would write getJointInterestPoint(BaseFrameOffset,"HeadButton") or getLinkInterestPoint(BaseFrameOffset,"HeadButton"). (The two are equivalent for the base frame.) Notice that the interest point name is passed as a string. This string can also be a comma-separated list of interest points, in which case their coordinates are averaged together. So to get the coordinates of the tip of the lower lip in the reference frame of the camera, you would write getJointInterestPoint(CameraFrameOffset,"LowerLeftLowerLip,LowerRightLowerLip"). The result is a 4x1 ColumnVector.