#include <ode/ode.h>

#include <drawstuff/drawstuff.h>



// select correct drawing functions

#ifdef dDOUBLE

#define dsDrawBox dsDrawBoxD

#define dsDrawSphere dsDrawSphereD

#define dsDrawCylinder dsDrawCylinderD

#define dsDrawCapsule dsDrawCapsuleD

#endif



static dWorldID world;

static dSpaceID space;

static dJointGroupID contactgroup;

static dGeomID ground;

static dBodyID body_up,body_low;

static dGeomID box_up,box_low;

static dJointID joint_up2low;

static dJointID joint_up2env;

static dSpaceID geom_group;



// box location, size, mass

double box_up_loc[3]   = {0  , 0  , 1  };

double box_low_loc[3]  = {0.4, 0  , 1  };

double box_up_size[3]  = {0.4, 0.1, 0.1};

double box_low_size[3] = {0.4, 0.1, 0.1};

double box_up_mass=1.0, box_low_mass=1.0;



// joint location

double joint_up2low_x, joint_up2low_y, joint_up2low_z, 

  joint_up2env_x, joint_up2env_y, joint_up2env_z;



#define DT 0.001

#define PI 3.14159



// start simulation - set viewpoint

static void start()

{

  static float xyz[3] = {0.8317f,-0.9817f,0.8000f};

  static float hpr[3] = {136.5000f,0.5000f,0.0000f};

  dsSetViewpoint (xyz,hpr);

  printf ("start simulation\n");

}



// control

double theta1, theta2;

//double omega1, omega2;

int controler_cnt=0;

void controler(){

  controler_cnt++;

  //--- get angular

  theta1 = dJointGetHingeAngle (joint_up2env);

  theta2 = dJointGetHingeAngle (joint_up2low);

  //omega1 = dJointGetHingeAngleRate (joint_up2env);

  //omega2 = dJointGetHingeAngleRate (joint_up2low);

  

  //--- control

  // velocity contorl

  double v;

  v = ( (2*PI * sin( 2*PI*(controler_cnt * DT) ))

	- PI/2 - theta1 );

  dJointSetHingeParam (joint_up2env,dParamVel,v);

  dJointSetHingeParam (joint_up2env,dParamFMax,20000);

  // torque control

  //dJointAddHingeTorque(joint_up2env,1);

}



// simulation loop

double t=0;

static void simLoop (int pause)

{

  int i,j;

  if (!pause) {

    dWorldStep (world,DT);

    // remove all contact joints

    dJointGroupEmpty (contactgroup);

    // control

    controler();

    // print real time

    //if( t > 1 && fmod(t,1) < DT ){ printf("%f sec\n",t); }

    t += DT;

  }



  dsSetTexture (DS_WOOD);

  dReal sides[3];

 

  dsSetColor (1,0,0);

  for(i=0; i<3; i++){ sides[i]=box_up_size[i]; }

  dsDrawBox( dBodyGetPosition(body_up),

	     dBodyGetRotation(body_up),

	     sides);

  dsSetColor (0,1,0);

  for(i=0; i<3; i++){ sides[i]=box_low_size[i]; }

  dsDrawBox( dBodyGetPosition(body_low),

	     dBodyGetRotation(body_low),

	     sides);

}



int main (int argc, char **argv)

{

  dMass m;



  //--- setup pointers to drawstuff callback functions

  dsFunctions fn;

  fn.version = DS_VERSION;

  fn.start = &start;

  fn.step = &simLoop;

  fn.stop = 0;

  fn.path_to_textures = "../simulator/ode-0.9/drawstuff/textures";

  if(argc==2){ fn.path_to_textures = argv[1]; }



  //--- create world

  world = dWorldCreate();

  space = dHashSpaceCreate (0);

  contactgroup = dJointGroupCreate (0);

  dWorldSetGravity (world,0,0,-9.8);

  ground = dCreatePlane (space,0,0,1,0);



  //--- create pendulum arm

  body_up  = dBodyCreate(world);

  body_low = dBodyCreate(world);

  dBodySetPosition( body_up,

		    box_up_loc[0], box_up_loc[1], box_up_loc[2]);

  dBodySetPosition( body_low,

		    box_low_loc[0], box_low_loc[1], box_low_loc[2]);

  dMassSetBox(&m,1,box_up_size[0], box_up_size[1], box_up_size[2] );

  dMassSetBox(&m,1,box_low_size[0], box_low_size[1], box_low_size[2] );

  dMassAdjust(&m,box_up_mass);

  dMassAdjust(&m,box_low_mass);

  dBodySetMass(body_up,&m);

  dBodySetMass(body_low,&m);

  box_up  = dCreateBox(0,box_up_size[0], box_up_size[1], box_up_size[2]);

  box_low = dCreateBox(0,box_low_size[0], box_low_size[1], box_low_size[2]);

  dGeomSetBody ( box_up, body_up );

  dGeomSetBody ( box_low, body_low );



  //--- create joint

  dVector3 a;

  dBodyGetRelPointPos(body_up,box_up_size[0]/2,0,0,a);

  joint_up2low_x=a[0];

  joint_up2low_y=a[1];

  joint_up2low_z=a[2];

  dBodyGetRelPointPos(body_up,-box_up_size[0]/2,0,0,a);

  joint_up2env_x=a[0];

  joint_up2env_y=a[1];

  joint_up2env_z=a[2];



  //--- joint up to low

  joint_up2low = dJointCreateHinge (world,0);

  dJointAttach (joint_up2low,body_up,body_low);

  dJointSetHingeAnchor (joint_up2low,

			joint_up2low_x,joint_up2low_y,joint_up2low_z);

  dJointSetHingeAxis ( joint_up2low,0,1,0);

  dJointSetHingeParam (joint_up2low,dParamLoStop,-dInfinity);

  dJointSetHingeParam (joint_up2low,dParamHiStop,dInfinity);

  dJointSetHingeParam (joint_up2low,dParamFudgeFactor,0.1);

  //--- joint up to evnvironment

  joint_up2env = dJointCreateHinge (world,0);

  dJointAttach (joint_up2env, body_up,0);

  dJointSetHingeAnchor (joint_up2env,

			joint_up2env_x,joint_up2env_y,joint_up2env_z);

  dJointSetHingeAxis ( joint_up2env,0,1,0);

  dJointSetHingeParam (joint_up2env,dParamLoStop,-dInfinity);

  dJointSetHingeParam (joint_up2env,dParamHiStop,dInfinity);

  dJointSetHingeParam (joint_up2env,dParamFudgeFactor,0.1);



  //--- add pendulum to world

  geom_group = dSimpleSpaceCreate (space);

  dSpaceSetCleanup (geom_group,0);

  dSpaceAdd(geom_group,box_up);

  dSpaceAdd(geom_group,box_low);

		

  //--- run simulation

  dsSimulationLoop (argc,argv,640,480,&fn);



  //--- destroy

  dGeomDestroy(box_up);

  dGeomDestroy(box_low);

  dJointGroupDestroy (contactgroup);

  dSpaceDestroy (space);

  dWorldDestroy (world);



  return 0;

}

Ref: 
http://www.koj-m.sakura.ne.jp/ode/index.php?%A5%C7%A5%E2%A1%CA%B4%F0%CB%DC%CA%D4%A1%CB

2 armed pendulum control start kit using Open Dynamics Engine

C++