View Full Version : Calculating frame rate and function time

05-19-2013, 04:55 PM
Hello all,

I'm working on trying to create my own hexapod program using the MarkII AX12 Hexapod hardware kit. I'm using the Commander libraries for external control input and the AX12 libraries to handle the servos.

I want the program to run at a certain frame rate. For each frame, I'm collecting command input, calculating the gait, running IK for the body and each leg, and updating the servo positions.

So far I have an IK function that works great for a single leg. I derived it using the multiplication of rotation matrices method from a textbook I have. The equations look a little gnarly and I wonder how much time it takes to crunch through. What is the best way for me to time this so that i can plan for my program's "frame rate".

void legIK(int leg, int X, int Y, int Z){

float tib1, tib2, fem1, fem2;

tib1 = (float)(sq(Y-LENGTH_COXA) + sq(X) + sq(Z)- sq( LENGTH_FEMUR) - sq(LENGTH_TIBIA) ) / (2*LENGTH_FEMUR*LENGTH_TIBIA);
tib2 = atan2( sqrt(1 - sq(tib1)) , tib1 );
fem1 = -atan2( Z , sqrt( sq(Y-LENGTH_COXA) + sq(X) ));
fem2 = atan2( LENGTH_TIBIA*sin(tib2) , LENGTH_FEMUR+LENGTH_TIBIA*cos(tib2) );

jointAngles[leg].femur = fem1 - fem2 + 1.571 - 0.237; // 90deg correction for my angular space - physical femur angle due to offset tibia servo mounting location
jointAngles[leg].coxa = asin( X / sqrt( sq(Y) + sq(X) )) + 1.571; // 90deg correction
jointAngles[leg].tibia = tib2 + 1.571 - 0.850 + 0.237; // 90deg correction - the tibia physical 60deg angle between servo mounting and foot + correction of femur offset


It looks like the stock NUKEcode from Trossen uses a lot of poses using the Bioloid libraries. Is this going to be too processor heavy for the Arbotix?

Any input or advice would be much appreciated!

05-19-2013, 06:32 PM
As I mentioned in a different thread today. What I do, is to find an unused pin and hook up a logic analyzer. So in a function like this, suppose I decide to use pin A1.
I would put a digitalWrite(A1, HIGH) at the start and (A1, LOW) at the end of the function (or simply around the call to legIK). Then simply look at it with the LA. Alternatively you can use built in timers to keep track and print out values later on...

What I have found with the Bioloid library and the like, is where you get hit is by the overhead of doing all of the serial IO. especially if you don't trust the servos to do the interpolation, so you have to keep pooling and doing your own interpolation to output the next time period. I tried to offload the work to the servos and did not have any luck, but I believe Kevin has done a better job of it.

Note with the Phoenix code base, especially with servos we don't have to babysit, we actually spend a reasonable time waiting for the servos to complete their move before we issue the next set. However I understand that if we could work at a fast enough frame rate we could break up each of the moves of the gait into smaller incremental sub moves. It is my understanding that doing this could make the moves even more precise and I know Nathan (Orion Robotics) is doing that with their robots.


05-19-2013, 06:56 PM
I deleted my previous post since Kurt's was much better. Kurt is right I only send poses on position change, one step has ten positions from start to finish. I walk rather slow so the camera vision is clean which means it could be a couple of seconds between new positions being transmitted. If it is standing still the gait class isn't even called which frees up the few cycles for other things like openCV stuff.

05-19-2013, 08:56 PM
Of course, the millis() function. Timing all my code for one leg's IK and servo processing, it takes 17ms. yikes. So over 100ms for six legs. Is 10hz a fast enough frame rate?... I might have to redo my IK trig style, but it was fun to learn this method.

05-19-2013, 09:08 PM
Well what you have above is trig. :p

05-19-2013, 09:28 PM
Just a heads up I switched the Phoenix code over to float point and it turned out very sluggish. It did work though not as nice as the fixed point version. Though with that said, isn't the stock code Trossen provides all float point?

05-21-2013, 09:43 PM
I have my new IK running the full body and all six legs in 13ms. Much better!

I'm currently running a 33ms frame rate.

How do i figure out how low i can go? What is the next weakest link?

1000000Mbps / (64 bits in a packet) / (18 servos) = 868 updates in a second or once every ~1ms. double that for some buffer, every 2ms? That seems way too fast, right? What am i missing here? What else do i need to consider?

05-22-2013, 08:50 AM
Here's the datasheet info i've been looking for:

I kept searching and finding some simplified datasheet on Robotis' website.

05-22-2013, 11:40 AM
I'm not sure I follow your question. Probably the fastest you can update is 20Hz and notice a difference. Which is I believe is how fast the Phoenix software updates the positions. The standard code from Arbotix is 30Hz which is around 33ms frame length. Also you don't have to updatethat often with dynamixels. I only update their position every other second depending on how fast I'm walking. Unless of course I need to change their direction immediately. Though you need to calculate their speed correctly.

05-22-2013, 12:47 PM
By frame i mean a set of fixed positions. So moving through the frames in time creates motion like a cartoon animation. I'm now to the point where i need to start moving my feet to a "clock" so that i can step and walk. I was trying to get it as low as possible, but it sounds like the 33ms frame interval i'm running now should work fine. So every 33ms every servo will move slightly to its next position in its journey to doing something meaningful like walking. I'm using the normal AX12 WRITE command (not SYNC WRITE), but it sounds like i should still be fine on the time it takes to get all the commands out with the 1Mbps serial. I'm not reading anything back.

05-22-2013, 01:25 PM
I work at DreamWorks Animation so I understand what a frame is. :P Why not a sync write? They are simple to write up and both the code bases use them. Granted I don't use either one anymore since moving to the raspberry pi. I guess I'm struggling with the question since you are basically re-writing what is already available and proven to work. Unless of course this is merely a mental exercise for you and you just don't want to look. :)

05-22-2013, 01:50 PM
hah I know you and most everyone on this forum knows what a frame is. I'm just trying to be clear for future reference.

Yes, i'm rewriting most of this from scratch. Just using the AX12 and Commander libs. I feel guilty calling it "my" robot if i don't understand all that's going on inside. The best way to learn it is to write it! So far my leg and body IK is working great. I can do body rotations and translations while standing still. Next step is the gait sequencing.

Btw, i have a friend at Dream Works. Know a Deven P.? I think he's a software guy who works on particle stuff like clouds, dust, etc.

05-22-2013, 02:23 PM
Devon P***** up in Redwood City? Effects Animator? If so I know who he is. I'm Head of Character Technical Direction so we don't have much cross over in our jobs but after eleven years here you inadvertently hear peoples names and remember them. :)

05-22-2013, 03:05 PM
Yep, that's him. He grew up with my fiancee and her brother and his family and theirs and close. We were all skiing in Utah a few months ago. It sounds(from him)/looks(from movies) like you are up to some really cool stuff over there, nice work!