[ODE] Having some hinge problems
David Whittaker
david at csworkbench.com
Sat Apr 12 00:23:02 2003
Hey, you used the axis as the anchor point... That's A Bad Thing
(usually). All your legs are rotating around (1,0,0) to start with. If
you look at a hinge on a door, it has a little pin that holds it together;
anchors in ODE would relate to the center of that pin. Or the position of
the ball in a ball joint (in world coordinates). You give it an absolute
position to start with, and then the anchor point attempts to remain at
the same place relative to the bodies the joint is attached to (so the
bodies can still move around, and the anchor point follows them around).
See the fix: sections in the code below.
David
> 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,
> };
>
fix: //set up anchors
fix: float anchor[NUM_ACTIVE_JOINTS][3] =
fix: {
fix: {0, 0, cylinderRadius + START_HEIGHT},
fix: { (SPACE_1 + LEG_POSITION),
fix: -(cylinderRadius + (SMALL_SPACE / 2)),
fix: cylinderRadius + START_HEIGHT},
fix: { (SPACE_1 + LEG_POSITION),
fix: (cylinderRadius + (SMALL_SPACE / 2)),
fix: cylinderRadius + START_HEIGHT},
fix: {-(SPACE_1 + LEG_POSITION),
fix: -(cylinderRadius + (SMALL_SPACE / 2)),
fix: cylinderRadius + START_HEIGHT},
fix: {-(SPACE_1 + LEG_POSITION),
fix: (cylinderRadius + (SMALL_SPACE / 2)),
fix: cylinderRadius + START_HEIGHT}
fix: };
> /*
> * 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]);
fix: dJointSetHingeAnchor(joint[0],anchor[0][0],anchor[0][1],anchor[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]);
fix: dJointSetHingeAnchor(joint[1],anchor[1][0],anchor[1][1],anchor[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]);
fix: dJointSetHingeAnchor(joint[2],anchor[2][0],anchor[2][1],anchor[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]);
fix: dJointSetHingeAnchor(joint[3],anchor[3][0],anchor[3][1],anchor[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]);
fix: dJointSetHingeAnchor(joint[4],anchor[4][0],anchor[4][1],anchor[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
> ==============================
>
>
> _______________________________________________
> ODE mailing list
> ODE@q12.org
> http://q12.org/mailman/listinfo/ode