add option for rigid body/typed constraint to set target velocity

compare joint feedback between multi body and rigid body. initial results are promising (not exactly the same, but reasonably close)

data/sphere2.urdf

@@ -43,5 +43,31 @@
     <child link="childA"/>
     <origin xyz="0 0 1.0"/>
   </joint>
+ <link name="childB">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+      <mass value="10.0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+      <geometry>
+        <sphere radius="0.5"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+      <geometry>
+        <sphere radius="0.5"/>
+      </geometry>
+    </collision>
+  </link>
+ <joint name="joint_childA_childB" type="continuous">
+    <parent link="childA"/>
+    <child link="childB"/>
+<axis xyz="0 1 0"/>
+    <origin xyz="0 0 0.5"/>
+  </joint>
+
 </robot>
 

examples/Constraints/TestHingeTorque.cpp

@@ -5,8 +5,8 @@
 #include "../CommonInterfaces/CommonParameterInterface.h"
 
 short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter);
-short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::StaticFilter|btBroadphaseProxy::CharacterFilter));
+short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::CharacterFilter));
-
+static btScalar radius(0.2);
 
 struct TestHingeTorque : public CommonRigidBodyBase
 {
@@ -50,7 +50,7 @@ TestHingeTorque::~ TestHingeTorque()
 
 void TestHingeTorque::stepSimulation(float deltaTime)
 {
-    if (m_once)
+    if (0)//m_once)
     {
         m_once=false;
         btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
@@ -63,7 +63,11 @@ void TestHingeTorque::stepSimulation(float deltaTime)
         
     }
     
-    m_dynamicsWorld->stepSimulation(1./60,0);
+    m_dynamicsWorld->stepSimulation(1./240,0);
+	
+	static int count = 0;
+	if ((count& 0x0f)==0)
+	{
 		btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
 		
 		b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
@@ -73,6 +77,7 @@ void TestHingeTorque::stepSimulation(float deltaTime)
 		
 		btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
     
+	
 		b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
 				 child->getAngularVelocity()[1],
 
@@ -80,13 +85,9 @@ void TestHingeTorque::stepSimulation(float deltaTime)
 		
 		for (int i=0;i<m_jointFeedback.size();i++)
 		{
-		b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n", 
+			b3Printf("Applied force B:(%f,%f,%f), torque B:(%f,%f,%f)\n", 
-			m_jointFeedback[i]->m_appliedForceBodyA.x(),
+
-			m_jointFeedback[i]->m_appliedForceBodyA.y(),
+		
-			m_jointFeedback[i]->m_appliedForceBodyA.z(),
-			m_jointFeedback[i]->m_appliedTorqueBodyA.x(),
-			m_jointFeedback[i]->m_appliedTorqueBodyA.y(),
-			m_jointFeedback[i]->m_appliedTorqueBodyA.z(),
 				m_jointFeedback[i]->m_appliedForceBodyB.x(),
 				m_jointFeedback[i]->m_appliedForceBodyB.y(),
 				m_jointFeedback[i]->m_appliedForceBodyB.z(),
@@ -94,6 +95,9 @@ void TestHingeTorque::stepSimulation(float deltaTime)
 				m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
 				m_jointFeedback[i]->m_appliedTorqueBodyB.z());
 		}
+	}
+	count++;
+
     //CommonRigidBodyBase::stepSimulation(deltaTime);
 }
 
@@ -101,10 +105,13 @@ void TestHingeTorque::stepSimulation(float deltaTime)
 
 void TestHingeTorque::initPhysics()
 {
-	m_guiHelper->setUpAxis(1);
+	int upAxis = 1;
+	m_guiHelper->setUpAxis(upAxis);
 
 	createEmptyDynamicsWorld();
-//    m_dynamicsWorld->setGravity(btVector3(0,0,0));
+	m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
+	
+    m_dynamicsWorld->setGravity(btVector3(0,-1,-10));
     
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	int mode = 	btIDebugDraw::DBG_DrawWireframe
@@ -115,7 +122,7 @@ void TestHingeTorque::initPhysics()
 
 	{ // create a door using hinge constraint attached to the world
         
-        int numLinks = 1;
+        int numLinks = 2;
         bool spherical = false;					//set it ot false -to use 1DoF hinges instead of 3DoF sphericals
         bool canSleep = false;
         bool selfCollide = false;
@@ -138,7 +145,9 @@ void TestHingeTorque::initPhysics()
         m_dynamicsWorld->removeRigidBody(base);
         base->setDamping(0,0);
         m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
-        btBoxShape* linkBox = new btBoxShape(linkHalfExtents);
+        btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
+		btSphereShape* linkSphere = new btSphereShape(radius);
+		
         btRigidBody* prevBody = base;
         
         for (int i=0;i<numLinks;i++)
@@ -148,10 +157,23 @@ void TestHingeTorque::initPhysics()
             
             linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0));
             
-            btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,linkBox);
+			btCollisionShape* colOb = 0;
+			
+			if (i==0)
+			{
+				colOb = linkBox1;
+			} else 
+			{
+				colOb = linkSphere;
+			}
+            btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,colOb);
             m_dynamicsWorld->removeRigidBody(linkBody);
             m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
             linkBody->setDamping(0,0);
