Does Newton support general 6 degree of freedom joints?

A place to discuss everything related to Newton Dynamics.

Moderators: Sascha Willems, walaber

Does Newton support general 6 degree of freedom joints?

Postby Julio Jerez » Thu Apr 19, 2007 8:54 am

Absolutely.
Newton comes with a fully functional custom joint interface that allowed for the implementation of any configuration. including pulley, gears, levels, unilateral joints and arbitrary number of unilateral dof.
All of the more commonly used joints are implemented using this interface and we recommended using these joints or a variant of them as first choice.
The reason for using this specially designed joint is that they are made using the geometry of the problem as the bases to calculate the parametric values that enforce the joint contraints.
On the other hand a generic six-degree of freedom joint will have to cope with the well known problems of lost of degree of freedom and angle alising when decomposing general rotation matrices into Eulers angles.
Another reason for designing specialized custom joints is that the user is not limited to the pseudo rule that the motion of a rigid body is specified by 3 linear and 3 angular degrees of freedoms. This is an erroneous interpretation that appears to come from the real rule the say that above criteria is the minimum number of degree of freedom that can represent the full motion of a rigid body in a three-dimensional space.
There is nothing that said the a body can move in more than just 6 dof. This come specially handy when making joints using the geometrical approach rather than the analytical one of decomposing a matrix into Euler angles.

However when making a new joint the 3dof can be very useful for quick prototyping, because they are a lot easier to visualize intuitively.
Here is a quick and very simplistic but fully functional implementation of a general 6dof joint using the cutom joint interface.
The joint support any combination of dofs with limits on all of them.
Code: Select all
//********************************************************************
// Newton Game dynamics
// copyright 2000-2004
// By Julio Jerez
// VC: 6.0
// simple demo list vector class with iterators
//********************************************************************

// Custom6DOF.h: interface for the Custom6DOF class.
//
//////////////////////////////////////////////////////////////////////

#if !defined(AFX_Custom6DOF_H__B631F556_B7D7_F85ECF3E9ADE__INCLUDED_)
#define AFX_Custom6DOF_H__B631F556_B7D7_F85ECF3E9ADE__INCLUDED_

#include "NewtonCustomJoint.h"

class Custom6DOF: public NewtonCustomJoint 
{
public:
   Custom6DOF (const dMatrix& pinsAndPivoChildFrame,
      const dMatrix& pinsAndPivoParentFrame,
      const dVector& minLinearLimits, const dVector& maxLinearLimits,
      const dVector& minAngularLimits, const dVector& maxAngulaLimits,
      const NewtonBody* child, const NewtonBody* parent = NULL);
   virtual ~Custom6DOF();

protected:
   virtual void SubmitConstrainst ();
   void SubmitConstraints (const dMatrix& matrix0, const dMatrix& matrix1);

   dMatrix CalculateBasisAndJointAngle (const dMatrix& matrix0, const dMatrix& matrix1) const;
   dMatrix CalculateHinge_Angles (const dMatrix& matrix0, const dMatrix& matrix1, int x, int y, int z) const;
   dMatrix CalculateUniversal_Angles (const dMatrix& matrix0, const dMatrix& matrix1, int x, int y, int z) const;

   dMatrix m_localMatrix0;
   dMatrix m_localMatrix1;
   dVector m_minLinearLimits;
   dVector m_maxLinearLimits;
   dVector m_minAngularLimits;
   dVector m_maxAngularLimits;
   dVector m_maxMaxLinearErrorRamp;
   dVector m_maxMaxAngularErrorRamp;
};


#endif // !defined(AFX_Custom6DOF_H__B631F556_B7D7_F85ECF3E9ADE__INCLUDED_)


Code: Select all
//********************************************************************
// Newton Game dynamics
// copyright 2000-2007
// By Julio Jerez
// VC: 6.0
// simple demo list vector class with iterators
//********************************************************************

// Custom6DOF.cpp: implementation of the Custom6DOF class.
//
//////////////////////////////////////////////////////////////////////
#include <stdafx.h>
#include "Custom6DOF.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

enum d6DOF_AngularGeometry
{
   m_ball,
   m_hinge_x,
   m_hinge_y,
   m_hinge_z,
   m_universal_xy,
   m_universal_yz,
   m_universal_zx,
   m_fixed,
   m_free,
};


