[ODE] Having some hinge problems

Rob Leclerc leclerc at cpsc.ucalgary.ca
Fri Apr 11 16:41:01 2003
Previous message: [ODE] Servo to reach a target position
Next message: [ODE] Having some hinge problems
Messages sorted by: [ date ] [ thread ] [ subject ] [ author ]
I am porting some of my code over from vortex but I am having some
problems with the hinge joint. Basically, I am creating a 4 legged
creature and I need to get the legs into place. I have some code that, I
thought, should have worked, but it appears that I have to apply so much
force to the hinges to get them into place, that it starts to get
unstable. My next thought was to create the creature at some height and
then drop it with the hopes that I could get the legs in place with a
smaller force during the fall -- this had some rather unexpected results
as the legs kind of fall off(?). 

Basically I want to create a 4 legged creature but I want to play with
the ranges of freedom so I don't want to line up rotated legs to the
body in a specific way every time.

Anyone have any ideas why the legs are falling off or how I might get
around this?

/** 
*I have attached the code (will compile & run) that shows the legs
falling        *off. You can change the params in #define that specify
start height and *motor strength & speed.
*/

#include <stdio.h>
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"
#include <iostream.h>

#ifdef MSVC
#pragma warning(disable:4244 4305)  // for VC++, no precision loss
complaints
#endif

/* select correct drawing functions */

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif

#define MASS (1.0)		/* mass of a box */


#define DEFAULT_HI_LIMIT M_PI/2
#define DEFAULT_LO_LIMIT -M_PI/2

#define NUM_ACTIVE_JOINTS 5
#define NUM_BODY_PARTS (6)
#define SPACE_1 0.1
#define SMALL_SPACE 0.001
#define POS_PARAMS 3
#define ROTATE_PARAMS 3
#define GEOM_PARAMS 2
#define START_HEIGHT 4              //Change this to start lower
#define LEG_POSITION 0.7

#define MAX_MOTOR_VEL 1000
#define MAX_MOTOR_STR 5

double cylinderHeightBody = (double)2.0;
double cylinderHeightLeg = (double)1.0;
double cylinderRadius    = (double)0.1;


/***
 *   NOTE: Axis goes: XZY not XYZ. Not XYZ 
*/

//Used to set orientation and position
#define NUM_PARAMS_IN_BODY_PARTS (GEOM_PARAMS + POS_PARAMS +
ROTATE_PARAMS) 
float bodyPartInfo[NUM_BODY_PARTS][NUM_PARAMS_IN_BODY_PARTS] = {
    {cylinderHeightBody, cylinderRadius, (SPACE_1 +
(cylinderHeightBody/2)), 0, cylinderRadius+START_HEIGHT, 0, 0, 0},
    {cylinderHeightBody, cylinderRadius, -(SPACE_1 +
(cylinderHeightBody/2)), 0, cylinderRadius+START_HEIGHT, 0, 0, 0},
    {cylinderHeightLeg, cylinderRadius, (SPACE_1 + LEG_POSITION),
-(cylinderRadius + SMALL_SPACE + (cylinderHeightLeg/2)),
cylinderRadius+START_HEIGHT, 0, 0, M_PI/2},
    {cylinderHeightLeg, cylinderRadius, (SPACE_1 + LEG_POSITION),
(cylinderRadius + SMALL_SPACE + (cylinderHeightLeg/2)),
cylinderRadius+START_HEIGHT, 0, 0, M_PI/2},
    {cylinderHeightLeg, cylinderRadius, -(SPACE_1 + LEG_POSITION),
-(cylinderRadius + SMALL_SPACE + (cylinderHeightLeg/2)),
cylinderRadius+START_HEIGHT, 0, 0, M_PI/2},
    {cylinderHeightLeg, cylinderRadius, -(SPACE_1 + LEG_POSITION),
(cylinderRadius + SMALL_SPACE + (cylinderHeightLeg/2)),
cylinderRadius+START_HEIGHT, 0, 0, M_PI/2},
};