+			btTypedConstraint* con = 0;
+			
+			if (i==0)
+			{
 				//create a hinge constraint
 				btVector3 pivotInA(0,-linkHalfExtents[1],0);
 				btVector3 pivotInB(0,linkHalfExtents[1],0);
@@ -161,17 +183,57 @@ void TestHingeTorque::initPhysics()
 				btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
 																 pivotInA,pivotInB,
 																 axisInA,axisInB,useReferenceA);
+				con = hinge;
+			} else
+			{
+				
+				btTransform pivotInA(btQuaternion::getIdentity(),btVector3(0, -radius, 0));						//par body's COM to cur body's COM offset
+				btTransform pivotInB(btQuaternion::getIdentity(),btVector3(0, radius, 0));							//cur body's COM to cur body's PIV offset
+				btGeneric6DofSpring2Constraint* fixed = new btGeneric6DofSpring2Constraint(*prevBody, *linkBody,
+																						   pivotInA,pivotInB);
+				fixed->setLinearLowerLimit(btVector3(0,0,0));
+				fixed->setLinearUpperLimit(btVector3(0,0,0));
+				fixed->setAngularLowerLimit(btVector3(0,0,0));
+				fixed->setAngularUpperLimit(btVector3(0,0,0));
+				
+				con = fixed;
+
+			}
+			btAssert(con);
+			if (con)
+			{
 				btJointFeedback* fb = new btJointFeedback();
 				m_jointFeedback.push_back(fb);
-			hinge->setJointFeedback(fb);
+				con->setJointFeedback(fb);
 
-            m_dynamicsWorld->addConstraint(hinge,true);
+				m_dynamicsWorld->addConstraint(con,true);
+			}
 			prevBody = linkBody;
             
         }
        
 	}
 	
+	if (1)
+	{
+		btVector3 groundHalfExtents(1,1,0.2);
+		groundHalfExtents[upAxis]=1.f;
+		btBoxShape* box = new btBoxShape(groundHalfExtents);
+		box->initializePolyhedralFeatures();
+		
+		btTransform start; start.setIdentity();
+		btVector3 groundOrigin(-0.4f, 3.f, 0.f);
+		btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
+		btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
+		
+		groundOrigin[upAxis] -=.5;
+		groundOrigin[2]-=0.6;
+		start.setOrigin(groundOrigin);
+	//	start.setRotation(groundOrn);
+		btRigidBody* body =  createRigidBody(0,start,box);
+		body->setFriction(0);
+		
+	}
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
 

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -63,11 +63,18 @@ struct ImportUrdfInternalData
     ImportUrdfInternalData()
     :m_numMotors(0)
     {
+		for (int i=0;i<MAX_NUM_MOTORS;i++)
+		{
+			m_jointMotors[i] = 0;
+			m_generic6DofJointMotors[i] = 0;
+		}
     }
 
     btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
     btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
+	btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
     int m_numMotors;
+
 };
 
 
@@ -91,7 +98,6 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 	} else
 	{
 		gFileNameArray.clear();
-		gFileNameArray.push_back("r2d2.urdf");
 		
 
 
@@ -116,6 +122,12 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 			fclose(f);
 		}
 		