Custom6DOF::Custom6DOF (
   const dMatrix& pinsAndPivoChildFrame,
   const dMatrix& pinsAndPivoParentFrame,
   const dVector& minLinearLimits, const dVector& maxLinearLimits,
   const dVector& minAngularLimits, const dVector& maxAngularLimits,
   const NewtonBody* child, const NewtonBody* parent)
   :NewtonCustomJoint(6, child, parent)
{
   int i;
   for (i = 0; i < 3; i ++) {
      m_minLinearLimits[i] =  (dAbs (minLinearLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : minLinearLimits[i];
      m_maxLinearLimits[i] =  (dAbs (maxLinearLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : maxLinearLimits[i];
      m_minAngularLimits[i] =  (dAbs (minAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : minAngularLimits[i];
      m_maxAngularLimits[i] =  (dAbs (maxAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : maxAngularLimits[i];
   }
   CalculateLocalMatrix (pinsAndPivoChildFrame, m_localMatrix0, m_localMatrix1);

   // set default max constraint violations correction
   m_maxMaxLinearErrorRamp = dVector (0.2f, 0.2f,0.2f, 0.0f);
   m_maxMaxAngularErrorRamp = dVector (1.0f * 3.1416f / 180.0f, 1.0f * 3.1416f / 180.0f, 1.0f * 3.1416f / 180.0f, 0.0f);;
}

Custom6DOF::~Custom6DOF()
{
}



void Custom6DOF::SubmitConstrainst ()
{
   dMatrix matrix0;
   dMatrix matrix1;

   // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
   CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
   SubmitConstraints (matrix0, matrix1);
}

void Custom6DOF::SubmitConstraints (const dMatrix& matrix0, const dMatrix& matrix1)
{
   int i;
   dFloat dist;

   // add the linear limits
   const dVector& p0 = matrix0.m_posit;
   dVector p1 (matrix1.m_posit);
   dVector dp (p0 - p1);
   for (i = 0; i < 3; i ++) {
      if (!((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f))) {
         p1 += matrix1[i].Scale (dp % matrix1[i]);
      }
   }

   for (i = 0; i < 3; i ++) {
      if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
         NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
         NewtonUserJointSetRowStiffness (m_joint, 1.0f);
      } else {
         // it is a limited linear dof, check if it pass the limits
         dist = dp % matrix1[i];
         if (dist > m_maxLinearLimits[i]) {
            // clamp the error, so the not too much energy is added when constraint violation occurs
            if (dist > m_maxMaxLinearErrorRamp[i]) {
               dist = m_maxMaxLinearErrorRamp[i];
            }

            dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i] - dist));
            NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);

            NewtonUserJointSetRowStiffness (m_joint, 1.0f);
            // allow the object to return but not to kick going forward
            NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

         } else if (dist < m_minLinearLimits[i]) {
            // clamp the error, so the not too much energy is added when constraint violation occurs
            if (dist < -m_maxMaxLinearErrorRamp[i]) {
               dist = -m_maxMaxLinearErrorRamp[i];
            }

            dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i] - dist));
            NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);

            NewtonUserJointSetRowStiffness (m_joint, 1.0f);
            // allow the object to return but not to kick going forward
            NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
         }
      }
   }

   dMatrix basisAndEulerAngles (CalculateBasisAndJointAngle (matrix0, matrix1));
   const dVector& eulerAngles = basisAndEulerAngles.m_posit;
   for (i = 0; i < 3; i ++) {
      if ((m_minAngularLimits[i] == 0.0f) && (m_maxAngularLimits[i] == 0.0f)) {
         NewtonUserJointAddAngularRow (m_joint, eulerAngles[i], &basisAndEulerAngles[i][0]);
         NewtonUserJointSetRowStiffness (m_joint, 1.0f);
      } else {
         // it is a limited linear dof, check if it pass the limits
         if (eulerAngles[i] > m_maxAngularLimits[i]) {
            dist = eulerAngles[i] - m_maxAngularLimits[i];
            // clamp the error, so the not too much energy is added when constraint violation occurs
            if (dist > m_maxMaxAngularErrorRamp[i]) {
               dist = m_maxMaxAngularErrorRamp[i];
            }

            // tell joint error will minimize the exceeded angle error
            NewtonUserJointAddAngularRow (m_joint, dist, &basisAndEulerAngles[i][0]);

            // need high stiffness here
            NewtonUserJointSetRowStiffness (m_joint, 1.0f);

            // allow the joint to move back freely
            NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);

         } else if (eulerAngles[i] < m_minAngularLimits[i]) {
            dist = eulerAngles[i] - m_minAngularLimits[i];
            // clamp the error, so the not too much energy is added when constraint violation occurs
            if (dist < -m_maxMaxAngularErrorRamp[i]) {
               dist = -m_maxMaxAngularErrorRamp[i];
            }

            // tell joint error will minimize the exceeded angle error
            NewtonUserJointAddAngularRow (m_joint, dist, &basisAndEulerAngles[i][0]);

            // need high stiffness here
            NewtonUserJointSetRowStiffness (m_joint, 1.0f);

            // allow the joint to move back freely
            NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
         }
      }
   }
}

