implement 'mimic' joint constraint or 'gear' constraint for btMultiBody, add example in pybullet/examples/mimicJointConstraint.py

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -10,6 +10,10 @@
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
 #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h"
+
+
+
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -5759,7 +5763,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
 									btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
 									btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
-									if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
+									
+
+									
+									if (clientCmd.m_userConstraintArguments.m_jointType == eGearType)
+									{
+										if (childBody && childBody->m_multiBody)
+										{
+											if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex <childBody->m_multiBody->getNumLinks()))
+											{
+												btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
+												multibodyGear->setMaxAppliedImpulse(defaultMaxForce);
+												m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear);
+												InteralUserConstraintData userConstraintData;
+												userConstraintData.m_mbConstraint = multibodyGear;
+												int uid = m_data->m_userConstraintUIDGenerator++;
+												serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+												serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
+												userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
+												m_data->m_userConstraints.insert(uid,userConstraintData);
+												serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+											}
+									
+										}
+									}
+									else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
 									{
 										if (childBody && childBody->m_multiBody)
 										{
@@ -5959,9 +5987,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
 									userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp);
 								}
+
+								if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
+								{
+									userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
+								}
+
 							}
 							if (userConstraintPtr->m_rbConstraint)
 							{
+								if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
+								{
+									btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime;
+									userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
+									//userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp);
+								}
+
 								if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
 								{
 									if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE)

examples/pybullet/examples/mimicJointConstraint.py

@@ -1,16 +1,19 @@
-#mimic joint currently only works for 'maximal coordinate' rigid bodies
-#one way to use it would be to attach maximal coordinate rigid bodies to multibody links using
-#fixed constraints
+#a mimic joint can act as a gear between two joints
+#you can control the gear ratio in magnitude and sign (>0 reverses direction)
 
 import pybullet as p
 import time
 p.connect(p.GUI)
-wheelA = p.loadURDF("wheel.urdf",[0,0,0],useMaximalCoordinates=1)
-wheelB = p.loadURDF("wheel.urdf",[0,0,1],useMaximalCoordinates=1)
-c = p.createConstraint(wheelA,-1,wheelB,-1,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
-p.changeConstraint(c,gearRatio=-0.1)
+wheelA = p.loadURDF("wheel.urdf",[0,0,0])
+wheelB = p.loadURDF("wheel.urdf",[0,0,1])
+p.setJointMotorControl2(wheelA,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
+p.setJointMotorControl2(wheelB,0,p.VELOCITY_CONTROL,targetVelocity=1,force=1)
+
+c = p.createConstraint(wheelA,0,wheelB,0,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-0.1, maxForce=10000)
+
 p.setRealTimeSimulation(1)
 while(1):
 	time.sleep(0.01)
-p.removeConstraint(c)
+#p.removeConstraint(c)
 	

setup.py

@@ -253,6 +253,7 @@ sources = ["examples/pybullet/pybullet.c"]\
 +["src/BulletDynamics/Featherstone/btMultiBody.cpp"]\
 +["src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"]\
 +["src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"]\
++["src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"]\
 +["src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"]\
 +["src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"]\
 +["src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"]\

src/BulletDynamics/CMakeLists.txt

@@ -37,6 +37,7 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBodyFixedConstraint.cpp
 	Featherstone/btMultiBodySliderConstraint.cpp
 	Featherstone/btMultiBodyJointMotor.cpp
+	Featherstone/btMultiBodyGearConstraint.cpp
 	MLCPSolvers/btDantzigLCP.cpp
 	MLCPSolvers/btMLCPSolver.cpp
 	MLCPSolvers/btLemkeAlgorithm.cpp
@@ -98,6 +99,7 @@ SET(Featherstone_HDRS
 	Featherstone/btMultiBodyFixedConstraint.h
 	Featherstone/btMultiBodySliderConstraint.h
 	Featherstone/btMultiBodyJointMotor.h
+	Featherstone/btMultiBodyGearConstraint.h	
 )
 
 SET(MLCPSolvers_HDRS

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -183,6 +183,7 @@ public:
 
 	virtual void debugDraw(class btIDebugDraw* drawer)=0;
 
+	virtual void setGearRatio(btScalar ratio) {}
 };
 
 #endif //BT_MULTIBODY_CONSTRAINT_H

src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

@@ -0,0 +1,169 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyGearConstraint.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
+	m_gearRatio(1)
+{
+   
+}
+
+void btMultiBodyGearConstraint::finalizeMultiDof()
+{
+	
+	allocateJacobiansMultiDof();
+	
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
+{
+}
+
+
+int btMultiBodyGearConstraint::getIslandIdA() const
+{
+
+	if (m_bodyA)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		{
+			if (m_bodyA->getLink(i).m_collider)
+				return m_bodyA->getLink(i).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyGearConstraint::getIslandIdB() const
+{
+	if (m_bodyB)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+
+		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		{
+			col = m_bodyB->getLink(i).m_collider;
+			if (col)
+				return col->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+
+void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal)
+{
+    // only positions need to be updated -- data.m_jacobians and force
+    // directions were set in the ctor and never change.
+	
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+        finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
+	
+	if (m_maxAppliedImpulse==0.f)
+		return;
+	
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+	unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	jacobianA(0)[offsetA] = 1;
+	jacobianB(0)[offsetB] = m_gearRatio;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+	btScalar erp = infoGlobal.m_erp;
+	btScalar kp = 1;
+	btScalar kd = 1;
+	int numRows = getNumRows();
+
+	for (int row=0;row<numRows;row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+
+        int dof = 0;
+        btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+        btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+
+		//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
+        //btScalar velocityError = (m_desiredVelocity - currentVelocity);
+
+        btScalar desiredRelativeVelocity =   0;
+    
+		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
+
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
+					
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1=prismaticAxisInWorld;
+					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();					
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+			
+		}
+
+	}
+
+}
+

src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h

@@ -0,0 +1,103 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H
+#define BT_MULTIBODY_GEAR_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyGearConstraint : public btMultiBodyConstraint
+{
+protected:
+
+	btRigidBody*	m_rigidBodyA;
+	btRigidBody*	m_rigidBodyB;
+	btVector3		m_pivotInA;
+	btVector3		m_pivotInB;
+  btMatrix3x3     m_frameInA;
+  btMatrix3x3     m_frameInB;
+	btScalar			m_gearRatio;
+	
+public:
+
+	btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+	virtual ~btMultiBodyGearConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+
+    const btVector3& getPivotInA() const
+    {
+        return m_pivotInA;
+    }
+    
+    void setPivotInA(const btVector3& pivotInA)
+    {
+        m_pivotInA = pivotInA;
+    }
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+    
+    const btMatrix3x3& getFrameInA() const
+    {
+        return m_frameInA;
+    }
+    
+    void setFrameInA(const btMatrix3x3& frameInA)
+    {
+        m_frameInA = frameInA;
+    }
+    
+    const btMatrix3x3& getFrameInB() const
+    {
+        return m_frameInB;
+    }
+    
+    virtual void setFrameInB(const btMatrix3x3& frameInB)
+    {
+        m_frameInB = frameInB;
+    }
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+	
+	virtual void setGearRatio(btScalar gearRatio)
+	{
+		m_gearRatio = gearRatio;
+	}
+
+};
+
+#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H