btHingeConstraint Usage and Nans

[mklingen]

I'm trying to implement a dynamic simulation of a rigid serial-link robot using bullet. Our lab is already using bullet for a kinematic simulation, but we wanted to make the next step up this year. Getting weird issues when trying to create hinge joints.

I am trying to implement the robot as a series of rigid bodies for links, which are connected by btHingeConstraints (similar to say, a ragdoll). Unfortunately, I'm running into issues when connecting the robot's links together using hinge constraints.

What I have from our kinematic simulation is this:

• * The transform of each link in the global world frame (call these t_A and t_B).
  * The transform of the joint in the global world frame (call this t_j)
  * The axis of the joint in the joint's frame (assumed to be the Z axis)

Having this information, the constructor:

Code: Select all

bt::HingeConstraintbtHingeConstraint(btRigidBody body1, btRigidBody body2, btTransform rbA,  btTransform rbB);

seems to be the most convenient. However, I can't find good documentation on this particular constructor. (everyone seems to be using the one which specifies local points and axes).

So, I am assuming

• * rbA is the coordinate frame of the joint expressed in the local coordinate frame of body1
  * rbB is the coordinate frame of the joint expressed in the local coordinate frame of body2.

Is this correct? If so, I just do this simple math to compute the needed transforms:

Code: Select all

rbA = (t_j).inverse() * t_A;
rbB = (t_j).inverse() * t_B;

And the axis is just (0, 0, 1).

Unfortunately, when I do this, the resulting transform of both bodies ends up being Nan after the first update step. Some example output is here:

Code: Select all

04:22:02:820 PM 
 t_j    =     
-1                     0          0                 0.22
 0                     0         -1                 0.14
 0                    -1           0                0.346
0                       0          0                1

t_A    =
1                            2.44921e-16           0        0.22
-2.44921e-16            1                          0        0.14
0                            0                          1        0.346
0                            0                          0        1

 t_B   =
  -1       0         0        0.22
   0       0         -1       0.14
   0      -1         0        0.346
   0       0         0         1

 rbA   =      
-1                0                      0           0
0                  0                    -1           0
0                 -1                     0           0
0                  0                     0           1


rbB     =
1           0           0           0
0           1           0           0
0           0           1           0
0           0           0           1

Any idea what's going on here?

[mklingen]

So I figured out my problems. They were:

1. The starting frame of the rigid body and the motion state need to be the same upon construction of the joint.
2. My calculations of rb_A and rb_B were backwards. It turns out it's body_pose.inverse() * joint_pose;

[petercy]

Hey mklingen, thanks for sharing the solution. I am having a similar issue with my dynamic simulation and this sounds like it could be it. I'll let you know if it works.