dMatrix Custom6DOF::CalculateBasisAndJointAngle (const dMatrix& matrix0, const dMatrix& matrix1) const
{

   d6DOF_AngularGeometry type;

   type = m_fixed;
   if ((m_minAngularLimits[2] == 0.0f) && (m_maxAngularLimits[2] == 0.0f)) {
      if ((m_minAngularLimits[1] == 0.0f) && (m_maxAngularLimits[1] == 0.0f)) {
         if ((m_minAngularLimits[0] == 0.0f) && (m_maxAngularLimits[0] == 0.0f)) {
            // this is a fix joint
            type = m_fixed;
         } else {
            //this is a hinge
            type = m_hinge_x;
         }
      } else {
         if ((m_minAngularLimits[0] == 0.0f) && (m_maxAngularLimits[0] == 0.0f)) {
            type = m_hinge_y;
         } else {
            type = m_universal_xy;
         }
      }

   } else {
      if ((m_minAngularLimits[1] == 0.0f) && (m_maxAngularLimits[1] == 0.0f)) {
         if ((m_minAngularLimits[0] == 0.0f) && (m_maxAngularLimits[0] == 0.0f)) {
            type = m_hinge_z;
         } else {
            type = m_universal_zx;
         }
      } else {
         if ((m_minAngularLimits[0] == 0.0f) && (m_maxAngularLimits[0] == 0.0f)) {
            type = m_universal_yz;
         } else {
            type = m_free;
         }
      }
   }

   switch (type)
   {
      case m_fixed:
      {
         //return CalculateHinge_X_Angles (matrix0, matrix1);
         _ASSERTE (0);
         return CalculateHinge_Angles (matrix0, matrix1, 0, 1, 2);
         break;
      }

      case m_ball:
      {
         _ASSERTE (0);
         return (GetIdentityMatrix());
         break;
      }

      case m_hinge_x:
      {
         //return CalculateHinge_X_Angles (matrix0, matrix1);
         return CalculateHinge_Angles (matrix0, matrix1, 0, 1, 2);
         break;
      }

      case m_hinge_y:
      {
         //return CalculateHinge_Y_Angles (matrix0, matrix1);
         return CalculateHinge_Angles (matrix0, matrix1, 1, 2, 0);
         break;
      }

      case m_hinge_z:
      {
         //return CalculateHinge_Z_Angles (matrix0, matrix1);
         return CalculateHinge_Angles (matrix0, matrix1, 2, 0, 1);
         break;
      }


      case m_universal_xy:
      {
         //return CalculateUniversal_Angles (matrix0, matrix1, 1, 0, 2);
         return CalculateUniversal_Angles (matrix0, matrix1, 0, 1, 2);
         break;
      }

      case m_universal_yz:
      {
         //return CalculateUniversal_Angles (matrix0, matrix1, 1, 2, 0);
         return CalculateUniversal_Angles (matrix0, matrix1, 2, 1, 0);
         break;
      }

      case m_universal_zx:
      {
         //return CalculateUniversal_Angles (matrix0, matrix1, 2, 0, 1);
         return CalculateUniversal_Angles (matrix0, matrix1, 0, 2, 1);
         break;
      }


      case m_free:
      {
         return CalculateFree_XYZ_Angles (matrix0, matrix1);
      }

   }

   return (GetIdentityMatrix());
}