+		if (gFileNameArray.size()==0)
+		{
+			gFileNameArray.push_back("r2d2.urdf");
+
+		}
+
 		int numFileNames = gFileNameArray.size();
 
 		if (count>=numFileNames)
@@ -175,7 +187,7 @@ void ImportUrdfSetup::initPhysics()
 
 
 	btVector3 gravity(0,0,0);
-	gravity[upAxis]=-9.8;
+	//gravity[upAxis]=-9.8;
 
 	m_dynamicsWorld->setGravity(gravity);
 
@@ -210,9 +222,7 @@ void ImportUrdfSetup::initPhysics()
 			if (m_useMultiBody)
 			{
 
-
+				//create motors for each btMultiBody joint
-
-				//create motors for each joint
 
 				for (int i=0;i<mb->getNumLinks();i++)
 				{
@@ -236,6 +246,7 @@ void ImportUrdfSetup::initPhysics()
 							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 							float maxMotorImpulse = 0.1f;
 							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
+							//motor->setMaxAppliedImpulse(0);
 							m_data->m_jointMotors[m_data->m_numMotors]=motor;
 							m_dynamicsWorld->addMultiBodyConstraint(motor);
 							m_data->m_numMotors++;
@@ -243,6 +254,44 @@ void ImportUrdfSetup::initPhysics()
 					}
 
 				}
+			} else
+			{
+				if (1)
+				{
+					//create motors for each generic joint
+					for (int i=0;i<creation.getNum6DofConstraints();i++)
+					{
+						btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
+						if (c->getUserConstraintPtr())
+						{
+							GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
+							if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) || 
+								(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
+								(jointInfo->m_urdfJointType ==URDFContinuousJoint))
+							{
+								int urdfLinkIndex = jointInfo->m_urdfIndex;
+								std::string jointName = u2b.getJointName(urdfLinkIndex);
+								char motorName[1024];
+								sprintf(motorName,"%s q'", jointName.c_str());
+								btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
+
+								*motorVel = 0.f;
+								SliderParams slider(motorName,motorVel);
+								slider.m_minVal=-4;
+								slider.m_maxVal=4;
+								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+								m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
+								bool motorOn = true;
+								c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
+								c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
+								c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
+								
+								m_data->m_numMotors++;
+							}
+						}
+					}
+				}
+				
 			}
 		}
 
@@ -285,9 +334,18 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
 	if (m_dynamicsWorld)
 	{
         for (int i=0;i<m_data->m_numMotors;i++)
+        {
+			if (m_data->m_jointMotors[i])
 			{
 				m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
 			}
+			if (m_data->m_generic6DofJointMotors[i])
+			{
+				GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
+				m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
+				//jointInfo->
+			}
+        }
 
 		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
 		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);

examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h

@@ -21,6 +21,13 @@ public:
     
     virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0;
     
+	virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+														const btVector3&	jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
+    virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+														const btVector3&	jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
+
+    virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0;
+   
     virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
     
     virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -9,7 +9,7 @@
 
 #include "btBulletDynamicsCommon.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
-
+#include "URDFJointTypes.h"
 
 MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
 	:m_guiHelper(guiHelper),
@@ -42,12 +42,147 @@ class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider
 }
 
 
