How To Program Hubo

How To Program Hubo

Part 2: Simple Gestures


As seen in “Part 1: The Basics” the Hubo has a loop called TimerHandler() which allows the system to run in Hard Real-Time, i.e. with a fixed sampling rate.  This tutorial will show you the basics of making the original Hubo, KHR2, make arm gestures using this loop and other pre-made functions and methods.  The ultimate goal of this tutorial is to make Hubo’s head move up and down smoothly.


Simple Control:

The code below in Figure 2 shows the function that executes when you press the button Man1 (B), Figure 1.


Figure 1: Button to execute function in CGesturePage3::OnPage3Man1B()


The code in Figure 2 below defines the desired angle for each joint.  In the code below each joint is given the desired angle of 0o except for the head pitch.  The head pitch is given a desired angle as the result of the funciton 15cos(2πt/100).  Note that t is an integer where each integer value is 10ms.  The reason why the cos() function was chosen instead of the sin() function was because the initial value of cos() is the same as the home value of the head, 0o.  In the main for loop “for(t=0, t<T,t++)” the local integer t is incremented and desired value for that time is stored in the public variable in SharedMemory called UpBody_MTN_CAP_Data[t][n] where n is the joint number. 

The highlighted portion in Figure 2 is the portion of the code that sends the current value of 15cos(2πt/100) to the head pitch.




void CGesturePage3::OnPage3Man1B()



                int i,j,T, t;

                T = 1000;

                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->MTN_CAP_Time = T;

                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->bAction_Flag = TRUE;            //SA

                for(i=0 ; i < 17 ; i++)

                                for(j=0 ; j < 10000 ; j++)

                                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[j][i] = NULL;

                for(t=0 ; t < T ; t++)


                // 0: Torso yaw

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][0] = (float)0;

                // 1: Head yaw

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][1] = (float)0;

                // 2: Head pitch\                                                                                                                                                                                                                                                                 

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][2] = (float)((cos(2*3.1415926536*1*t/100))*15);

                // 3: LSP left shoulder pitch

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][3] = (float)0;

                // 4: LSY left shoulder yaw

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][4] = (float)0;

                // 5: LSR left shoulder rotate                                                                                                                                                                                                                                                            

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][5] = (float)0;

                // 6: LEB left elbow bend                                                                                                                                                                                                                                                   

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][6] = (float)0;

                // 7: LWR               left wrist rotate                                                                                                                                                                                                                                                   

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][7] = (float)0;

                // 8: LWP               left wrist pitch

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][8] = (float)0;

                // 9: RSP                right shoulder pitch

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][9] = (float)0;

                // 10: RSY              right shoulder yaw

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][10] = (float)0;

                // 11: RSR              right shoulder rotate

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][11] = (float)0;

                // 12: REB              right elbow bend

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][12] = (float)0;

                // 13: RWR            right wrist rotate

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][13] = (float)0;

                // 14: RWP            right wrist pitch

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][14] = (float)0;

                // 15: LRHP           left hip pitch

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][15] = (float)0;

                // 16: RRHP right hip pitch

                                ((CStarTrackApp*)AfxGetApp())->pSharedMemory->UpBody_MTN_CAP_Data[t][16] = 0;



                SetTimer(1, TIMER_MILLISECOND, NULL);



Figure 2: Code to execute when Man1 (B) button is pressed.  Moves head up and down by ±15o, centered at 0o, and at a frequency of 1Hz.


Please note that the code will only execute for up to T increments of t, i.e. T/100 sec.

After the desired angle for each joint is put into UpBody_MTN_CAP_Data[t][n] the function talked about in “Part 1: The Basics” called TimerHandler().  Within this code the proper message is sent to the CAN Bus to set each joint to the desired angle denoted by UpBody_MTN_CAP_Data[t][n].  Please note that the code inside of TimerHandler() is extremely long and messy for the KHR2 but this is NOT the same code that will be used on the New Hubo.

The final result for running this code can be found in the video below.