dMatrix Custom6DOF::CalculateHinge_Angles (const dMatrix& matrix0, const dMatrix& matrix1, int x, int y, int z) const
{
   dFloat sinAngle;
   dFloat cosAngle;
   dVector angles;

   // get a point along the pin axis at some reasonable large distance from the pivot
   //   dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
   //   dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
   // two constraints row perpendicular to the pin vector
   //   NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
   //   NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);


   // assume that angles relative to y and z and error angles and therefore small,
   // we only concern wit the joint angle relative to the x axis in parent space.
   // this is possible because for a one DoF joint the error angle are small, and if tow angel are small
   // the the order of multiplication is irrelevant.
   // this code will not work of more than one dof joint or in the rigid angel error is too large.
   cosAngle = matrix0[x] % matrix1[x];
   dVector sinDir (matrix0[x] * matrix1[x]);

   sinAngle = sinDir % matrix1[y];
   angles[y] = dAtan2 (sinAngle, cosAngle);

   sinAngle = sinDir % matrix1[z];
   angles[z] = dAtan2 (sinAngle, cosAngle);

   cosAngle = matrix0[y] % matrix1[y];
   sinAngle = (matrix0[y] * matrix1[y]) % matrix1[x];
   angles[x] = dAtan2 (sinAngle, cosAngle);

   angles[3] = 1.0f;

   _ASSERTE (dAbs (angles[y]) < 0.3f);
   _ASSERTE (dAbs (angles[z]) < 0.3f);

   return dMatrix (matrix1[0], matrix1[1], matrix1[2], angles);
}

dMatrix Custom6DOF::CalculateUniversal_Angles (const dMatrix& matrix0, const dMatrix& matrix1, int x, int y, int z) const
{
   dFloat sinAngle;
   dFloat cosAngle;

   // this assumes calculation assumes that the angle relative to the z axis is alway small
   // because it is the unconstitutionally strong angle
   dMatrix basisAndEulerAngles;
   basisAndEulerAngles[x] = matrix0[x];
   basisAndEulerAngles[y] = matrix1[y];
   basisAndEulerAngles[z] = basisAndEulerAngles[x] * basisAndEulerAngles[y];
   basisAndEulerAngles[z] = basisAndEulerAngles[z].Scale (1.0f / dSqrt (basisAndEulerAngles[z] % basisAndEulerAngles[z]));

   cosAngle = matrix0[y] % matrix1[y];
   sinAngle = (matrix0[y] * matrix1[y]) % basisAndEulerAngles[x];
   basisAndEulerAngles[3][x] = dAtan2 (sinAngle, cosAngle);

   cosAngle = matrix0[x] % matrix1[x];
   sinAngle = (matrix0[x] * matrix1[x]) % matrix1[y];
   basisAndEulerAngles[3][y] = dAtan2 (sinAngle, cosAngle);

   dVector dir3 (basisAndEulerAngles[z] * basisAndEulerAngles[x]);
   dir3 = dir3.Scale (1.0f / dSqrt (dir3 % dir3));
   cosAngle = dir3 % basisAndEulerAngles[y];
   sinAngle = (dir3 * basisAndEulerAngles[y]) % basisAndEulerAngles[z];
   basisAndEulerAngles[3][z] = dAtan2 (sinAngle, cosAngle);
   //   _ASSERTE (dAbs (angle_z) < 0.1f);
   basisAndEulerAngles[3][3] = 1.0f;

   _ASSERTE (dAbs (basisAndEulerAngles[3][z]) < 0.3f);
   return basisAndEulerAngles;
}




Note: this joint do not implement all of the possible permutation of Euler angles decompositions, just enough to reproduce all of the most of the commonly used joints
However it is quite easy to extend it.