+
+
 class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) 
 {
     btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
+	
     return c;
 }
 
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+													const btVector3&	jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
+{
+	int rotateOrder=0;
+	btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA , rbB, offsetInA, offsetInB, rotateOrder);
+	 //todo(erwincoumans) for now, we only support principle axis along X, Y or Z
+	int principleAxis = jointAxisInJointSpace.closestAxis();
+
+	GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
+	userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
+	userInfo->m_jointAxisIndex = principleAxis;
+
+	userInfo->m_urdfJointType = URDFPrismaticJoint;
+	userInfo->m_lowerJointLimit = jointLowerLimit;
+	userInfo->m_upperJointLimit = jointUpperLimit;
+	userInfo->m_urdfIndex = urdfLinkIndex;
+	dof6->setUserConstraintPtr(userInfo);
+
+	
+	switch (principleAxis)
+	{
+		case 0:
+		{
+			dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
+			dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
+			break;
+		}
+		case 1:
+		{
+			dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
+			dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
+			break;
+		}
+		case 2:
+		default:
+		{
+			dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
+			dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
+		}
+	};
+
+	dof6->setAngularLowerLimit(btVector3(0,0,0));
+	dof6->setAngularUpperLimit(btVector3(0,0,0));
+	m_6DofConstraints.push_back(dof6);
+	return dof6;
+}
+
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+													const btVector3&	jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
+{
+	btGeneric6DofSpring2Constraint* dof6 = 0;
+
+	 //only handle principle axis at the moment,
+    //@todo(erwincoumans) orient the constraint for non-principal axis
+    int principleAxis = jointAxisInJointSpace.closestAxis();
+    switch (principleAxis)
+    {
+        case 0:
+        {
+            dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_ZYX);
+            dof6->setLinearLowerLimit(btVector3(0,0,0));
+            dof6->setLinearUpperLimit(btVector3(0,0,0));
+
+            dof6->setAngularUpperLimit(btVector3(-1,0,0));
+            dof6->setAngularLowerLimit(btVector3(1,0,0));
+
+            break;
+        }
+        case 1:
+        {
+            dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XZY);
+            dof6->setLinearLowerLimit(btVector3(0,0,0));
+            dof6->setLinearUpperLimit(btVector3(0,0,0));
+
+            dof6->setAngularUpperLimit(btVector3(0,-1,0));
+            dof6->setAngularLowerLimit(btVector3(0,1,0));
+            break;
+        }
+        case 2:
+        default:
+        {
+            dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XYZ);
+            dof6->setLinearLowerLimit(btVector3(0,0,0));
+            dof6->setLinearUpperLimit(btVector3(0,0,0));
+
+            dof6->setAngularUpperLimit(btVector3(0,0,-1));
+            dof6->setAngularLowerLimit(btVector3(0,0,0));
+
+        }
+    };
+
+	GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
+	userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
+	userInfo->m_jointAxisIndex = 3+principleAxis;
+
+	if (jointLowerLimit > jointUpperLimit)
+	{
+		userInfo->m_urdfJointType = URDFContinuousJoint;
+	} else
+	{
+		userInfo->m_urdfJointType = URDFRevoluteJoint;
+		userInfo->m_lowerJointLimit = jointLowerLimit;
+		userInfo->m_upperJointLimit = jointUpperLimit;
+		userInfo->m_urdfIndex = urdfLinkIndex;
+	}
+	dof6->setUserConstraintPtr(userInfo);
+	m_6DofConstraints.push_back(dof6);
+	return dof6;
+}
+
+
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB)
+{
+	btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB);
+	
+	GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
+	userInfo->m_urdfIndex = urdfLinkIndex;
+	userInfo->m_urdfJointType = URDFFixedJoint;
+
+	dof6->setUserConstraintPtr(userInfo);
+
+    dof6->setLinearLowerLimit(btVector3(0,0,0));
+    dof6->setLinearUpperLimit(btVector3(0,0,0));
+
+    dof6->setAngularLowerLimit(btVector3(0,0,0));
+    dof6->setAngularUpperLimit(btVector3(0,0,0));
+
+	return dof6;
+}
+   
+
+
 void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex) 
 {
 //    m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -8,6 +8,17 @@
 struct GUIHelperInterface;
 class btMultiBody;
 
+struct GenericConstraintUserInfo
+{
+	int	m_urdfIndex;
+	int m_urdfJointType;
+	btVector3 m_jointAxisInJointSpace;
+	int		m_jointAxisIndex;
+	btScalar m_lowerJointLimit;
+	btScalar m_upperJointLimit;
+
+};
+
 class MyMultiBodyCreator : public MultiBodyCreationInterface
 {
 
@@ -16,6 +27,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
 	struct GUIHelperInterface* m_guiHelper;
 
 	
+	btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
 
 public:
 
@@ -37,12 +49,28 @@ public:
     
     virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0);
     
+	virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+														const btVector3&	jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
+    virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+														const btVector3&	jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
+
+    virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB);
+   
     virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
     
     virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
 
 	btMultiBody* getBulletMultiBody();
 	
