[ODE] controlling forces on sliding joints

[Nandan Joshi]

Hallo,
I'm trying to design a Robot arm, which functions like human arm, and muscles 
r added using slider joint. The main aim of the project is to control 
muscles, i.e. slider joints, with neural networks to assess the movement of 
the arm. But neural networks algorithms r not added yet, and simple working 
model is made, but I dunno exactly how to have control the forces working on 
slider joints, coze after applying force on one muscle or other, the whole 
motion looks uncanny. The simulation is stable, but I can't have control on 
sliding forces. I used the parameters for these joints, but still it didn't 
show any difference. I'm new to ODE, maybe someone can help me out defining 
the controls on the movement of the arm.

The programme is as follows:
#include <stdio.h>
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"

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

/* select correct drawing functions */

#ifdef dDOUBLE 
#define dsDrawBox dsDrawBoxD    
#endif


/* some constants */

#define SIDE (0.2)              /* side length of a box */
#define MASS (1.0)              /* mass of each body */


/* dynamics and collision objects */

static dWorldID world;
static dSpaceID space;
static dBodyID fixed_body[2], body[15];
dJointID hinge[6], slider[2];

// state set by keyboard commands
static int occasional_error = 0;


/* start simulation - set viewpoint */

static void start()
{
  static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
  printf("Press 'e' to start/stop occassional error.\n");
}

static void command (int cmd)
{
    if (cmd == 'e' || cmd == 'E')
    {
      occasional_error^=1;
    }
}

/* simulation loop */

static void simLoop (int pause)
{
//   const dReal kd = -0.3;	// angular damping constant
  const dReal ks = 0.5;	// spring constant
  if (!pause) {
    // add a spring force to keep the bodies together, otherwise they will
    // fly apart along the slider axis.
    const dReal *p1 = dBodyGetPosition (body[2]);
    const dReal *p2 = dBodyGetPosition (body[3]);
    dBodyAddForce (body[2],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
		   ks*(p2[2]-p1[2]));
    dBodyAddForce (body[3],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
		   ks*(p1[2]-p2[2]));
    
//     const dReal *p3 = dBodyGetPosition (body[4]);
//     const dReal *p4 = dBodyGetPosition (body[5]);
//     dBodyAddForce (body[4],ks*(p4[0]-p3[0]),ks*(p4[1]-p3[1]),
// 		   ks*(p4[2]-p3[2]));
//     dBodyAddForce (body[5],ks*(p3[0]-p4[0]),ks*(p3[1]-p4[1]),
// 		   ks*(p3[2]-p4[2]));

//     Force to the arm 1
//     dBodyAddForce (body[1], 0,0.5,0);

    // occasionally re-orient one of the bodies to create a deliberate error.
    if (occasional_error) {
      static int count = 0;
      if ((count % 20)==0) {
	// randomly adjust orientation of body[0]
	const dReal *R1;
	dMatrix3 R2,R3;
	R1 = dBodyGetRotation (body[2]);
	dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
			    dRandReal()-0.5,dRandReal()-0.5);
	dMultiply0 (R3,R1,R2,3,3,3);
	dBodySetRotation (body[2],R3);
	
	// randomly adjust position of body[0]
	const dReal *pos = dBodyGetPosition (body[2]);
	dBodySetPosition (body[2],
			  pos[0]+0.2*(dRandReal()-0.5),
			  pos[1]+0.2*(dRandReal()-0.5),
			  pos[2]+0.2*(dRandReal()-0.5));
      }
      count++;
    }
    dWorldStep (world,0.05);
  }
  dReal sides_fixed[3] = {SIDE,SIDE*1.7f,SIDE};
  dReal sides0[3] = {SIDE*0.2f,SIDE*0.2f,SIDE*3.0f};
  dReal sides1[3] = {SIDE*0.2f,SIDE*0.2f,SIDE*4.0f};
  dReal sides2[3] = {SIDE*0.2f,SIDE*0.2f,SIDE*2.0f};
  dReal sides3[3] = {SIDE*0.2f,SIDE*0.2f,SIDE*2.0f};
  dReal sides4[3] = {SIDE*0.2f,SIDE*0.2f,SIDE*2.0f};
  dReal sides5[3] = {SIDE*0.2f,SIDE*0.2f,SIDE*2.0f};
  dReal sides6[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  dReal sides7[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  dReal sides8[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  dReal sides9[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  dReal sides10[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  dReal sides11[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  dReal sides12[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  dReal sides13[3] = {SIDE*0.1f,SIDE*0.1f,SIDE*0.5f};
  
  dsSetTexture (DS_WOOD);
  dsSetColor (1,1,0);
  dsDrawBox 
(dBodyGetPosition(fixed_body[0]),dBodyGetRotation(fixed_body[0]),sides_fixed);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides0);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides1);
  dsSetColor (1,0,1);
  dsDrawBox (dBodyGetPosition(body[2]),dBodyGetRotation(body[2]),sides2);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[3]),dBodyGetRotation(body[3]),sides3);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[4]),dBodyGetRotation(body[4]),sides4);
  dsSetColor (1,0,1);
  dsDrawBox (dBodyGetPosition(body[5]),dBodyGetRotation(body[5]),sides5);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[6]),dBodyGetRotation(body[6]),sides6);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[7]),dBodyGetRotation(body[7]),sides7);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[8]),dBodyGetRotation(body[8]),sides8);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[9]),dBodyGetRotation(body[9]),sides9);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[10]),dBodyGetRotation(body[10]),sides10);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[11]),dBodyGetRotation(body[11]),sides11);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[12]),dBodyGetRotation(body[12]),sides12);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[13]),dBodyGetRotation(body[13]),sides13);
}

