Category Archives: Project Hexapod B.E.T.H.

Project B.E.T.H.

New Processor: Robotis OpenCM9.04 w/ ARM Cortex M3

The last “brain” BETH had was a Raspberry Pi. The Pi worked well and was of course very powerful, but I started to get sick of it.

Having to power on, wait a minute for boot, SSH in, samba share my source files, run program, issue shutdown command, wait for shutdown, and power off all the time got old. I tried to improve things by hooking up an LCD that would run a start up menu on boot up. It would show me the IP address so i could remote-in easier and allow me to run the BETH program with a button press. It helped, but ultimately all it was doing was adding ever more complexity to both hardware and software. I began to realize that all of this was slowing down development because I often didn’t feel like going through the hoops to get started. (boo hoo, right?)

Another con to the Raspberry Pi was that relative to a microcontroller, it used a lot of power and battery life was much less than what it was with the Arbotix microcontroller.

Lastly, I no longer wanted to deal with the Linux overhead. Robot projects are fun, Linux projects are fun, but i’d rather keep them separate so i can focus on one thing at a time.

So all this drove me to find a new processor. The timing was right, though, because Robotis just released the OpenCM9.04.

Robotis OpenCM 9.04

Robotis OpenCM 9.04

This is a totally open-source ARM Cortex M3 based microcontroller designed for use with Dynamixel servos and general robotics use. It uses what’s essentially the Arduino IDE with an ARM toolchain hooked up. It’s very easy to use with the built in 3-pin Dynamixel connectors, voltage regulator, and 3 UARTs. For hardware, all I i had to do was run power, ground, TX, and RX to an Xbee break out board. The board is powered from my powered Dynamixel hub through the Dynamixel connector. The other nice thing is that this board is quite small, about half the size of the original Arbotix controller, and allows my to install the battery back inside the hex body.

For software, I reverted back to my Arduino BETH code, updated to my more recent Raspberry Pi trajectory/gait algorithms, and had it all running in a couple hours. The functioning code is on my GitHub.

I must say its refreshing to have an “instant-on” robot. It makes it much easier to casually boot up and work on it without having to commit for a few hours due to booting up, software updates, remote logins, etc. It’s also nice to focus purely on the software that is controlling the robot directly.

As for power, this is significantly more powerful than the ol AVR in the Arbotix controller (72MHz vs 16MHz, 32bit vs 8bit, etc). The only thing this lacks is a hardware FPU for floating point math. I think this will cut it for my FK/IK/trajectory type needs for now. I do have an upcoming project that may push this limit, but more on that later.

Videos of B.E.T.H.!

I’ve been really slack about ironing out the last few bugs and taking some video of B.E.T.H. in action… But here it is! Some of the movements are a little erratic, but that’s because i’m filming with one hand while controlling the dual joystick controller with the other.

Here is a body forward kinematics demo. Here you can see that i can input body angles via the controller and the legs move as necessary to allow the body rotation without moving the feet.

Next is the tripod gait. This is a simple and fast gait in which two sets of three legs forming a triangle move alternately.

This is a wave gait. The wave gait is slow and moves only one foot at a time. The rest of the legs have to pull backwards in unison.

Finally, we have my favorite, the ripple gait. Two legs move at a time with one leg beginning to step as the other hits it’s peak height in the step. Because two legs move at once, its faster than the wave gait, but slower than the tripod.

For more info on gaits, here is a good article from Oricom Tech. My final code used here is available on my GitHub.

For more info on B.E.T.H., head back to the beginning.

Raspberry Pi Hardware Integration

A few weeks ago I got a Raspberry Pi to run in B.E.T.H. I had been running it all strewn around on the desk, but yesterday I finally got it all packed inside the chassis. Here’s a run down of what I did.



So as you can see, I mounted the Xbee breakout on the Pi Proto Plate. At first the Plate was sitting up too tall. It originally was the full length of the Raspberry Pi and spaced up enough using an extended header that it covered the ethernet and USB ports. I ended up cutting the board down and mounting it on a standard height header so it could sit lower. I mounted the Pi by glue gunning it to plastic standoffs which screw into the chassis.


As for power, I’m running the battery power into the regulator breakout board and using it as a distribution block to jumper battery power to the power Dynamixel hub. The 5V output of the regulator goes to the Raspberry Pi. I’m powering the Pi through the GPIO power pins accessed through the Pi Proto Plate.


I plan to order shorter Dynamixel cables and shorten up the battery/switch wires to clean things up, but as of now it looks like this:


Unfortunately the battery has to go on top of the robot. I now run the wifi adapter and USB2AX in the Pi’s USB ports. I then SSH and/or VNC in over the network to work on bot. If i have to I can reach in and plug a USB hub cable in in place of the wifi adapter.

Back to B.E.T.H.’s main index.

Phase 4: Foot Trajectory Planning and Gait Sequencing Part II

