Pyramid limit ball and socket

A place to discuss everything related to Newton Dynamics.

Moderators: Sascha Willems, walaber

Pyramid limit ball and socket

Postby JoeWright » Mon Jan 19, 2009 12:43 pm

This have driven me quite mad. I walk around slowly rotating my hands as I think of 3D rotations. I even had a dream about them.

Anyway, I think I've got a joint that I like. Its like the CustomLimitBallAndSocket but has two independent limit angles forming a pyramid, as well as twist limits.

Julio, I think that the code for CustomLimitBallAndSocket produced an incorrect twist angle because the projection was onto a slanting frame (sometimes). I may be wrong but I think I'm right. In my code, I construct a new twist reference frame from the parent pin by rotating it so that the parent and child pin m_fronts align. This is done without rotating around m_front so a suitable base reference frame is formed in order to determine the twist angle. Hopefully my code makes sense and is correct.

I've used some non-standard things in my code but I hope its pretty readable. I went down a complete dead end with euler angles which I don't think are at all helpful for this kind of thing. However, its a good learning exercise.

Code: Select all
void CustomLimitBallAndSocket::SubmitConstrainst (dFloat timestep, int threadIndex)
{
   // get global version of the pin and pivot from the perspective of each body
   dMatrix global_pin_pivot0=mPinPivot0;
   mpParentBody->ConvertLocalToGlobalMatrix(global_pin_pivot0);
   dMatrix global_pin_pivot1=mPinPivot1;
   mpChildBody->ConvertLocalToGlobalMatrix(global_pin_pivot1);
   // end get global version of the pin and pivot from the perspective of each body

   // first add linear contraints to fix the pivot point relative to both bodies
   NewtonUserJointAddLinearRow(mpNewtonJoint,&global_pin_pivot0.m_posit[0],&global_pin_pivot1.m_posit[0],&global_pin_pivot0.m_front()[0]);
   NewtonUserJointAddLinearRow(mpNewtonJoint,&global_pin_pivot0.m_posit[0],&global_pin_pivot1.m_posit[0],&global_pin_pivot0.m_up()[0]);
   NewtonUserJointAddLinearRow(mpNewtonJoint,&global_pin_pivot0.m_posit[0],&global_pin_pivot1.m_posit[0],&global_pin_pivot0.m_right()[0]);
   // end first add linear contraints

   // 'twist' is rotation around m_front on the child body.
   // to do this we need to project onto to orthogonal axes.
   // However, we can't use the two on the child body because they
   // rotate with the child.  Also, we can't use the two of the parents because rotations
   // around the other two axes put this reference plane at an angle.  This then makes
   // the determination of the angle incorrect.
   // The solution here is to take a reference frame that starts as the parents pin frame.
   // This is then rotated so that the m_front vector points in the same direction as for the
   // child.  However, the rotation does not rotate around m_front and so provides a solid reference
   // frame.
   // In order to perform this rotation.  First, we get a pin frame for the child that is relative
   // to the parent's pin frame.  This allows a frame of reference that is relative to the parent's
   // pin (and so the parent's m_front is {1,0,0} ).
   // So, we have to m_front's - one for the parent at {1,0,0} and one for the child at {a,b,c}.
   // An axis of rotation is found by taking the cross product.  Then a rotation matrix is found to
   // rotate around this axis.  Below this is shown as one matrix (twist_reference_frame).  I worked
   // this out using mathematica and http://en.wikipedia.org/wiki/Rotation_matrix#Rotation_about_an_arbitrary_vector
   // A couple of signs needed changing - I'm not sure why but who cares.
   // Then this matrix is multiplied with the parent's pin frame.
   // This gives a reference frame that has m_front pointing in the same direction as for the child.
   // But the other two axes are in a position that hasn't been rotated around m_front.
   // Therefore this set of axes can be used to project onto in order to determine the twist angle
   dMatrix rotation_0,rotation_1,inverse_rotation_0,relative_frame;

   rotation_0=global_pin_pivot0;
   rotation_0.m_posit=dVector(0.0,0.0,0.0);
   rotation_1=global_pin_pivot1;
   rotation_1.m_posit=dVector(0.0,0.0,0.0);
   inverse_rotation_0=rotation_0.Inverse();

   relative_frame=rotation_1*inverse_rotation_0;

   // get rotated reference frame that doesn't rotate around the twist axis (axis 0)
   dMatrix twist_reference_frame(GetIdentityMatrix());
   dFloat a,b,c;
   a=relative_frame.m_front().m_x;
   b=relative_frame.m_front().m_y;
   c=relative_frame.m_front().m_z;
   if (fabs(b)>0.001 || fabs(c)>0.001)
   {
      dFloat axis0_angle=atan2(sqrt(b*b+c*c),a);
      twist_reference_frame.m_front()=dVector(cos(axis0_angle),b*sin(axis0_angle)/sqrt(b*b+c*c),c*sin(axis0_angle)/sqrt(b*b+c*c),0.0);
      twist_reference_frame.m_up()=dVector(-b*sin(axis0_angle)/sqrt(b*b+c*c),(c*c+b*b*cos(axis0_angle))/(b*b+c*c),(b*c*(-1+cos(axis0_angle)))/(b*b+c*c),0.0);
      twist_reference_frame.m_right()=dVector(-c*sin(axis0_angle)/sqrt(b*b+c*c),(b*c*(-1+cos(axis0_angle)))/(b*b+c*c),(b*b+c*c*cos(axis0_angle))/(b*b+c*c),0.0);
   }
   twist_reference_frame=twist_reference_frame*rotation_0;
   // end get rotated reference frame that doesn't rotate around the twist axis (axis 0)

   double x,y,angle,constrained_angle_1,constrained_angle_2,min_angle,max_angle;

   // twist around axis 0
   y = global_pin_pivot1.m_up() % twist_reference_frame.m_right();
   x = global_pin_pivot1.m_up() % twist_reference_frame.m_up();
 
   angle = atan2(y,x);

   min_angle=mMinAngle.X*PI/180.0f;
   max_angle=mMaxAngle.X*PI/180.0f;

    if (angle > max_angle + 0.001f)
   {
      NewtonUserJointAddAngularRow (mpNewtonJoint, angle - max_angle, &global_pin_pivot1.m_front()[0]);
    }
   else if (angle < min_angle - 0.001f)
   {
      NewtonUserJointAddAngularRow (mpNewtonJoint, angle - min_angle, &global_pin_pivot1.m_front()[0]);
    }
   // end twist around axis 0

   // As well as contraining the twist, we want to constrain rotations around the other two angles to fall
   // inside a pyramid.  This is done by projecting the m_front vector of the child onto two pairs of axes
   // of the parent (as shown below).
   // If outside the pyramid, the limited position is calculated and constrained to
   // Limits are ok if comfortably in [-pi/2, pi/2] but I think the twist angle gets messed up if outside
   // this range.

   bool constrain_pyramid=false;

   // around axis 1 (pyramid)
   y = global_pin_pivot1.m_front() % global_pin_pivot0.m_right();
   x = global_pin_pivot1.m_front() % global_pin_pivot0.m_front();
 
   constrained_angle_1 = angle = atan2(y,x);
   min_angle=mMinAngle.Y*PI/180.0f;
   max_angle=mMaxAngle.Y*PI/180.0f;

   if (angle > max_angle + 0.001f)
   {
      constrained_angle_1=max_angle;
      constrain_pyramid=true;
   }
   else if (angle < min_angle - 0.001f)
   {
      constrained_angle_1=min_angle;
      constrain_pyramid=true;
   }
   // end around axis 1 (pyramid)

   // around axis 2 (pyramid)
   y = global_pin_pivot1.m_front() % global_pin_pivot0.m_up();
   x = global_pin_pivot1.m_front() % global_pin_pivot0.m_front();
 
   constrained_angle_2 = angle = atan2(y,x);
   min_angle=mMinAngle.Z*PI/180.0f;
   max_angle=mMaxAngle.Z*PI/180.0f;

   if (angle > max_angle + 0.001f)
   {
      constrained_angle_2=max_angle;
      constrain_pyramid=true;
   }
   else if (angle < min_angle - 0.001f)
   {
      constrained_angle_2=min_angle;
      constrain_pyramid=true;
   }

   // end around axis 2 (pyramid)

   if (constrain_pyramid)
   {
      dFloat constrained_axis_0_length=global_pin_pivot1.m_front() % global_pin_pivot0.m_front();
      dFloat constrained_axis_1_length=constrained_axis_0_length * tan(constrained_angle_1);
      dFloat constrained_axis_2_length=constrained_axis_0_length * tan(constrained_angle_2);
      dVector constrained_vector(  global_pin_pivot0.m_front().Scale(constrained_axis_0_length)
                           + global_pin_pivot0.m_right().Scale(constrained_axis_1_length)
                           + global_pin_pivot0.m_up().Scale(constrained_axis_2_length)
                          );
      constrained_vector=constrained_vector.Normalise();

      dVector point_on_child=(  global_pin_pivot1.m_posit
                        + global_pin_pivot1.m_front().Scale(MIN_JOINT_PIN_LENGTH)
                        );
      dVector point_on_parent=(  global_pin_pivot0.m_posit
                        + constrained_vector.Scale(MIN_JOINT_PIN_LENGTH)
                        );

      NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_parent[0],&point_on_child[0],&global_pin_pivot0.m_front()[0]);
      NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_parent[0],&point_on_child[0],&global_pin_pivot0.m_up()[0]);
      NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_parent[0],&point_on_child[0],&global_pin_pivot0.m_right()[0]);
   }
}
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: Pyramid limit ball and socket

