Page 1 of 4 1234 LastLast
Results 1 to 10 of 33

Thread: Project BETH aka Dan's journey into a lot of things.

  1. #1
    Join Date
    Apr 2013
    Boston, MA
    Rep Power

    Project BETH aka Dan's journey into a lot of things.

    Hello all,

    I'd like to share my progress so far with "BETH" which i hope to someday live up to her name - Battle Enhanced all-Terrain Hexapod. I'll keep working on the name...

    I'm a EE by trade, and am working on this, of course, out of interest, but also to brush up on embedded systems and programming.

    Right now BETH is physically a MKII PhantomX hexapod using AX12 servos. I really want to learn how it all works so I'm starting over software-wise with my own code. Well mostly, I am using the ax12 and commander libraries. What I'm REALLY interested in is traversing uneven terrain. The Boston Dynamic robots are absolutely incredible. I hope to someday have my hex able to navigate off road. I'm also interested in MechWarfare so I'd like to work toward an airsoft gun turret that can maybe follow a color/target and shoot it. Sort of scary, sort of awesome.

    But first, baby steps. (Or baby stands...)

    First, i learned about leg IK. Fortunately there are lots of great resources online. It was really satisfying to go through the math, implement the functions and see them actually work. BETH can point a foot in 3D space!

    Next I tackled the body translations and rotations. Basically this consists of using a rotation matrix to figure out leg X,Y,Z offsets for a given body rotation or translation. These offsets are factored into the legIK algorithm.

    My source for a lot of this info online was this blog: He provides a nice Excel sheet for "simulating" your functions and seeing the hex move. I cleaned up his sheet and made it fit the MkII PhantomX Hex and my coding style. I made the sheet in Open Office. The attachment is both old Excel 97 and ODS format:

    The last thing I did was implement a SYNC-WRITE function for driving my servos. Somehow I sleep better at night knowing my servo instructions are nice and neat and aren't taking any longer than they need to to execute. It could all be in my head, but i swear it moves more smoothly now. Here's my probably less-than-elegant function (I wish i indexed my servos and angles more cleverly):

        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) 
        //RIGHT FRONT
        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);       
        //RIGHT MIDDLE
        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);   
        //RIGHT REAR
        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);  
        //LEFT REAR
        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); 
        //LEFT MIDDLE
        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); 
        //LEFT FRONT
        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));
    So as it stands, I have a hex that can stand still and rotate its body about three axis and can translate its body in three dimensions. I'm running 30hz update clock which runs pretty smoothly. So far I have plenty of time left in the cycle for more computation. I think i'm using 12ms out of the 33ms. I'm sure there is plenty of room for optimizing my current code as well.

    Video here:

    The next step is to think about gait sequencing. I'm currently thinking about how to 'architecture' the code to accomplish the various parameters I'll need to input. I'm trying to be disciplined about designing algorithms before hacking out code. I'm spending quality time with a pencil and paper and flow charts which i'm finding very helpful. I'm not using poses, so I'll need to calculate every foot position along its trajectory at every frame. So far i have one foot stepping so my trajectories are working at a basic level. Now to sequencing...

    I also ordered a 12V 25A fixed DC supply that I hope to use while running the bot on the bench. This should help with the hassle associated with swapping between FTDI power and lipo power all the time.

    I'll keep you guys posted on my journey. Thanks!
    Last edited by TXBDan; 06-07-2013 at 11:21 AM. Reason: added syncwrite code

  2. #2
    Join Date
    Dec 2007
    Whidbey Island, WA
    Rep Power

    Re: Project BETH aka Dan's journey into a lot of things.

    This sounds really cool and interesting. I have been scared of realy learning IK for a long time. Reading about your experience has emboldened me. I'm diving in right now. I know some stuff, and probably enough, but I have never really taken the plunge in a way that is meaningful.

  3. #3
    Join Date
    Apr 2013
    Boston, MA
    Rep Power

    Re: Project BETH aka Dan's journey into a lot of things.

    Awesome, go for it! It's very satisfying to see things moving around based on your calculations.

    The leg IK (strictly converting xyz in space to required joint angles) in our 3DOF case is pretty simple. I originally did it for the leg following a textbook using rotation matrices. It was really just a matter setting up the matrices per the form in the book and using Matlab to string them together and crunch it up. It worked great, but the equations came out a little heavy and were slow to compute.

    I then did it simply using trig like in the oscarliang link above (or any other of the tons of links if you google around for 3DOF IK). It works great and the equations come out simpler and process faster. All I had to do to optimize his equations for my hex were to add angular offsets for my femur-tibia angular difference and the tibia shape and mounting. At this point you can point your foot around in 3D space.

    The next step is the body translation in the X,Y,Z axis. Making the body translate in one direction is the same as having all the legs translate in the opposite. So just subtract your body translation input (from Commander, etc) from all your foot positions and run the legIK(). Easy.

    For rotation, you use three rotations matrices (one for each axis) multiplied together. Its as simple as XYZout = XYZin*Rgamma*Ralpha*Rbeta. ( So you feed your Command rotation inputs along with your current XYZ into the equation and out pops the XYZ offset values. Apply these offsets to all six leg XYZ positions (just like you did for body translations) and run the legIK() and voila, the body is rotating.

    The last thing to do is to rotate the corner legs to compensate for their mounting angles. For example the front right leg coxa is mounted 45deg from straight forward. To do this you use a single rotation matrix rotated about the Z (up and down) axis. XYZout = XYZin*Rgamma

    for all six legs:
    newPosition(xyz) = initial position(xyz) - body translation offsets(xyz) - body rotation offsets(xyz) + coxa rotation offsets(xyz)

    I'm still cleaning up my spreadsheet, but I hope to post it up tonight. It shows all my functions, equations, and plots the robot moving based on inputs.
    Last edited by TXBDan; 06-06-2013 at 11:59 AM.

  4. #4

    Re: Project BETH aka Dan's journey into a lot of things.

    Great work.

    FYI - There is another great spreadsheet for doing the IK stuff that was done by the member Zenta(Kåre Halvorsen) called PEP. This is mentioned on several forum threads up on Lynxmotion. Also there was a project page for it (


  5. #5

    Re: Project BETH aka Dan's journey into a lot of things.

    Quote Originally Posted by KurtEck View Post
    Great work.

    FYI - There is another great spreadsheet for doing the IK stuff that was done by the member Zenta(Kåre Halvorsen) called PEP. This is mentioned on several forum threads up on Lynxmotion. Also there was a project page for it (

    FWIW, it complains when you try to load into Numbers (Mac spreadsheet) about a few formulas. I'm not at the point yet where I can tell if it's a serious issue, or if any other non Excel spreadsheets run it better or worse. Just thought I'd throw that out there for non Excel users.

  6. #6
    Join Date
    Apr 2013
    Boston, MA
    Rep Power

    Re: Project BETH aka Dan's journey into a lot of things.

    Today's update.

    First I posted up my IK spreadsheet. I made it in Open Office (ODS format) and also saved it as Excel '97 format. Both are included in the zip.

    I also posted up my syncwrite code. Elegant it is not, but hopefully its very clear for someone who wants to write one. The comments match the AX12 datasheet description of the packet info.

    More interestingly, my hex is taking steps! I have a simple tripodWalk function that allows BETH to walk straight forward at a speed proportional to the Commander joystick position.

  7. #7
    Join Date
    Apr 2013
    Boston, MA
    Rep Power

    Re: Project BETH aka Dan's journey into a lot of things.

    I updated my Commander to run Kurt's CommanderEx firmware. This gives me +/-127 range and smooths the transitions. I haven't looked into the code much, but i do noticed that things are smoother and less glitchy. Really noticeable in my body rotations/translations. Its great!

    BETH is mobile! She can now walk forward, backward, and strafe left/right. It's setup so both the leg movement speed and the stride length is proportional to the Commander input and it looks and works nicely. The speed response from the joystick feels pretty good.

    I basically set it up so given a desired speed (from Commander), the time duration of the step is calculated, then the number of 30hz "ticks" that are in that duration. I then divide the duration into four quarters which each have a unique parts of the stepping motion. For tripod leg set A:

    Quarter 1
    Z: step up
    X: move forward

    Quarter 2
    Z: step down
    X: move forward

    Quarter 3
    Z: 0
    X: move backward

    Quarter 4
    Z: 0
    X: move backward

    Tripod leg set B is shifted two quarters down. Works the same for lateral strafing with the Y direction as well.

    The tediousness of this is fine for a tripod gait, but i think i might need something a little more elegant for the ripple gaits. I also need a way to smooth the transition from standing still to tripod stepping and tripod stepping to standing still. Right now the legs just snap to "home" when the sequence is over.

  8. #8

    Re: Project BETH aka Dan's journey into a lot of things.

    What I do is different.
    First, I drive the legs with IK.
    Second, I calculate the trajectory of the leg based on its position through the gait cycle.
    For a ripple gait, I say that the leg is on the ground, moving from front to back, for 75% (270 degrees) of the cycle, and off the ground, moving back to front, for 25% of the cycle. While it's moving back-to-front, it's lifted by some height times a sine function set to be 0 at that start and end of the forward motion, and has apex at 50% of the forward motion.
    For plain fast walks, I do "trotting" with two crosswise feet moving in parallel, and the others being 180 degrees offset, with 50% down/up duty cycle.
    To control speed, I control the length of the motion -- how much the leg reaches forward/backward from a designated center point, along the trajectory. The frequency is always the same (although I have a separate adjustment for this.)

    Because the leg offset amount (distance) is what's controlled, the transition from walking to standing still is fairly smooth, as it simply shrinks the step stride length down to zero. I smooth the control changes to allow some amount of adjustment without snapping. Also, when the stride goes close to zero (< 10% of max stride) I scale down the amount of foot lifting in the cycle so it's 0% lift at 0% stride, so the bot doesn't "trot in place" while standing still.

    Turning and strafing is similar, except the trajectories of ground contacts are warped/bent/slanted before sending to IK. And doing it all at once is a "simple" matter of just blending the different desired inputs into where the contact point is supposed to be.
    Last edited by jwatte; 06-10-2013 at 02:10 PM.

  9. #9
    Join Date
    Apr 2013
    Boston, MA
    Rep Power

    Re: Project BETH aka Dan's journey into a lot of things.

    A sine wave is a good idea for step height. Right now my "side view" of a step looks like a triangle. It's linear so it processes quickly though. Hrmm a sin/cos for stride and height would make a nice semicircle step and it'd be easy to scale down to zero for stopping. I'd like to try parabolic lift velocities some day so that it steps more gently. These things really pound the ground when they walk.

    I was assuming the ripple gait stepping of a single leg on a hexapod is 1/6th duty cycle. Is that not true?

  10. #10

    Re: Project BETH aka Dan's journey into a lot of things.

    Yes, for a hex, ripple gait uses 60 degrees phase difference -- mine is a quad.

Thread Information

Users Browsing this Thread

There are currently 1 users browsing this thread. (0 members and 1 guests)

Similar Threads

  1. Making Things Controller
    By LinuxGuy in forum Arbotix, Microcontrollers, Arduino
    Replies: 29
    Last Post: 08-06-2008, 05:47 PM

Tags for this Thread

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts