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

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

          }// 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:

/*************************************************
  tripodGaitSine()

**************************************************/
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);
            else
		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;
                    break;

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

          }// 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.

11 thoughts on “Phase 4: Foot Trajectory Planning and Gait Sequencing Part II

  1. Larry

    Where are you sending the move values to the servo for each servo on the leg? Is that the leg[legNum].footPos.* ? Just trying to understand your code!

    Thanks for all the code examples,
    -larry

    Reply
    1. admin Post author

      Yep, the leg positions are stored in leg[legNum].footPos.x/y/z which are global variables. Then in the ik.cpp, those values are used by the IK algorithm to calculate the required servo angles and then those angles are sent to the servos with the driveServos() function.

      Hope that helps!

      Reply
  2. Guilherme Alegre

    can explain to me what is the tick and the numticks?
    and already now better explain the reasoning that used on walk.

    Thanks
    Guilherme Alegre

    Reply
    1. admin Post author

      Sure. And sorry for the delay, I hope you see this! When i use the word “tick” i’m thinking of a tick from a clock as a way to count time. “tick” is the current tick in time. It’s the time counter. “numticks” is the total number of ticks in a step cycle.

      Reply
  3. Martin Turner

    Hi There.

    I am building a Hexapod right now and I am working on the code. I have got my bot to strafe left right and move forwards and backwards in a similar way to your code. I am using RC transmitter and receiver and decoding that with Arduino. I worked out that the gait should be rotated (using a rotational matrix) around the starting position of the foot based on the angle of the input from stick on the transmitter to get it to walk in any direction.

    However what i trying to get my head around now is walking and turning at the same time. I have sort of achieved a turn by shortening the left hand step length and increasing the right hand side or visa / versa. However this only works when the bot is walking forwards or backwards, and not when crabbing left or right.

    I also understand how to make the bot turn on the spot by rotating the gait (again using the matrix) in either a clockwise / anticlockwise direction around the foot starting points.

    From what i can see your code above may well allow for walking and turning at the same time. Any chance you can explain the logic / maths a bit more so i can understand it.

    Thanks. Your help would be much appreciated.

    Reply
    1. Martin Turner

      Think i just worked it out.

      I think i am right in saying that the gait should be rotated around a point at 90 degrees to the direction of travel which is located away from / outside of the hexapod. The closer the point to the center the faster the turn will be?

      Reply
      1. Dan Post author

        If you can translate forward/backward/left/right and rotate clockwise/counterclockwise, doing them both at the same time is literally adding the foot positions together. X = rotated X + translated X, Y = rotated Y + translated Y

        Do be careful that the feet won’t run into each other or go out of reach. You should have some bounds checking done on each foot step.

        Hope that helps and good luck!

        Reply
  4. Martin Turner

    Hey Dan…
    I’ve built a quadruped.
    https://www.youtube.com/watch?v=jWKsVjQuVpQ

    I’m trying to implement a 2 step gait which is similar to your’s using loop timing and a formula for calculating a semicircle curve rather than using delays which i have been doing up to now.

    I have managed to get that working, but what i really want to happen is that the foot decelerates when nearing the landing point so that the foot has a soft landing. Any ideas ? I have spent at least 2 days playing with log curves and S curves, but haven’t managed to come up with a simple solution that makes sense.

    Any Suggestions much appreciated.
    Cheers Martin

    Reply

Leave a Reply

Your email address will not be published. Required fields are marked *