Postby Julio Jerez » Mon Jan 19, 2009 1:48 pm

Ok Joe now that I release teh SDK let us do taht joint.
I was bussy having teh Beta fo realase.
I will make weit teh contartyon I explened to you in the previus post.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Pyramid limit ball and socket

Postby JoeWright » Mon Jan 19, 2009 3:35 pm

Cool, cheers.

I think what I've posted works but I'd like to see what you come up with.
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: Pyramid limit ball and socket

Postby JoeWright » Thu Jan 29, 2009 11:38 am

My joint seems to be blowing up now and then.

Julio, if you do find time to do a version of the joint that would be great.

Thanks, Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: Pyramid limit ball and socket

Postby Julio Jerez » Thu Jan 29, 2009 4:15 pm

I notice that too I will check it out.
Joe do you have a test I can check?
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Pyramid limit ball and socket

Postby JoeWright » Thu Jan 29, 2009 6:10 pm

Not easily. My programme's too interweaved with other stuff.

If you need an example I'll try and modify one of your demos over the weekend with my joint code.
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: Pyramid limit ball and socket

Postby JoeWright » Mon Feb 09, 2009 12:04 pm

Julio, I've started making a test using your demos as a template. Hopefully you'll be able to see where I'm going wrong.

These are the files:

http://www.badmintology.org/Badmintology/temp/CustomPyramid.cpp
http://www.badmintology.org/Badmintology/temp/CustomPyramid.h
http://www.badmintology.org/Badmintology/temp/PyramidJoints.cpp

