constraint between object / different initial orientation

[moniker]

I'm trying to setup a slider-like joint using a 6dof constraint. The problem I'm trying to solve is when 2 objects with different orientations are connected, I'd like the joint to constrain them to their relative orientations when the joint is attached.

Given:
Body1: pos = -0.5, 0, 0 and quat = 0.70710676908493 0 0 0.70710676908493 (90 degree rotation from identity)
Body2: pos = 0.5, 0, 0 and quat = 1 0 0 0

Attach joint:

Code: Select all

btTransform frameInA, frameInB;
	frameInA = btTransform::getIdentity();
	frameInB = btTransform::getIdentity();
	frameInA.setOrigin(btVector3(0., 0., 0.));
	frameInB.setOrigin(btVector3(0., 0., 0.));
//	frameInA.setRotation(btQuaternion(0.70710676908493, 0, 0, 0.70710676908493));
	//frameInB.setRotation(btQuaternion((0.70710676908493, 0, 0, 0.70710676908493));
	
	
	m_joint = new btGeneric6DofConstraint(
		body1, 
		body2,
		frameInA, 
		frameInB, 
		true
	);

	btVector3 lowerSliderLimit = btVector3(0.2, 0, 0);
	btVector3 hiSliderLimit = btVector3(1.5, 0, 0);
	m_joint->setLinearLowerLimit(lowerSliderLimit);
	m_joint->setLinearUpperLimit(hiSliderLimit);

As you can see in the code, I was playing around with the transform rotations, but the rotations on frameInB seem to have no effect and nothing I do to account for the rotation in frameInA seems to account for its orientation. What I am seeing is the joint constrain to the local x-axis of body1 and body2 translated without rotation to meet the length requirement of the 6dof constraint.

In the images, X-axis is blue and Y-axis is pink. The white points is the joint origin in world coordinates on each body and the white lines the axes of the joint on each body

initial condition:


final state due to constraint:


How can I setup the joint such that it constraints the -y-axis of body1 (on the left) and the -x-axis of body2 (on the right) to align? In this test, I want the final state of the objects to be both on the x-axis 1.5 units apart.

[moniker]

FWIW, I managed to use a btSliderConstraint to accomplish my goal although I still want to figure out how to do this with a 6DOF, so I'm not quite finished yet.

It would be really nice if there were some functions in Bullet to handle these kinds of operations. It's pretty tricky stuff that would be nice not to have to think about. In particular, handling the quaternions in a nice way is not simple because of degeneracies at certain angles.

Ideally, there would be a constrain constructor that allowed me to say "Connect these 2 bodies with this constraint while maintaining relative orientation". From the code below, this requires:

1) deriving the inverse local orientations of the 2 bodies
2) deriving the relative orientation in the global frame of the 2 bodies
3) compositing the results from 1 and 2

At the very least, there should be a function in btQuaternion that constructs a quat by taking the ratio of 2 vectors, which is effectively what the code below does.

Code: Select all

btVector3 diff = (
			body2.getWorldTransform().getOrigin()-body1.getWorldTransform().getOrigin()
		).normalize();
	
	btVector3 axis = (
			btVector3(1, 0, 0).cross(diff)
		).normalize();
	
	btScalar angle = acos(
		btVector3(1, 0, 0).dot(diff)
	);

	btQuaternion qq = btQuaternion(axis, angle);
	// degenerate cases
	if(ABS(angle) < 1e-5)  {
		qq = btQuaternion::getIdentity();
	}
	else if(ABS(angle - 3.141593) < 1e-5) {
		qq = btQuaternion(0, 0, 1, 0);
	}

	btTransform frameInA, frameInB;
	frameInA = btTransform::getIdentity();
	frameInA.setRotation(
		body1.getWorldTransform().inverse().getRotation()*qq
	);
	frameInB = btTransform::getIdentity();
	frameInB.setRotation(
		body2.getWorldTransform().inverse().getRotation()*qq
	);
	
	m_joint = new btSliderConstraint(
		body1, 
		body2,
		frameInA, 
		frameInB, 
		true
	);