[ODE] per-joint erp and cfm values

Adam Rotaru adam_rotaru at yahoo.com
Wed Mar 13 00:37:02 2002


  I need ball-socket joints with different ERP/CFM
values
in my simulation (in the same simulation).  I modified
ODE to have per-joint ERP/CFM settings for ball
joints.
The changes I did are not too complicated, and I hope
they are correct.  Russ, I'd be glad if you could have
a 
quick look.  
I added joint_erp and joint_cfm fields to dxJointBall,
added an extra erp parameter to the setBall() utility
f.,
and added dJointSetBallParam() f.
The only thing which I'm not sure of is whether
all three components of info->cfm[] should be set.
The diffs are not too long, so I include them below
(they are against the cvs version as of today).
cheers, 
  --adam

Index: include/ode/common.h
===================================================================
RCS file: /cvsroot/ode/include/ode/common.h,v
retrieving revision 1.25
diff -c -r1.25 common.h
*** include/ode/common.h	2001/12/28 07:47:22	1.25
--- include/ode/common.h	2002/03/13 07:25:07
***************
*** 246,251 ****
--- 246,252 ----
    dParamFMax, \
    dParamFudgeFactor, \
    dParamBounce, \
+   dParamERP, \
    dParamCFM, \
    dParamStopERP, \
    dParamStopCFM, \
***************
*** 261,266 ****
--- 262,269 ----
    dParamFMax ## x, \
    dParamFudgeFactor ## x, \
    dParamBounce ## x, \
+   dParamERP ## x, \
+   dParamCFM ## x, \
    dParamStopERP ## x, \
    dParamStopCFM ## x, \
    /* parameters for suspension */ \
Index: include/ode/objects.h
===================================================================
RCS file: /cvsroot/ode/include/ode/objects.h,v
retrieving revision 1.29
diff -c -r1.29 objects.h
*** include/ode/objects.h	2002/01/06 19:22:23	1.29
--- include/ode/objects.h	2002/03/13 07:25:07
***************
*** 127,132 ****
--- 127,133 ----
  dBodyID dJointGetBody (dJointID, int index);
  
  void dJointSetBallAnchor (dJointID, dReal x, dReal
y, dReal z);
+ void dJointSetBallParam (dJointID, int parameter,
dReal value);
  void dJointSetHingeAnchor (dJointID, dReal x, dReal
y, dReal z);
  void dJointSetHingeAxis (dJointID, dReal x, dReal y,
dReal z);
  void dJointSetHingeParam (dJointID, int parameter,
dReal value);
Index: ode/src/joint.cpp
===================================================================
RCS file: /cvsroot/ode/ode/src/joint.cpp,v
retrieving revision 1.38
diff -c -r1.38 joint.cpp
*** ode/src/joint.cpp	2001/12/28 07:47:22	1.38
--- ode/src/joint.cpp	2002/03/13 07:25:08
***************
*** 45,53 ****
  
  // set three "ball-and-socket" rows in the
constraint equation, and the
  // corresponding right hand side.
  
  static inline void setBall (dxJoint *joint,
dxJoint::Info2 *info,
! 			    dVector3 anchor1, dVector3 anchor2)
  {
    // anchor points in global coordinates with
respect to body PORs.
    dVector3 a1,a2;
--- 45,55 ----
  
  // set three "ball-and-socket" rows in the
constraint equation, and the
  // corresponding right hand side.
+ // `erp' is the erp value to use with this
constraint.
  
  static inline void setBall (dxJoint *joint,
dxJoint::Info2 *info,
! 			    dVector3 anchor1, dVector3 anchor2,
! 			    dReal erp)
  {
    // anchor points in global coordinates with
respect to body PORs.
    dVector3 a1,a2;
***************
*** 69,75 ****
    }
  
    // set right hand side
!   dReal k = info->fps * info->erp;
    if (joint->node[1].body) {
      for (int j=0; j<3; j++) {
        info->c[j] = k * (a2[j] +
joint->node[1].body->pos[j] -
--- 71,77 ----
    }
  
    // set right hand side
!   dReal k = info->fps * erp;
    if (joint->node[1].body) {
      for (int j=0; j<3; j++) {
        info->c[j] = k * (a2[j] +
joint->node[1].body->pos[j] -
***************
*** 501,506 ****
--- 503,511 ----
  {
    dSetZero (j->anchor1,4);
    dSetZero (j->anchor2,4);
+ 
+   j->joint_erp = j->world->global_erp;
+   j->joint_cfm = j->world->global_cfm;
  }
  
  
***************
*** 513,519 ****
  
  static void ballGetInfo2 (dxJointBall *joint,
dxJoint::Info2 *info)
  {
!   setBall
(joint,info,joint->anchor1,joint->anchor2);
  }
  
  
--- 518,529 ----
  
  static void ballGetInfo2 (dxJointBall *joint,
dxJoint::Info2 *info)
  {
!   setBall (joint,info,joint->anchor1,joint->anchor2,
joint->joint_erp); 
! 
!   // set CFM for this joint
!   info->cfm[0] = joint->joint_cfm;
!   info->cfm[1] = joint->joint_cfm;
!   info->cfm[2] = joint->joint_cfm;
  }
  
  
***************
*** 535,540 ****
--- 545,560 ----
  }
  
  
+ extern "C" void dJointSetBallParam (dxJointBall
*joint,
+ 				    int parameter, dReal value)
+ {
+   dUASSERT(joint,"bad joint argument");
+   dUASSERT(joint->vtable == &__dball_vtable,"joint
is not a ball");
+   if (parameter == dParamERP) joint->joint_erp =
value;
+   else if (parameter == dParamCFM) joint->joint_cfm
= value;
+ }
+
+ 
  dxJoint::Vtable __dball_vtable = {
    sizeof(dxJointBall),
    (dxJoint::init_fn*) ballInit,
***************
*** 580,586 ****
  static void hingeGetInfo2 (dxJointHinge *joint,
dxJoint::Info2 *info)
  {
    // set the three ball-and-socket rows
!   setBall
(joint,info,joint->anchor1,joint->anchor2);
  
    // set the two hinge rows. the hinge axis should
be the only unconstrained
    // rotational axis, the angular velocity of the
two bodies perpendicular to
--- 600,606 ----
  static void hingeGetInfo2 (dxJointHinge *joint,
dxJoint::Info2 *info)
  {
    // set the three ball-and-socket rows
!   setBall
(joint,info,joint->anchor1,joint->anchor2,info->erp);
  
    // set the two hinge rows. the hinge axis should
be the only unconstrained
    // rotational axis, the angular velocity of the
two bodies perpendicular to
Index: ode/src/joint.h
===================================================================
RCS file: /cvsroot/ode/ode/src/joint.h,v
retrieving revision 1.31
diff -c -r1.31 joint.h
*** ode/src/joint.h	2001/12/28 07:47:23	1.31
--- ode/src/joint.h	2002/03/13 07:25:08
***************
*** 159,164 ****
--- 159,165 ----
  struct dxJointBall : public dxJoint {
    dVector3 anchor1;		// anchor w.r.t first body
    dVector3 anchor2;		// anchor w.r.t second body
+   dReal joint_erp,joint_cfm;	// joint parameters
(erp,cfm)
  };
  extern struct dxJoint::Vtable __dball_vtable;
  



__________________________________________________
Do You Yahoo!?
Try FREE Yahoo! Mail - the world's greatest free email!
http://mail.yahoo.com/