As mentioned in another thread, I've got the strangeness of torques going in the opposite direction. I've corrected for this but never tracked down the cause.

Secondly, my third test is not working here (joint blows up) even though it works in my original code. As far as I can see all my data agrees in both versions to at least 5d.p. so I don't know the reason for the failure. Maybe you'll spot something. I was thinking maybe its to do with joint frictions or something but I can't account for it working fine in my original version but failing in the version posted here.

At the top of PyramidJoints.cpp is a test number define (currently 3) which selects which tests to do. Below it are a description of the tests. I was going to post several more tests but didn't expect it to fail so soon. If you can work out what's wrong with test 3 then I'll post some more tests.

To recap on the joint: limited twist around one axis and limited movement within a pyramid for the other two axes.

Thanks, Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: Pyramid limit ball and socket

Postby Julio Jerez » Wed Feb 11, 2009 2:09 pm

I will check it out this saturday. I will add teh joint to the joint library.
I also have other reported bugs that I will check too.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Pyramid limit ball and socket

Postby JoeWright » Wed Feb 11, 2009 6:40 pm

Cool, cheers Julio. I can't work out why I'm getting inconsistencies between my original code and the version I've posted here.
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: Pyramid limit ball and socket

Postby JoeWright » Mon Feb 16, 2009 11:37 am

Julio, how's it going? Is there anything else you need from me?
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm


Return to General Discussion

Who is online

Users browsing this forum: No registered users and 15 guests

cron