In the previous post, I went over my initial linear approach to stepping and why it didn’t work out so well. This post outlines my current method which is working much better.

The new method uses a sinusoidal “arch” shaped step in the ZX plane. This also works out well since a single sine function can cover the raise and lower part of the step in one time segment. By using a sinusoidal function for the front-to-back stepping in the X direction as well, i could cut the number of case in half to just two. The final benefit is that it produces a very smooth and natural motion with the smooth curves and foot speeds. Here’s what step code looks like:

switch (caseStep[legNum]){

                case 1: //forward raise and lower

                    leg[legNum].footPos.x = -amplitudeX*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.y = -amplitudeY*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.z = -abs(amplitudeZ)*sin(M_PI*tick/numTicks);

                    if( tick >= numTicks-1 ) caseStep[legNum] = 2;

                case 2: // pull back

                    leg[legNum].footPos.x = amplitudeX*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.y = amplitudeY*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.z = 0;

                    if( tick >= numTicks-1 ) caseStep[legNum] = 1;

          }// end of case statement

As you noticed, the sequencing itself is carried out by the case statement block. They key to the sequencing is in the caseStep[] array. The array has six elements, one for each leg. The variable defines at which case each leg begins. For a tripod gait, the front right leg (#1), rear right (#3), and middle left (#5) leg all start at the first case and step together. The other legs start 180deg off which is the second case. So for my prior four-case linear “triangle” step function, caseStep[] is defined as {1,3,1,3,1,3}. For the new two-case “sine” step function, caseStep[] is defined as {1,2,1,2,1,2}.

Each case statement increments caseStep[] to the next case in order to advance to the next portion of the foot step. The final code looks like:


void tripodGaitSine(){

    float sinRotZ, cosRotZ;
    int totalX, totalY;
    float rotSpeedOffsetX[6], rotSpeedOffsetY[6];
    float amplitudeX, amplitudeY, amplitudeZ;
    int duration;
    int numTicks;
    int speedX, speedY, speedR;

    if( (abs(commanderInput.Xspeed) > 5) || (abs(commanderInput.Yspeed) > 5) || (abs(commanderInput.Rspeed) > 5 ) ){

        duration = 500;                               //duration of one step cycle (ms)
        numTicks = round(duration / SERVO_UPDATE_PERIOD / 2.0); //total ticks divided into the two cases

        speedX = 180*commanderInput.Xspeed/127;        //180mm/s top speed for 180mmm stride in one sec
        speedY = 180*commanderInput.Yspeed/127;        //180mm/s top speed
        speedR = 40*commanderInput.Rspeed/127;         //40deg/s top rotation speed

        sinRotZ = sin(radians(speedR));
        cosRotZ = cos(radians(speedR));

        for( int legNum=0; legNum<6; legNum++){                        totalX = leg[legNum].initialFootPos.x + leg[legNum].legBasePos.x;              totalY = leg[legNum].initialFootPos.y + leg[legNum].legBasePos.y;                          rotSpeedOffsetX[legNum] = totalY*sinRotZ + totalX*cosRotZ - totalX;               rotSpeedOffsetY[legNum] = totalY*cosRotZ - totalX*sinRotZ - totalY;                           if( abs(speedX + rotSpeedOffsetX[legNum]) > abs(speedY + rotSpeedOffsetY[legNum]) )
		amplitudeZ = ((speedX + rotSpeedOffsetX[legNum])*duration/3000.0);
		amplitudeZ = ((speedY + rotSpeedOffsetY[legNum])*duration/3000.0);

            amplitudeX = ((speedX + rotSpeedOffsetX[legNum])*duration/2000.0);
            amplitudeY = ((speedY + rotSpeedOffsetY[legNum])*duration/2000.0);

            switch (caseStep[legNum]){

                case 1: //forward raise and lower

                    leg[legNum].footPos.x = -amplitudeX*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.y = -amplitudeY*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.z = -abs(amplitudeZ)*sin(M_PI*tick/numTicks);

                    if( tick >= numTicks-1 ) caseStep[legNum] = 2;

                case 2: // pull back

                    leg[legNum].footPos.x = amplitudeX*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.y = amplitudeY*cos(M_PI*tick/numTicks);
                    leg[legNum].footPos.z = 0;

                    if( tick >= numTicks-1 ) caseStep[legNum] = 1;

          }// end of case statement

        }// end of loop over legs
        if (tick < numTicks-1) tick++;
        else tick = 0;

    }//end if joystick active


I think it needs a little fine tuning of the speeds and step durations, but its essentially done. B.E.T.H. is now fully mobile!

Check out where this all began.

Phase 4: Foot Trajectory Planning and Gait Sequencing Part I

Thanks to my handy IK engine, I no longer need to think in terms of joint angles. All i need to do is move the tip of the foot around in 3D space using the footPos.x, footPos.y, and footPos.z variables.   Stepping is really as simple as looping and incrementing them in such a way to produce the desired movement. Incrementing X moves the foot in the forward X direction. Incrementing X and Y at the same time moves the foot on that diagonal, etc.

I’ve been putting this post off for awhile because I keep changing how I’m implementing the foot stepping. I’ll go through the history for learning’s sake.

At first I was using linear equations to “draw” straight lines with the foot. A foot would step diagonally up and forward, down and forward, then pull back along the ground in the shape of a triangle.

            switch (caseStep[legNum]){

                case 1: //forward raise

                    leg[legNum].footPos.x = ((long)(speedX + strideRotOffsetX[legNum])*tick*SERVO_UPDATE_PERIOD)/duration - (speedX + strideRotOffsetX[legNum])/4;
                    leg[legNum].footPos.y = ((long)(speedY + strideRotOffsetY[legNum])*tick*SERVO_UPDATE_PERIOD)/duration - (speedY + strideRotOffsetY[legNum])/4;
                    leg[legNum].footPos.z = ((long)height*tick*SERVO_UPDATE_PERIOD)/(duration/4);

                    if( tick >= numTicks-1 ) caseStep[legNum] = 2;

                case 2: // forward lower

                    leg[legNum].footPos.x = ((long)(speedX + strideRotOffsetX[legNum])*tick*SERVO_UPDATE_PERIOD)/duration;
                    leg[legNum].footPos.y = ((long)(speedY + strideRotOffsetY[legNum])*tick*SERVO_UPDATE_PERIOD)/duration;
                    leg[legNum].footPos.z = height - ((long)height*tick*SERVO_UPDATE_PERIOD)/(duration/4);

                    if( tick >= numTicks-1 ) caseStep[legNum] = 3;

                case 3: // down pull back

                    leg[legNum].footPos.x = -((long)(speedX + strideRotOffsetX[legNum])*tick*SERVO_UPDATE_PERIOD)/duration + (speedX + strideRotOffsetX[legNum])/4;
                    leg[legNum].footPos.y = -((long)(speedY + strideRotOffsetY[legNum])*tick*SERVO_UPDATE_PERIOD)/duration + (speedY + strideRotOffsetY[legNum])/4;
                    leg[legNum].footPos.z = 0;

                    if( tick >= numTicks-1 ) caseStep[legNum] = 4;

                case 4: // down pull back

                    leg[legNum].footPos.x = -((long)(speedX + strideRotOffsetX[legNum])*tick*SERVO_UPDATE_PERIOD)/duration;
                    leg[legNum].footPos.y = -((long)(speedY + strideRotOffsetY[legNum])*tick*SERVO_UPDATE_PERIOD)/duration;
                    leg[legNum].footPos.z = 0;

                    if( tick >= numTicks-1 ) caseStep[legNum] = 1;

          }// end of case statement

This worked well and was fast to process on the Arduino, but it had a couple of issues. First, the leg movement looked a little rigid and unnatural. Sort of jerky with the sudden direction changes. Second and most importantly, the robot wasn’t walking smoothly forward. It would sort of stutter as it moved continuously in a direction. I realized that this was because the foot was coming down and hitting the ground in the opposite direction to travel. This caused a momentary halt in the forward momentum before the foot began to pull backwards. The foot should at least be coming straight down toward the ground before pulling back. I’ll explain how I do this once its ready for prime time.

To be continued in Part II!

Jump back to the beginning of Project B.E.T.H.

B.E.T.H.’s New Brain: Raspberry Pi

Half out of future necessity and half out of simply the urge to tinker with something new, I got a Raspberry Pi. I had considered other microcomputers like the Beagle Bone Black, but I decided to go with the Pi because of software/driver maturity and large community. All the peripheral support, drivers, dev tools, etc I’d need for my projects on the Pi has been well sorted out, optimized, and nicely documented. I want to make cool stuff, not be a pioneer of the embedded linux world.

I have a couple of projects in mind for the Pi. The first will be to power my hexapod robot, B.E.T.H. The Pi will have enough grunt to run all my IK and future terrain adaption algorithms as well as do some light machine vision. The second project I think will be a digital dashboard/data logger for my car.

So far i have the Pi up and running pretty well. I’m using the WiringPi libraries to recreate some of the Arduino functions. It was fairly straight forward porting over the original arduino code to C++ on the Pi. I’ve been coding on the Pi directly using the Geany IDE in X. I’m using floating point math to improve accuracy where ever effective.

The overall hardware plan for B.E.T.H. looks like this:


I’m using an Xbee breakout board from Sparkfun to physically connect the Xbee. It only takes power, ground, TX, and RX connected to the Pi’s UART and it works like a champ. Note that the Pi’s UART is configured to run a console output by defualt. You have to disable that functionality before its free to use.

To control the Dynamixel AX-12A servos, I’m using the USB2AX from Xevelabs. Its great due to its small form factor and USB speed optimizations. For software, you use the Robotis Dynamixel SDK.

The BEC is an RC type from Castle Creations. It works perfectly because its small and light, can take up to 25VDC in, and the adjustable output is programmed to 5.1V by default which is perfect for powering the USB hub and Pi. UPDATE: The CC BEC crapped out on me after about 20 minutes. I’m not sure why. I’ve since switched to the Murata switching regulator and it’s been great.

The D-Link DWA-131 USB wifi adapter isn’t necessary for running B.E.T.H., but its needed to SSH/VNC into the Pi or to run the Pi directly and access the network/internet wirelessly. For $20 its worth having. I chose this model because the chipset is supported out of the box by the linux kernel and again, its pretty small.

The last part I needed was a powered Dynamixel hub. This acts as a normal Dynamixel hub but it also provides an easy interface for applying power to the servos. I’ll probably use the set screw terminals as a distribution block for branching power.

My next task is cramming all of this stuff into the hex body!

Phase 3: Body Kinematics, i.e., Body Translation/Rotation with the Feet Planted

The next thing I wanted to accomplish was being able to translate and rotate the body while keeping the feet planted. For now this will simple enable the robot to wiggle around and “dance”, but eventually these movements will be necessary inputs for traversing uneven terrain.

The concept for this is pretty straightforward. If i want the body to translate right while keeping the feet planted, you simply move all the feet to the left relative to the body:

newFootPositionX = initialFootPositionX - translateX
newFootPositionY = initialFootPositionY - translateY
newFootPositionZ = initialFootPositionZ - translateZ

I’m subtracting the translation because the feet are moving opposite the direction i want the body to go in. ‘translateY’ could be an input from your controller. I using the Bioloid Commander library for easy access to the Commander controller inputs.

Rotations aren’t quite as simple. The concept is to take an angular input, and provide the X,Y,Z offsets necessary for each leg to create that body rotation. A perfect job for the rotation matrix!

Rotation Matrices

Here is some good background info on rotation matrices. Its not nearly as hard as it looks. You simply apply one rotation matrix per axis of rotation. I wanted to be able to rotate on all three axis so i strung all three together.

(X,Y,Z)rotated = (X,Y,Z)initial*Rx*Ry*Rz

The angle pheta in each matrix is the input rotation angle for that axis. You do need some sort of software to crunch this up into closed form equations. I used Matlab which can do the symbolic matrix multiplication. Once I got my equations i was able to calculate my (X,Y,Z)rotated offsets and add them into the equations above to get:

newFootPositionX = initialFootPositionX - translateX - rotatedX
newFootPositionY = initialFootPositionY - translateY - rotatedY
newFootPositionZ = initialFootPositionZ - translateZ - rotatedZ

This *almost* works worked perfectly. The issue is that my corner legs were behaving funny. That i realized was because my math wasn’t taking into account their mounting angle.  Basically each corner points 45 degrees out. I need to rotate all my coordinates for these legs and there’s no better tool than… the rotation matrix! This time its easier because this is a single axis rotation so there is only one matrix, Rz.

(X,Y,Z)coxaCorrected = (X,Y,Z)newFootPosition*Rz

Note that the 45deg input is the angle for the front right leg. The right rear leg would be 135deg, the left rear would be 225deg, and the upper left would be 315deg as you go around the clock.

Here’s the code:

    Calculates necessary foot position (leg space) to acheive commanded body rotations, translations, and gait inputs
void bodyFK(){

    float sinRotX, cosRotX, sinRotY, cosRotY, sinRotZ, cosRotZ;
    int totalX, totalY, totalZ;
    int tempFootPosX[6], tempFootPosY[6], tempFootPosZ[6];
    int bodyRotOffsetX[6], bodyRotOffsetY[6], bodyRotOffsetZ[6];

    sinRotX = sin(radians(-commanderInput.bodyRotX));
    cosRotX = cos(radians(-commanderInput.bodyRotX));
    sinRotY = sin(radians(-commanderInput.bodyRotY));
    cosRotY = cos(radians(-commanderInput.bodyRotY));
    sinRotZ = sin(radians(-commanderInput.bodyRotZ));
    cosRotZ = cos(radians(-commanderInput.bodyRotZ));

    for( int legNum=0; legNum<6; legNum++){

        totalX = leg[legNum].initialFootPos.x + leg[legNum].legBasePos.x;
        totalY = leg[legNum].initialFootPos.y + leg[legNum].legBasePos.y;
        totalZ = leg[legNum].initialFootPos.z + leg[legNum].legBasePos.z;

        bodyRotOffsetX[legNum] = round(( totalY*cosRotY*sinRotZ + totalY*cosRotZ*sinRotX*sinRotY + totalX*cosRotZ*cosRotY - totalX*sinRotZ*sinRotX*sinRotY - totalZ*cosRotX*sinRotY) - totalX);
        bodyRotOffsetY[legNum] = round(  totalY*cosRotX*cosRotZ - totalX*cosRotX*sinRotZ         + totalZ*sinRotX         - totalY);
        bodyRotOffsetZ[legNum] = round(( totalY*sinRotZ*sinRotY - totalY*cosRotZ*cosRotY*sinRotX + totalX*cosRotZ*sinRotY + totalX*cosRotY*sinRotZ*sinRotX + totalZ*cosRotX*cosRotY) - totalZ);

        // Calculated foot positions to acheive xlation/rotation input. Not coxa mounting angle corrected
        tempFootPosX[legNum] = leg[legNum].initialFootPos.x + bodyRotOffsetX[legNum] - commanderInput.bodyTransX + leg[legNum].footPos.x;
        tempFootPosY[legNum] = leg[legNum].initialFootPos.y + bodyRotOffsetY[legNum] - commanderInput.bodyTransY + leg[legNum].footPos.y;
        tempFootPosZ[legNum] = leg[legNum].initialFootPos.z + bodyRotOffsetZ[legNum] - commanderInput.bodyTransZ + leg[legNum].footPos.z;

    // Rotates X,Y about coxa to compensate for coxa mounting angles.
    leg[0].footPosCalc.x = round( tempFootPosY[0]*cos(radians(COXA_ANGLE))   - tempFootPosX[0]*sin(radians(COXA_ANGLE)) );
    leg[0].footPosCalc.y = round( tempFootPosY[0]*sin(radians(COXA_ANGLE))   + tempFootPosX[0]*cos(radians(COXA_ANGLE)) );
    leg[0].footPosCalc.z =        tempFootPosZ[0];
    leg[1].footPosCalc.x = round( tempFootPosY[1]*cos(radians(COXA_ANGLE*2)) - tempFootPosX[1]*sin(radians(COXA_ANGLE*2)) );
    leg[1].footPosCalc.y = round( tempFootPosY[1]*sin(radians(COXA_ANGLE*2)) + tempFootPosX[1]*cos(radians(COXA_ANGLE*2)) );
    leg[1].footPosCalc.z =        tempFootPosZ[1];
    leg[2].footPosCalc.x = round( tempFootPosY[2]*cos(radians(COXA_ANGLE*3)) - tempFootPosX[2]*sin(radians(COXA_ANGLE*3)) );
    leg[2].footPosCalc.y = round( tempFootPosY[2]*sin(radians(COXA_ANGLE*3)) + tempFootPosX[2]*cos(radians(COXA_ANGLE*3)) );
    leg[2].footPosCalc.z =        tempFootPosZ[2];
    leg[3].footPosCalc.x = round( tempFootPosY[3]*cos(radians(COXA_ANGLE*5)) - tempFootPosX[3]*sin(radians(COXA_ANGLE*5)) );
    leg[3].footPosCalc.y = round( tempFootPosY[3]*sin(radians(COXA_ANGLE*5)) + tempFootPosX[3]*cos(radians(COXA_ANGLE*5)) );
    leg[3].footPosCalc.z =        tempFootPosZ[3];
    leg[4].footPosCalc.x = round( tempFootPosY[4]*cos(radians(COXA_ANGLE*6)) - tempFootPosX[4]*sin(radians(COXA_ANGLE*6)) );
    leg[4].footPosCalc.y = round( tempFootPosY[4]*sin(radians(COXA_ANGLE*6)) + tempFootPosX[4]*cos(radians(COXA_ANGLE*6)) );
    leg[4].footPosCalc.z =        tempFootPosZ[4];
    leg[5].footPosCalc.x = round( tempFootPosY[5]*cos(radians(COXA_ANGLE*7)) - tempFootPosX[5]*sin(radians(COXA_ANGLE*7)) );
    leg[5].footPosCalc.y = round( tempFootPosY[5]*sin(radians(COXA_ANGLE*7)) + tempFootPosX[5]*cos(radians(COXA_ANGLE*7)) );
    leg[5].footPosCalc.z =        tempFootPosZ[5];


NOW it works!

<insert video here>

Head back to the beginning of Project B.E.T.H.

Phase 2: Leg Inverse Kinematics, i.e., Pointing a Foot in 3D Space

Inverse Kinematics (IK) is a really handy tool used when controlling moving linkages. It’s used whenever you want to move a series of linkages by locating the end of the linkages instead of having to worry about all the joint angles in between. For example, your brain does this when you want to grab a cup of coffee. You don’t think “shoulder angle to X, elbow angle to Y, wrist angle to Z”. You think “hand to coffee mug” and your brain takes care of all the angles necessary to achieve the hand goal position. IK is commonly used not only in robotics, but also in 2D and 3D animation, for example.

Like I mentioned in my intro post, I first worked on some leg IK a few  years ago. To get started, i dug up my old code. The method I used was a bit overkill and involved using rotation matrices rotating coordinate spaces defined for each joint in the leg. It basically involves setting up a bunch of matrices in a certain form and then crunching them up into closed form equations. It ended up being too processor intensive and took like 17ms to run. That’s out of my 33ms frame budget! There’s a lot more crunching ahead so that’s just too much. I decided to take a simpler approach which is possible with our simple 3DOF legs. You can figured it all out with some basic trig.

One great resource I found is Oscar Liang’s blog. His tutorial on solving 3DOF IK using trig is very clear and has better diagrams than i can draw so take a look at it here.

Going through his tutorial I was most of the way home. The final step was to add in angular offsets for my physical hardware. Because the Tibia servo is mounted on an offset bracket off the Femur, I added in that angle. And then because the tibia itself is mounted straight, i added in 90deg so that it pointing downward is centered. Finally the tibia itself has a bend to it so i added an offset for that as well.

My final function looks like:

Calculates the coxa, femur, and tibia angles for input foot position in X,Y,Z.
void legIK(){

float CoxaFootDist, IKSW, IKA1, IKA2, tibAngle;

for( int legNum=0; legNum<6; legNum++ ){ CoxaFootDist = sqrt( sq(leg[legNum].footPosCalc.y) + sq(leg[legNum].footPosCalc.x) ); IKSW = sqrt( sq(CoxaFootDist-LENGTH_COXA) + sq(leg[legNum].footPosCalc.z) ); IKA1 = atan2( (CoxaFootDist - LENGTH_COXA) , leg[legNum].footPosCalc.z ); IKA2 = acos( (sq(LENGTH_TIBIA) - sq(LENGTH_FEMUR) - sq(IKSW) ) / (-2*IKSW*LENGTH_FEMUR) ); tibAngle = acos( (sq(IKSW) - sq(LENGTH_TIBIA) - sq(LENGTH_FEMUR)) / (-2*LENGTH_FEMUR*LENGTH_TIBIA) ); leg[legNum].jointAngles.coxa = 90 - degrees( atan2( leg[legNum].footPosCalc.y , leg[legNum].footPosCalc.x) ); leg[legNum].jointAngles.femur = 90 - degrees( IKA1 + IKA2 ); leg[legNum].jointAngles.tibia = 90 - degrees( tibAngle ); } // Applies necessary corrections to servo angles to account for hardware for( int legNum=0; legNum<3; legNum++ ){ leg[legNum].jointAngles.coxa = leg[legNum].jointAngles.coxa; leg[legNum].jointAngles.femur = leg[legNum].jointAngles.femur - 13.58; // accounts for offset servo bracket on femur leg[legNum].jointAngles.tibia = leg[legNum].jointAngles.tibia - 48.70 + 13.58 + 90; //counters offset servo bracket on femur, accounts for 90deg mounting, and bend of tibia } for( int legNum=3; legNum<6; legNum++ ){ leg[legNum].jointAngles.coxa = leg[legNum].jointAngles.coxa; leg[legNum].jointAngles.femur = -(leg[legNum].jointAngles.femur - 13.58); leg[legNum].jointAngles.tibia = -(leg[legNum].jointAngles.tibia - 48.70 + 13.58 + 90); } }[/cpp] On to Phase 3: Body Kinematics, i.e., Body Translation/Rotation with the Feet Planted!

Or head back to the beginning of Project B.E.T.H.

Phase 1: Driving Servos

Robotis Dynamixel AX-12A

The first order of business is making the servos move.

First, I found the handy Dynamixel AX-12A datasheet. A slimmed down version is here. These describe the serial interface, the instruction set, and how to create an instruction packet.

Some key points are that the servos run daisy chained on a half duplex serial bus. This means that on your UART hardware, you need to connect the RX and TX lines. This is easily done on the Arbotix Robocontroller with a jumper. It also says that the max communication speed is 1Mbps and provides details on the format of the instruction packet.

Honestly, this part isn’t especially thrilling to me, so i borrowed the ax12 library from the Arbotix bioloid library. This library includes handy low level functions like the basic data write to the bus. I downloaded the full Arbotix libraries here. I copied the ax12.cpp and ax12.h files in the arbotix\libraries\Bioloid directory into my Arduino sketch. This way i could modify them without affecting the public library.

I got pretty far using just these two files. I first initialized the servo serial interface by calling ‘ax12Init(1000000);’ in my Setup() loop. This initializes the serial port and sets it to the maximum 1Mbps baud rate. Full throttle or bust I say.

To command a servo to a position, I simply call ‘SetPosition(id, pos);’ from the library. ID is the ID of my servo and POS is the commanded position. Here’s how the AX-12 maps position:

AX-12A Position Orientation

Note that it’s a little strange how the Position is oriented on the servo. Position ranges from 0 to 1024 which corresponds to angle 0 to 300. There is a 60 degree dead space at the bottom of the servo and Positon 0 starts 30degrees off from where you’d expect to start. I designed my hex so that 0 degrees was pointing straight up and the servo could angle +/-  CW or CCW from there. The code to convert my angle system to Dynamix positioin looks like:

servoPosition  = round((abs( anglePosition  - 210) - 60 )  / 0.293);

Where ‘anglePosition’ is the angle I want using my orientation/nomenclature and ‘servoPositoin’ is the corresponding position in AX-12 speak.

But wait, there’s more!

So using the using the above SetPosition function from the ax12 library, I’m sending one command instruction per servo. This is slow and just generally ugly when there is slick ‘SYNC WRITE’ function in the AX-12 instruction set. The only problem is that there isn’t such a function in the ax12 library to use. But hey, I’m supposed to be doing this all myself right?

Page 23 of the datasheet explains the SYNC WRITE instruction and how to set it up. Basically with one instruction, you can command a whole range of servos, each to their own Goal Position. Nice and fast and neat. Here’s what my rather ugly function looks like:

    Writes positions to all servos at once using sync write
void ax12SyncWriteServos(){

    int length = (2 + 1)* NUM_SERVOS + 4;   // (L + 1) * N + 4 (L: Data length for each Dynamixel actuator, N: The number of Dynamixel actuators)
    int checksum = 254 + length + AX_SYNC_WRITE + 2 + AX_GOAL_POSITION_L;
    ax12write(0xFF);                // Start bits
    ax12write(0xFF);                // Start bits
    ax12write(0xFE);                // Broadcast ID for all servos
    ax12write(AX_SYNC_WRITE);       // Instruction
    ax12write(AX_GOAL_POSITION_L);  // Parameter 1: Starting address of the location where the data is to be written
    ax12write(2);                   // Parameter 2: The length of the data to be written (L)

    checksum += RF_COXA_ID + (leg[RIGHT_FRONT].servoPos.coxa & 0xff) + (leg[RIGHT_FRONT].servoPos.coxa >> 8);
    ax12write(leg[RIGHT_FRONT].servoPos.coxa & 0xff);
    ax12write(leg[RIGHT_FRONT].servoPos.coxa >> 8);
    checksum += RF_FEMUR_ID + (leg[RIGHT_FRONT].servoPos.femur & 0xff) + (leg[RIGHT_FRONT].servoPos.femur >> 8);
    ax12write(leg[RIGHT_FRONT].servoPos.femur & 0xff);
    ax12write(leg[RIGHT_FRONT].servoPos.femur >> 8);
    checksum += RF_TIBIA_ID + (leg[RIGHT_FRONT].servoPos.tibia & 0xff) + (leg[RIGHT_FRONT].servoPos.tibia >> 8);
    ax12write(leg[RIGHT_FRONT].servoPos.tibia & 0xff);
    ax12write(leg[RIGHT_FRONT].servoPos.tibia >> 8);

    checksum += RM_COXA_ID + (leg[RIGHT_MIDDLE].servoPos.coxa & 0xff) + (leg[RIGHT_MIDDLE].servoPos.coxa >> 8);
    ax12write(leg[RIGHT_MIDDLE].servoPos.coxa & 0xff);
    ax12write(leg[RIGHT_MIDDLE].servoPos.coxa >> 8);
    checksum += RM_FEMUR_ID + (leg[RIGHT_MIDDLE].servoPos.femur & 0xff) + (leg[RIGHT_MIDDLE].servoPos.femur >> 8);
    ax12write(leg[RIGHT_MIDDLE].servoPos.femur & 0xff);
    ax12write(leg[RIGHT_MIDDLE].servoPos.femur >> 8);
    checksum += RM_TIBIA_ID + (leg[RIGHT_MIDDLE].servoPos.tibia & 0xff) + (leg[RIGHT_MIDDLE].servoPos.tibia >> 8);
    ax12write(leg[RIGHT_MIDDLE].servoPos.tibia & 0xff);
    ax12write(leg[RIGHT_MIDDLE].servoPos.tibia >> 8);

    checksum += RR_COXA_ID + (leg[RIGHT_REAR].servoPos.coxa & 0xff) + (leg[RIGHT_REAR].servoPos.coxa >> 8);
    ax12write(leg[RIGHT_REAR].servoPos.coxa & 0xff);
    ax12write(leg[RIGHT_REAR].servoPos.coxa >> 8);
    checksum += RR_FEMUR_ID + (leg[RIGHT_REAR].servoPos.femur & 0xff) + (leg[RIGHT_REAR].servoPos.femur >> 8);
    ax12write(leg[RIGHT_REAR].servoPos.femur & 0xff);
    ax12write(leg[RIGHT_REAR].servoPos.femur >> 8);
    checksum += RR_TIBIA_ID + (leg[RIGHT_REAR].servoPos.tibia & 0xff) + (leg[RIGHT_REAR].servoPos.tibia >> 8);
    ax12write(leg[RIGHT_REAR].servoPos.tibia & 0xff);
    ax12write(leg[RIGHT_REAR].servoPos.tibia >> 8);

    checksum += LR_COXA_ID + (leg[LEFT_REAR].servoPos.coxa & 0xff) + (leg[LEFT_REAR].servoPos.coxa >> 8);
    ax12write(leg[LEFT_REAR].servoPos.coxa & 0xff);
    ax12write(leg[LEFT_REAR].servoPos.coxa >> 8);
    checksum += LR_FEMUR_ID + (leg[LEFT_REAR].servoPos.femur & 0xff) + (leg[LEFT_REAR].servoPos.femur >> 8);
    ax12write(leg[LEFT_REAR].servoPos.femur & 0xff);
    ax12write(leg[LEFT_REAR].servoPos.femur >> 8);
    checksum += LR_TIBIA_ID + (leg[LEFT_REAR].servoPos.tibia & 0xff) + (leg[LEFT_REAR].servoPos.tibia >> 8);
    ax12write(leg[LEFT_REAR].servoPos.tibia & 0xff);
    ax12write(leg[LEFT_REAR].servoPos.tibia >> 8);

    checksum += LM_COXA_ID + (leg[LEFT_MIDDLE].servoPos.coxa & 0xff) + (leg[LEFT_MIDDLE].servoPos.coxa >> 8);
    ax12write(leg[LEFT_MIDDLE].servoPos.coxa & 0xff);
    ax12write(leg[LEFT_MIDDLE].servoPos.coxa >> 8);
    checksum += LM_FEMUR_ID + (leg[LEFT_MIDDLE].servoPos.femur & 0xff) + (leg[LEFT_MIDDLE].servoPos.femur >> 8);
    ax12write(leg[LEFT_MIDDLE].servoPos.femur & 0xff);
    ax12write(leg[LEFT_MIDDLE].servoPos.femur >> 8);
    checksum += LM_TIBIA_ID + (leg[LEFT_MIDDLE].servoPos.tibia & 0xff) + (leg[LEFT_MIDDLE].servoPos.tibia >> 8);
    ax12write(leg[LEFT_MIDDLE].servoPos.tibia & 0xff);
    ax12write(leg[LEFT_MIDDLE].servoPos.tibia >> 8);

    checksum += LF_COXA_ID + (leg[LEFT_FRONT].servoPos.coxa & 0xff) + (leg[LEFT_FRONT].servoPos.coxa >> 8);
    ax12write(leg[LEFT_FRONT].servoPos.coxa & 0xff);
    ax12write(leg[LEFT_FRONT].servoPos.coxa >> 8);
    checksum += LF_FEMUR_ID + (leg[LEFT_FRONT].servoPos.femur & 0xff) + (leg[LEFT_FRONT].servoPos.femur >> 8);
    ax12write(leg[LEFT_FRONT].servoPos.femur & 0xff);
    ax12write(leg[LEFT_FRONT].servoPos.femur >> 8);
    checksum += LF_TIBIA_ID + (leg[LEFT_FRONT].servoPos.tibia & 0xff) + (leg[LEFT_FRONT].servoPos.tibia >> 8);
    ax12write(leg[LEFT_FRONT].servoPos.tibia & 0xff);
    ax12write(leg[LEFT_FRONT].servoPos.tibia >> 8);

    ax12write(0xff - (checksum % 256));

The comments match the descriptions in the datasheet so you can follow along. Basically, the way my servos are ID’ed and leg array is numbered, I can’t cleanly loop over them and index. Maybe someday I’ll redo that. But for now this works well enough.

The last thing I thought about is the servo update rate. After some research i settled on a 30hz update rate or frame rate. This means that the main loop of my program needs to run every 33ms and every loop should call the ax12SyncWriteServos() function to update all the servo positions. This also implies that ALL my calculations for the frame need to be calculated in 33ms! (foreshadowing…)

So at this point I can give all my servos a unique angle and they’ll move to it. Brilliant!

On to Phase 2: Leg Inverse Kinematics, i.e., Pointing a Foot in 3D Space!

Or head back to the beginning of Project B.E.T.H.

The Game Plan for B.E.T.H.

So at this point B.E.T.H. is a stock MkII PhantomX Hexapod with stock code. Pretty boring. The provided code is great for impressing your Mom, but if you really want the attention of your heart’s desire, you’re going to need your own code!

I think the best part of this is learning how it all works. You can stare at other people’s code all day long, but to really “get it” you have to figure it out and write it yourself. So here’s the game plan:

On to Phase 1: Driving the Servos!

Or head back to the beginning of Project B.E.T.H.