+	int	getNum6DofConstraints() const
+	{
+		return m_6DofConstraints.size();
+	}
+	
+	btGeneric6DofSpring2Constraint*	get6DofConstraint(int index)
+	{
+		return m_6DofConstraints[index];
+	}
 };
 
 #endif //MY_MULTIBODY_CREATOR

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -296,19 +296,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                     {
                         printf("Fixed joint\n");
 						
-                        btMatrix3x3 rm(offsetInA.getBasis());
+						btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
-                        btScalar y,p,r;
-                        rm.getEulerZYX(y,p,r);
-                        printf("y=%f,p=%f,r=%f\n", y,p,r);
-
-                        //we could also use btFixedConstraint but it has some issues
-                        btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
-
-                        dof6->setLinearLowerLimit(btVector3(0,0,0));
-                        dof6->setLinearUpperLimit(btVector3(0,0,0));
-
-                        dof6->setAngularLowerLimit(btVector3(0,0,0));
-                        dof6->setAngularUpperLimit(btVector3(0,0,0));
                        
                         if (enableConstraints)
                             world1->addConstraint(dof6,true);
@@ -330,51 +318,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
 
                     } else
                     {
-                        //only handle principle axis at the moment,
-                        //@todo(erwincoumans) orient the constraint for non-principal axis
-                        int principleAxis = jointAxisInJointSpace.closestAxis();
-                        switch (principleAxis)
-                        {
-                            case 0:
-                            {
-                                btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
-                                dof6->setLinearLowerLimit(btVector3(0,0,0));
-                                dof6->setLinearUpperLimit(btVector3(0,0,0));
 
-                                dof6->setAngularUpperLimit(btVector3(-1,0,0));
+						btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
-                                dof6->setAngularLowerLimit(btVector3(1,0,0));
 
 						if (enableConstraints)
                                     world1->addConstraint(dof6,true);
-                                break;
-                            }
-                            case 1:
-                            {
-                                btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
-                                dof6->setLinearLowerLimit(btVector3(0,0,0));
-                                dof6->setLinearUpperLimit(btVector3(0,0,0));
-
-                                dof6->setAngularUpperLimit(btVector3(0,-1,0));
-                                dof6->setAngularLowerLimit(btVector3(0,1,0));
-
-                                if (enableConstraints)
-                                    world1->addConstraint(dof6,true);
-                                break;
-                            }
-                            case 2:
-                            default:
-                            {
-                                btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
-                                dof6->setLinearLowerLimit(btVector3(0,0,0));
-                                dof6->setLinearUpperLimit(btVector3(0,0,0));
-
-                                dof6->setAngularUpperLimit(btVector3(0,0,-1));
-                                dof6->setAngularLowerLimit(btVector3(0,0,0));
-
-                                if (enableConstraints)
-                                    world1->addConstraint(dof6,true);
-                            }
-                        };
                         printf("Revolute/Continuous joint\n");
                     }
                     break;
@@ -396,33 +344,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
 
                     } else
                     {
-                        btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
-                        //todo(erwincoumans) for now, we only support principle axis along X, Y or Z
-                        int principleAxis = jointAxisInJointSpace.closestAxis();
-                        switch (principleAxis)
-                        {
-                            case 0:
-                            {
-                                dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
-                                dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
-                                break;
-                            }
-                            case 1:
-                            {
-                                dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
-                                dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
-                                break;
-                            }
-                            case 2:
-                            default:
-                            {
-                                dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
-                                dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
-                            }
-                        };
                         
-                        dof6->setAngularLowerLimit(btVector3(0,0,0));
+						btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
-                        dof6->setAngularUpperLimit(btVector3(0,0,0));
+						
+                       
                         if (enableConstraints)
                             world1->addConstraint(dof6,true);
 

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -46,9 +46,12 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
 
 }
 