//Set up axis
float axis[NUM_ACTIVE_JOINTS][3] = {
        0, 0, 1,
        1, 0, 0,
        1, 0, 0,
        1, 0, 0,
        1, 0, 0,
};

/*
 * These are the *desired* stop limits for any the various joints
 */
double jointInfo[NUM_ACTIVE_JOINTS][2] = {
    {M_PI/4, -M_PI/4},
    {-M_PI/3, -M_PI/2},
    {M_PI/3, M_PI/2},
    {-M_PI/3, -M_PI/2},
    {M_PI/3, M_PI/2}
};


/* dynamics and collision objects */
static dWorldID world;
static dSpaceID space;
static dBodyID body[NUM_BODY_PARTS];
static dJointID joint[NUM_ACTIVE_JOINTS];
static dJointGroupID contactgroup;
static dGeomID geom[NUM_BODY_PARTS];

//Used to init position
static bool isEverythingInPosition = false;


static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
    int i;
    // exit without doing anything if the two bodies are connected by a
joint
    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);
    if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) 
    {   return;
    }

    dContact contact[4];			// up to 4 contacts per
box-box
    for (i=0; i<4; i++) {
        contact[i].surface.mode = dContactBounce | dContactSoftCFM;
        contact[i].surface.mu = dInfinity;
        contact[i].surface.mu2 = 0;
        contact[i].surface.bounce = 0.1;
        contact[i].surface.bounce_vel = 0.1;
        contact[i].surface.soft_cfm = 0.01;
    }
    if (int numc = dCollide (o1,o2,4,&contact[0].geom,sizeof(dContact)))
{
        dMatrix3 RI;
        dRSetIdentity (RI);
        const dReal ss[3] = {0.02,0.02,0.02};
        for (i=0; i<numc; i++) {
            dJointID c = dJointCreateContact
(world,contactgroup,contact+i);
            dJointAttach (c,b1,b2);
        }
    }
}


/* start simulation - set viewpoint */
static void start()
{
  static float xyz[3] = {5.1640f,-10.3079f,3.7600f};
  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}


static void initPosition()
{   
     bool areAllInRange = true;
     //Check if outside of desired position
     for(int i = 0; i < NUM_ACTIVE_JOINTS; i++)
     {  if( dJointGetHingeAngle(joint[i]) > jointInfo[i][0] ||
dJointGetHingeAngle(joint[i]) < jointInfo[i][1])
        {   areAllInRange = false; 
            break;
        }
     }
     //If in desired position, set new HI an LO stops and now we can
start
     if(areAllInRange)
     {   isEverythingInPosition = true;
         for(i = 0; i < NUM_ACTIVE_JOINTS; i++)
         {      dJointSetHingeParam
(joint[i],dParamLoStop,jointInfo[i][1]);
                dJointSetHingeParam
(joint[i],dParamHiStop,jointInfo[i][0]);
         }
         return;
     }
     //Else, keep applying forces until we get within our range.
     for(i = 0; i < NUM_ACTIVE_JOINTS; i++)
     {   dJointSetHingeParam (joint[i],dParamFMax, MAX_MOTOR_STR);
         double relPosition = dJointGetHingeAngle(joint[i]);
         double diff = (2*relPosition) - ((jointInfo[i][0] +
jointInfo[i][0]) * 0.5);
         dJointSetHingeParam (joint[i],dParamVel, -diff *
MAX_MOTOR_VEL);
         dJointSetHingeParam (joint[i],dParamFudgeFactor,0.005);
     }
}

/* simulation loop */
static void simLoop (int pause)
{
  int i;

    if (pause) {
     if(!isEverythingInPosition)
     {  initPosition();
     }
     else
        cout << "In Position!" << endl;
     dSpaceCollide (space,0,&nearCallback);
     dWorldStep (world,0.05);

     /* remove all contact joints */
     dJointGroupEmpty (contactgroup);
  }
  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (i=0; i<NUM_BODY_PARTS; i++){
      dReal sides[3] = {bodyPartInfo[i][0], bodyPartInfo[i][1],
bodyPartInfo[i][1]};
 
dsDrawBox(dBodyGetPosition(body[i]),dBodyGetRotation(body[i]),sides);

  }
}


