[ODE] Nice Motion Control
Manohar B.S
sciphilog at yahoo.com
Tue Jul 20 22:09:25 MST 2004
Dear Friends,
PAST: Few days back there was a discussion on how to
control the 'ODE' body position and orientation using
external commands, that is with out directly using
dBodySetPosition or dGeomSetPosition. There was also
discussion on using Springs and Fuzzy springs to pull
the bodies around.
PRESENT: Here's is method that I found working fine.
This might have been done by others, but just posting
it in case..
Only Position Control:
-------------------------------------
Include the "Joint.h" in the program
Say dBox BOX is the object whose position is the
controlled.
Create a joint dBallJoint bJoint;
.....
bJoint.attach(BOX, 0);// attach it to static env.
// Function to set the position
SetMyODEBodyPosition(dVector3 pos[3] )
{
dxJointBall *pJB = (dxJointBall*)bJoint.id();
for(int i=0;i<3;i++)
pJB->anchor2[i] = pos[i];
}////////////// That's all.
if you are using Universal Joint, then this way we can
even change the axis2 parameters to control the
rotation also..
pJB->axis2[...]
//// In case if both the position and orientation
needs to be controlled use FixedJoints this way:
SetMyODEBodyPosition(dVector3 pos[3] )
{
dxJointFixed *pFJ = (dxJointFixed*)bJoint.id();
pFJ->offset[0] += x;
pFJ->offset[1] += y;
pFJ->offset[2] += z;
dMatrix3 m3;
dRFromEulerAngles(m3, EuAng[0], EuAng[1], EuAng[2]);
dQfromR(pFJ->qrel, m3);
}////////////// That's all.
Thanks to ODE it is OpenSource, other wise there was
no chance that I could peep into and get the right
thing I want.
For people who want to get ForceFeedBack use
dJointSetFeedback(jFix.id(), &JFB); and send this to
the device......
It works believe me. I have tested it.
Here's a simple implementation attached.
__________________________________
Do you Yahoo!?
Vote for the stars of Yahoo!'s next ad campaign!
http://advision.webevents.yahoo.com/yahoo/votelifeengine/
-------------- next part --------------
#include <ode\ode.h>
#include <drawstuff/drawstuff.h>
#include <joint.h>
#include <math.h>
#pragma comment (lib, "C:\\ode-0.5\\lib\\ode.lib")
#pragma comment (lib, "C:\\ode-0.5\\lib\\opcode.lib")
#pragma comment (lib, "C:\\ode-0.5\\lib\\drawstuff.lib")
#ifdef _MSC_VER
#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
// some constants
#define NUM 40 // number of boxes
#define SIDE (0.2) // side length of a box
#define MASS (1.0) // mass of a box
#define RADIUS (0.1732f) // sphere radius
// dynamics and collision objects
static dWorld world;
static dSimpleSpace space (0);
static dJointGroup contactgroup;
static dJointGroup JointSet;
// Tool Body.
static dBody bRod;
static dCCylinder Rod;
static dUniversalJoint jToolHold;
static dBody bHold;
static dFixedJoint jFix;
static dJointFeedback JFB;
float theta = 0.0;
dsFunctions fn;
// this is called by space.collide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
// @@@ it's still more convenient to use the C interface here.
dContact contact;
contact.surface.mode = dContactBounce;
contact.surface.bounce = 0.1;
contact.surface.bounce_vel = 0.3;
contact.surface.mu = dInfinity;
if (dCollide (o1,o2,0,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact);
dJointAttach (c,b1,b2);
}
}
// start simulation - set viewpoint
static void start()
{
static float xyz[3] = {0.0, -4.0, 2.0};
static float hpr[3] = {90.000f, -25.0000f, 0.0000f};
dsSetViewpoint (xyz,hpr);
}
// simulation loop
static void simLoop (int pause)
{
if (!pause) {
space.collide (0,&nearCallback);
world.step (0.05);
contactgroup.empty();
}
{
// The Tool.
dReal sides[3] = {0.10,0.10,2.0};
dsSetColor (1,1,0); dsSetTexture (DS_WOOD);
dsDrawBox (Rod.getPosition(),Rod.getRotation(),sides);
}
{
// The Hold.
dReal sides[3] = {0.2, 0.2, 0.2};
dsSetColor (0,1,0); dsSetTexture (DS_WOOD);
dsDrawBox (Hold.getPosition(), Hold.getRotation(), sides);
}
}
int inc = 1;
float EuAng[3] = {0.0,0.0,0.0};
static void command (int cmd)
{
dxJointFixed *pFJ = (dxJointFixed*)jFix.id();
switch (cmd)
{
case '1':
dBodyAddForceAtRelPos(Rod.getBody(), 100.0, 0.0, 0.0, 0.0, -1.0, 0.0);
break;
case '2':
dBodyAddForceAtRelPos(Rod.getBody(), 0.0, 100.0, 0.0, 0.0, -1.0, 0.0);
break;
case '3':
dBodyAddForceAtRelPos(Rod.getBody(), 0.0, 0.0, 100.0, 0.0, -1.0, 0.0);
break;
// Positions
case 'x':
pFJ->offset[0] += inc * 0.05;
break;
case 'y':
pFJ->offset[1] += inc * 0.05;
break;
case 'z':
pFJ->offset[2] += inc * 0.05;
break;
// Angles..
case 'X':
EuAng[0] += inc * 0.05;
break;
case 'Y':
EuAng[1] += inc * 0.05;
break;
case 'Z':
EuAng[2] += inc * 0.05;
break;
case 'i':
case 'I': inc *= -1;
break;
}
dMatrix3 m3;
dRFromEulerAngles(m3, EuAng[0], EuAng[1], EuAng[2]);
dQfromR(pFJ->qrel, m3);
printf("\nJFB: %f %f %f",pFJ->qrel[0],pFJ->qrel[1],pFJ->qrel[2]);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = command;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
// create world
int i;
contactgroup.create (0);
world.setGravity (0,0,-0.9);
dWorldSetCFM (world.id(),1e-2);
dPlane plane (space,0,0,1,0);
// Create Rod.
bRod.create(world);
dMass m; m.setSphere(1.0,1.0);
bRod.setMass(&m);
Rod.create(space, 0.1, 1.8);
Rod.setBody(bRod);
Rod.setPosition(0.0,0.0,2.0);
JointSet.create(1);
jFix.create(world, JointSet.id());
jFix.attach(Rod.getBody(), 0);
jFix.set();
dJointSetFeedback(jFix.id(), &JFB);
//
// run simulation
dsSimulationLoop (argc,argv,800,600,&fn);
return 0;
}
More information about the ODE
mailing list