+///this is a temporary global, until we determine if we need the option or not
+extern  bool gJointFeedbackInWorldSpace;
 void TestJointTorqueSetup::initPhysics()
 {
     int upAxis = 1;
+	gJointFeedbackInWorldSpace = true;
 	m_guiHelper->setUpAxis(upAxis);
 
     btVector4 colors[4] =
@@ -90,7 +93,9 @@ void TestJointTorqueSetup::initPhysics()
             groundOrigin[upAxis] -=.5;
 			groundOrigin[2]-=0.6;
             start.setOrigin(groundOrigin);
+			btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
 		
+		//	start.setRotation(groundOrn);
             btRigidBody* body =  createRigidBody(0,start,box);
 			body->setFriction(0);
             btVector4 color = colors[curColor];
@@ -101,7 +106,7 @@ void TestJointTorqueSetup::initPhysics()
 
     {
         bool floating = false;
-        bool damping = true;
+        bool damping = false;
         bool gyro = false;
         int numLinks = 2;
         bool spherical = false;					//set it ot false -to use 1DoF hinges instead of 3DoF sphericals
@@ -156,7 +161,14 @@ void TestJointTorqueSetup::initPhysics()
 			//	linkMass= 1000;
 			btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
 
-			btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//new btSphereShape(linkHalfExtents[0]);
+			btCollisionShape* shape = 0;
+			if (i==0)
+			{
+				shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
+			} else
+			{
+				shape = new btSphereShape(radius);
+			}
 			shape->calculateLocalInertia(linkMass, linkInertiaDiag);
 			delete shape;
 
@@ -223,10 +235,8 @@ void TestJointTorqueSetup::initPhysics()
             mbC->setAngularDamping(0.9f);
         }
         //
-        btVector3 gravity(0,0,0);
+    	m_dynamicsWorld->setGravity(btVector3(0,-1,-10));
-        gravity[upAxis] = -9.81;
+
-		gravity[0] = 0;
-        m_dynamicsWorld->setGravity(gravity);
         //////////////////////////////////////////////
         if(0)//numLinks > 0)
         {
@@ -288,7 +298,7 @@ void TestJointTorqueSetup::initPhysics()
                 btVector3 color(0.0,0.0,0.5);
                 m_guiHelper->createCollisionObjectGraphicsObject(col,color);
 
-                col->setFriction(friction);
+//                col->setFriction(friction);
                 pMultiBody->setBaseCollider(col);
 
             }
@@ -330,7 +340,7 @@ void TestJointTorqueSetup::initPhysics()
             tr.setOrigin(posr);
             tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
             col->setWorldTransform(tr);
-            col->setFriction(friction);
+     //       col->setFriction(friction);
 			bool isDynamic = 1;//(linkMass > 0);
 			short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
 			short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
@@ -355,7 +365,7 @@ void TestJointTorqueSetup::initPhysics()
 void TestJointTorqueSetup::stepSimulation(float deltaTime)
 {
 	//m_multiBody->addLinkForce(0,btVector3(100,100,100));
-    if (1)//m_once)
+    if (0)//m_once)
     {
        m_once=false;
         m_multiBody->addJointTorque(0, 10.0);
@@ -364,9 +374,12 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
         b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
     }
     
-    m_dynamicsWorld->stepSimulation(1./60,0);
+    m_dynamicsWorld->stepSimulation(1./240,0);
+
+	static int count = 0;
+	if ((count& 0x0f)==0)
+	{
 
-	if (1)
 	for (int i=0;i<m_jointFeedbacks.size();i++)
 	{
 			b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
@@ -382,6 +395,8 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
 		);
 
 	}
+	}
+	count++;
 
 
 	/*

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -30,6 +30,9 @@
 #include "Bullet3Common/b3Logging.h"
 // #define INCLUDE_GYRO_TERM 
 
+///todo: determine if we need this option. If so, make a proper API, otherwise delete the global
+bool gJointFeedbackInWorldSpace = false;
+
 namespace {
     const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
     const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
@@ -1026,11 +1029,16 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 
 		
 			m_internalNeedsJointFeedback = true;
-			m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
+			//m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
-			m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
+			if (gJointFeedbackInWorldSpace)
-			m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+			{
-			//m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
+				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
-			//m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+			} else
+			{
+				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
+				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+			}	
 	}
 
     }

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -616,7 +616,6 @@ private:
     btAlignedObjectArray<btVector3> m_vectorBuf;
     btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
 
-    //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
 
 	btMatrix3x3 m_cachedInertiaTopLeft;
 	btMatrix3x3 m_cachedInertiaTopRight;

src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h

@@ -22,7 +22,6 @@ subject to the following restrictions:
 struct btMultiBodyJointFeedback
 {
 	btSpatialForceVector	m_reactionForces;
-	btSymmetricSpatialDyad	m_spatialInertia;
 };
 
 #endif //BT_MULTIBODY_JOINT_FEEDBACK_H