Here are few examples
And up vector would be:
Code: Select all
dMatrix pinsAndPivoFrame (location);
dVector minLinearLimits (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
dVector maxLinearLimits ( 1.0e10f,  1.0e10f,  1.0e10f, 0.0f);
dVector minAngularLimits ( -1.0e10f, 0.0, 0.0f, 0.0f);
dVector maxAngularLimits (  1.0e10f, 0.0, 0.0f, 0.0f);
new Custom6DOF (pinsAndPivoFrame, pinsAndPivoFrame, minLinearLimits, maxLinearLimits, minAngularLimits, maxAngularLimits, boxBody, NULL);


a hinge would be as simple as
Code: Select all
dMatrix pinsAndPivoFrame (location);
dVector minLinearLimits (0.0f, 0.0f, 0.0f, 0.0f);
dVector maxLinearLimits (0.0f, 0.0f, 0.0f, 0.0f);
dVector minAngularLimits ( -1.0e10f, 0.0f, 0.0f, 0.0f);
dVector maxAngularLimits (  1.0e10f, 0.0f, 0.0f, 0.0f);
new Custom6DOF (pinsAndPivoFrame, pinsAndPivoFrame, minLinearLimits, maxLinearLimits, minAngularLimits, maxAngularLimits, boxBody, NULL);


a limited hinge would be as simple as
Code: Select all
dMatrix pinsAndPivoFrame (location);
dVector minLinearLimits (0.0f, 0.0f, 0.0f, 0.0f);
dVector maxLinearLimits (0.0f, 0.0f, 0.0f, 0.0f);
dVector minAngularLimits (-30.0f * 3.1416 / 180.0f, 0.0f, 0.0f, 0.0f);
dVector maxAngularLimits ( 30.0f * 3.1416 / 180.0f, 0.0f, 0.0f, 0.0f);
new Custom6DOF (pinsAndPivoFrame, pinsAndPivoFrame, minLinearLimits, maxLinearLimits, minAngularLimits, maxAngularLimits, boxBody, NULL);


a simple slider would be as simple as
Code: Select all
dMatrix pinsAndPivoFrame (location);
dVector minLinearLimits (minLimit, 0.0f, 0.0f, 0.0f);
dVector maxLinearLimits (maxLimit, 0.0f, 0.0f, 0.0f);
dVector minAngularLimits (0.0f, 0.0f, 0.0f, 0.0f);
dVector maxAngularLimits (0.0f, 0.0f, 0.0f, 0.0f);
new Custom6DOF (pinsAndPivoFrame, pinsAndPivoFrame, minLinearLimits, maxLinearLimits, minAngularLimits, maxAngularLimits, boxBody, NULL);


The joint can also be subclassed to implement simple motors, controlled PID constraints, and angular and linear springs, or combinations along the free DoF.
This is an example of a simple Hinge with Limits and PID motor.
Code: Select all
class MotorisedAnLimitedHinge: public Custom6DOF
{
    public:

    MotorisedAnLimitedHinge (
   const dMatrix& pinsAndPivoChildFrame,
   const dMatrix& pinsAndPivoParentFrame,
   NewtonBody* child, NewtonBody* parent = NULL)
      :Custom6DOF (pinsAndPivoChildFrame, pinsAndPivoParentFrame,
          dVector (0.0, 0.0f, 0.0f), dVector (0.0f, 0.0f, 0.0f),
          dVector (-1.0e10f, 0.0f, 0.0f), dVector (1.0e10f, 0.0f, 0.0f),
          child, parent)
    {
       m_jointAngle = 0.0f;
       m_controlAngle = 0.0f;
       m_minAngle = -30.0f * 3.1416f / 180.0f;
       m_maxAngle =  30.0f * 3.1416f / 180.0f;
    }

    protected:
    virtual void SubmitConstrainst ()
    {
   dMatrix matrix0;
   dMatrix matrix1;

   // summit the contraints to inforce teh normal unconstraned cylinder
   CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
   SubmitConstraints (matrix0, matrix1);

   // now add the contaraint rown to implement teh motor with limits
   float sinAngle;
   float cosAngle;

   sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
   cosAngle = matrix0.m_up % matrix1.m_up;
   m_jointAngle = dAtan2 (sinAngle, cosAngle);
   if (m_jointAngle < m_minAngle) {
       dFloat relAngle;

       relAngle = m_jointAngle - m_minAngle;

       // tell joint error will minimize the exceeded angle error
       NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

       // need high stiffness here
       NewtonUserJointSetRowStiffness (m_joint, 1.0f);

       // allow the joint to move back freely
       NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);


   } else if (m_jointAngle > m_maxAngle) {
       dFloat relAngle;

       relAngle = m_jointAngle - m_maxAngle;

       // tell joint error will minimize the exceeded angle error
       NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

       // need high stiffness here
       NewtonUserJointSetRowStiffness (m_joint, 1.0f);

       // allow the joint to move back freely
       NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
   } else {
       // add a the control angle
       dFloat relAngle;

       // make sure the control angle in never outside the limits
       rotterAngle = min (rotterAngle, m_maxAngle);
       rotterAngle = max (rotterAngle, m_minAngle);
       m_controlAngle = rotterAngle;

       relAngle = m_jointAngle - m_controlAngle;

       // tell joint error will minimize the exceeded angle error
       NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

       // need high stiffness here
       NewtonUserJointSetRowStiffness (m_joint, 1.0f);
   }
    }

    float m_minAngle;
    float m_maxAngle;
    float m_jointAngle;
    float m_controlAngle;
};
Code: Select all
Julio Jerez
Moderator
Moderator
 
Posts: 11153
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Return to General Discussion

Who is online

Users browsing this forum: No registered users and 4 guests