int main (int argc, char **argv)
{
  int i;
  dReal k;
  dMass m;

  /* setup pointers to drawstuff callback functions */
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = 0;
  fn.stop = 0;
  fn.path_to_textures = "../../drawstuff/textures";

  /* create world */

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (1000000);
  dWorldSetGravity (world,0,0,-0.5);
  dCreatePlane (space,0,0,1,0);

  //Init bodies
  for(i = 0; i < NUM_BODY_PARTS; i++)
  {     //Create Body
        body[i] = dBodyCreate (world);
        //Set Position
        dBodySetPosition (body[i],bodyPartInfo[i][2],
bodyPartInfo[i][3], bodyPartInfo[i][4]);
        //Set its mass and geom
        dMassSetBox(&m,5, bodyPartInfo[i][0], bodyPartInfo[i][1],
bodyPartInfo[i][1]);
        dMassAdjust (&m,MASS);
        dBodySetMass (body[i],&m);
        geom[i] = dCreateBox(space, bodyPartInfo[i][0],
bodyPartInfo[i][1], bodyPartInfo[i][1]);
        dGeomSetBody (geom[i],body[i]);
        //Rotate legs into place
        if(i >= 2)
        {   dMatrix3 R;
            dRFromAxisAndAngle(R,0,0,1,bodyPartInfo[i][7]);
            dGeomSetRotation (geom[i],R);
        }  
  }
  
  //Init joints and attach them to the bodies
  for (i=0; i<(NUM_ACTIVE_JOINTS); i++) {
    joint[i] = dJointCreateHinge(world,0);
        dJointSetHingeParam (joint[i],dParamFMax,1);
        dJointSetHingeParam (joint[i],dParamLoStop, DEFAULT_LO_LIMIT);
        dJointSetHingeParam (joint[i],dParamHiStop, DEFAULT_HI_LIMIT);
  }
   
    //Two Main segments making the length of the body
   dJointAttach (joint[0],body[0],body[1]);   
   dJointSetHingeAxis(joint[0],axis[0][0],axis[0][1],axis[0][2]);
   dJointSetHingeAnchor(joint[0],axis[0][0],axis[0][1],axis[0][2]);

    //Front Legs
   dJointAttach (joint[1],body[0],body[2]);   
   dJointSetHingeAxis(joint[1],axis[1][0], axis[1][1], axis[1][2]);
   dJointSetHingeAnchor(joint[1],axis[1][0], axis[1][1], axis[1][2]);
  
   dJointAttach (joint[2],body[0],body[3]);   
   dJointSetHingeAxis(joint[2],axis[2][0], axis[2][1], axis[2][2]);
   dJointSetHingeAnchor(joint[2],axis[2][0], axis[2][1], axis[2][2]);

    //Back Legs
   dJointAttach (joint[3],body[1],body[4]);   
   dJointSetHingeAxis(joint[3],axis[3][0], axis[3][1], axis[3][2]);
   dJointSetHingeAnchor(joint[3],axis[3][0], axis[3][1], axis[3][2]);
  
   dJointAttach (joint[4],body[1],body[5]);   
   dJointSetHingeAxis(joint[4],axis[4][0], axis[4][1], axis[4][2]);
   dJointSetHingeAnchor(joint[4],axis[4][0], axis[4][1], axis[4][2]);


   /* run simulation */
  dsSimulationLoop (argc,argv,2*352,2*288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}





==============================
Rob Leclerc
University of Calgary
Department of Computer Science
Office: ICT 728
www.cpsc.ucalgary.ca/~leclerc
==============================
Previous message: [ODE] Servo to reach a target position
Next message: [ODE] Having some hinge problems
Messages sorted by: [ date ] [ thread ] [ subject ] [ author ]