static void BodyCreate(int n, dMass m, dReal x, dReal y, dReal z, dReal qx, 
dReal qy, dReal qz, dReal qangle)
{
  body[n] = dBodyCreate (world);
  dBodySetMass (body[n],&m);
  dBodySetPosition (body[n],x,y,z);
  dQuaternion q[n];
  dQFromAxisAndAngle (q[n],qx,qy,qz,qangle);
  dBodySetQuaternion (body[n],q[n]);

}

int main (int argc, char **argv)
{
  /* setup pointers to drawstuff callback functions */
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "/home/nandan/ode/drawstuff/textures";

  /* create world */

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  dWorldSetERP (world,0.2);
  dWorldSetCFM (world,1e-6);
  dWorldSetGravity (world,0,0,0);
  dCreatePlane (space,0,0,1,0);

  dMass f_m;
  dMassSetBox (&f_m,1,SIDE,SIDE,SIDE);
  dMassAdjust (&f_m,MASS);

  fixed_body[0] = dBodyCreate (world);
  dBodySetMass (fixed_body[0],&f_m);
  dQuaternion q1;
  dQFromAxisAndAngle (q1,1,0,0,0);
  dBodySetPosition (fixed_body[0],0.25*SIDE,0.25*SIDE,5.2*SIDE);
  dBodySetQuaternion (fixed_body[0],q1);

  BodyCreate(0, f_m, 0.25*SIDE,0.25*SIDE,3.2*SIDE, 1,0,0,0);
  BodyCreate(1, f_m, 0.25*SIDE,1.25*SIDE,1.5*SIDE, 1,0,0,M_PI*0.5);
  BodyCreate(2, f_m, 0.25*SIDE,1.0*SIDE,2.7*SIDE, 1,0,0,0);
  BodyCreate(3, f_m, 0.25*SIDE,1.0*SIDE,3.7*SIDE, 1,0,0,0);
  BodyCreate(4, f_m, 0.25*SIDE,-0.5*SIDE,2.7*SIDE, 1,0,0,0);
  BodyCreate(5, f_m, 0.25*SIDE,-0.5*SIDE,3.7*SIDE, 1,0,0,0);
  BodyCreate(6, f_m, 0.25*SIDE,-0.25*SIDE,4.37*SIDE, 1,0,0,M_PI/4);
  BodyCreate(7, f_m, 0.25*SIDE,0.0*SIDE,4.1*SIDE, 1,0,0,M_PI/4);
  BodyCreate(8, f_m, 0.25*SIDE,0.75*SIDE,4.37*SIDE, 1,0,0,3*M_PI/4);
  BodyCreate(9, f_m, 0.25*SIDE,0.5*SIDE,4.1*SIDE, 1,0,0,3*M_PI/4);
  BodyCreate(10, f_m, 0.25*SIDE,-0.25*SIDE,2.00*SIDE, 1,0,0,3*M_PI/4);
  BodyCreate(11, f_m, 0.25*SIDE,0.0*SIDE,2.3*SIDE, 1,0,0,3*M_PI/4);
  BodyCreate(12, f_m, 0.25*SIDE,0.75*SIDE,2.00*SIDE, 1,0,0,M_PI/4);
  BodyCreate(13, f_m, 0.25*SIDE,0.5*SIDE,2.3*SIDE, 1,0,0,M_PI/4); 

  /* hinge joint between 2 main arms */
  hinge[0] = dJointCreateHinge (world,0);
  dJointAttach (hinge[0],body[0],body[1]);
  dJointSetHingeAnchor (hinge[0], 0.25*SIDE, 0.25*SIDE, 1.7*SIDE);
  dJointSetHingeAxis (hinge[0], 1, 0, 0);

  /* hinge joint between upper arm and fixed body */
  hinge[1] = dJointCreateHinge (world,0);
  dJointAttach (hinge[1],fixed_body[0],body[0]);
  dJointSetHingeAnchor (hinge[1], 0.25*SIDE, 0.25*SIDE, 4.7*SIDE);
  dJointSetHingeAxis (hinge[1], 1, 0, 0);

  /* hinge joint between muscle 3 and fixed body */
  hinge[2] = dJointCreateHinge (world,0);
  dJointAttach (hinge[2],fixed_body[0],body[3]);
  dJointSetHingeAnchor (hinge[2], 0.25*SIDE, 1.0*SIDE, 4.7*SIDE);
  dJointSetHingeAxis (hinge[2], 1, 0, 0);

  /* hinge joint between muscle 5 and fixed body */
  hinge[3] = dJointCreateHinge (world,0);
  dJointAttach (hinge[3],fixed_body[0],body[5]);
  dJointSetHingeAnchor (hinge[3], 0.25*SIDE, -0.5*SIDE, 4.7*SIDE);
  dJointSetHingeAxis (hinge[3], 1, 0, 0);

  /* hinge joint between muscle 2 and arm 1 */
  hinge[4] = dJointCreateHinge (world,0);
  dJointAttach (hinge[4],body[1],body[2]);
  dJointSetHingeAnchor (hinge[4], 0.25*SIDE, 1.0*SIDE, 1.7*SIDE);
  dJointSetHingeAxis (hinge[4], 1, 0, 0);

  /* hinge joint between muscle 4 and arm 1 */
  hinge[5] = dJointCreateHinge (world,0);
  dJointAttach (hinge[5],body[1],body[4]);
  dJointSetHingeAnchor (hinge[5], 0.25*SIDE, -0.5*SIDE, 1.7*SIDE);
  dJointSetHingeAxis (hinge[5], 1, 0, 0);

  /* hinge joint between muscle 6 and fixed body */
  hinge[6] = dJointCreateHinge (world,0);
  dJointAttach (hinge[6],fixed_body[0],body[6]);
  dJointSetHingeAnchor (hinge[6], 0.25*SIDE, -0.5*SIDE, 4.7*SIDE);
  dJointSetHingeAxis (hinge[6], 1, 0, 0);

  /* hinge joint between muscle 7 and arm 0 */
  hinge[7] = dJointCreateHinge (world,0);
  dJointAttach (hinge[7],body[0],body[7]);
  dJointSetHingeAnchor (hinge[7], 0.25*SIDE, 0.15*SIDE, 3.7*SIDE);
  dJointSetHingeAxis (hinge[7], 1, 0, 0);

  /* hinge joint between muscle 8 and fixed */
  hinge[8] = dJointCreateHinge (world,0);
  dJointAttach (hinge[8],fixed_body[0],body[8]);
  dJointSetHingeAnchor (hinge[8], 0.25*SIDE, 1.0*SIDE, 4.7*SIDE);
  dJointSetHingeAxis (hinge[8], 1, 0, 0);

  /* hinge joint between muscle 9 and arm 0 */
  hinge[9] = dJointCreateHinge (world,0);
  dJointAttach (hinge[9],body[0],body[9]);
  dJointSetHingeAnchor (hinge[9], 0.25*SIDE, 0.35*SIDE, 3.7*SIDE);
  dJointSetHingeAxis (hinge[9], 1, 0, 0);

  /* hinge joint between muscle 10 and arm 1 */
  hinge[10] = dJointCreateHinge (world,0);
  dJointAttach (hinge[10],body[1],body[10]);
  dJointSetHingeAnchor (hinge[10], 0.25*SIDE, -0.5*SIDE, 1.7*SIDE);
  dJointSetHingeAxis (hinge[10], 1, 0, 0);

  /* hinge joint between muscle 11 and arm 0 */
  hinge[11] = dJointCreateHinge (world,0);
  dJointAttach (hinge[11],body[0],body[11]);
  dJointSetHingeAnchor (hinge[11], 0.25*SIDE, 0.15*SIDE, 2.7*SIDE);
  dJointSetHingeAxis (hinge[11], 1, 0, 0);

  /* hinge joint between muscle 12 and arm 1 */
  hinge[12] = dJointCreateHinge (world,0);
  dJointAttach (hinge[12],body[1],body[12]);
  dJointSetHingeAnchor (hinge[12], 0.25*SIDE, 1.0*SIDE, 1.7*SIDE);
  dJointSetHingeAxis (hinge[12], 1, 0, 0);

  /* hinge joint between muscle 13 and arm 0 */
  hinge[13] = dJointCreateHinge (world,0);
  dJointAttach (hinge[13],body[0],body[13]);
  dJointSetHingeAnchor (hinge[13], 0.25*SIDE, 0.35*SIDE, 2.7*SIDE);
  dJointSetHingeAxis (hinge[13], 1, 0, 0);
  
  /* fixed body is fixed to the world */
  dJointID j = dJointCreateFixed (world, 0);
  dJointAttach (j,fixed_body[0],0);
  dJointSetFixed (j);

  /* slider joint between muscle 2 and arm 1 */
  slider[1] = dJointCreateSlider (world,0);
  dJointAttach (slider[1], body[2], body[3]);
  dJointSetSliderAxis (slider[1],0,0,1);

  /* slider joint between muscle 3 and arm 1 */
  slider[2] = dJointCreateSlider (world,0);
  dJointAttach (slider[2], body[4], body[5]);
  dJointSetSliderAxis (slider[2],0,0,1);

  /* slider joint between muscle 6 and muscle 7 */
  slider[3] = dJointCreateSlider (world,0);
  dJointAttach (slider[3], body[6], body[7]);
  dJointSetSliderAxis (slider[3],0,-1,1);

  /* slider joint between muscle 8 and muscle 9 */
  slider[4] = dJointCreateSlider (world,0);
  dJointAttach (slider[4], body[8], body[9]);
  dJointSetSliderAxis (slider[4],0,1,1);

  /* slider joint between muscle 10 and muscle 11 */
  slider[5] = dJointCreateSlider (world,0);
  dJointAttach (slider[5], body[10], body[11]);
  dJointSetSliderAxis (slider[5],0,-1,-1);

  /* slider joint between muscle 12 and muscle 13 */
  slider[6] = dJointCreateSlider (world,0);
  dJointAttach (slider[6], body[12], body[13]);
  dJointSetSliderAxis (slider[6],0,-1,1);

//  int i;
//   for (i=0; i<6; i++);
//   {
//     dJointSetSliderParam(slider[i],dParamFMax,dInfinity);
//     dJointSetSliderParam(slider[i],dParamCFM, 0.8);
//   }

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

  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}


Thanx in advance!!!

nandan

-- 
Build a system [MS Windows] that even a fool can use, and only a fool
will want to use it.
---- George Bernard Shaw

Just as there are many parts needed to make a human a human, there's a
remarkable number of things needed to make an individual who he is. A
face to distinguish yourself from others, a voice you aren't aware of
yourself, the hand you see when you awaken, the memories of childhood,
the feelings for the future. That's not all. There's the expanse of
the data net my cyber-brain can access. All of that goes into making
me what I am, giving rise to a consciousness that I call "me." And
simultaneously confining "me" within set limits.
---- Motoko Kusanagi, from Ghost In The Shell