allow auxilary link to be used for gear btMultiBodyGearConstraint.

data/racecar/racecar_differential.urdf

@@ -20,12 +20,12 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/chassis_differential.STL"/>
+        <mesh scale="1 1 1 1" filename="meshes/chassis_differential.STL"/>
       </geometry>
       <material name="blue"/>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.1477 0 0"/>
+      <origin rpy="0 0 0" xyz="0.2 0 0."/>
       <mass value="4.0"/>
       <inertia ixx="0.010609" ixy="0" ixz="0" iyy="0.050409" iyz="0" izz="0.05865"/>
     </inertial>
@@ -33,14 +33,14 @@
   <!-- Add the left rear wheel with its joints and tranmissions -->
   <link name="left_rear_wheel">
     <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value=".5"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 -0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -66,14 +66,14 @@
   <!-- Add the right rear wheel with its joints and tranmissions -->
   <link name="right_rear_wheel">
 <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value=".5"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -163,14 +163,14 @@
   <!-- Add the left front wheel with its joints and tranmissions -->
   <link name="left_front_wheel">
 <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value=".8"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 -0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -207,14 +207,14 @@
   <!-- Add the left front wheel with its joints and tranmissions -->
   <link name="right_front_wheel">
 <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value="0.8"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -533,7 +533,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.5637"/>
+      <mass value="0.0637"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -573,7 +573,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -612,7 +612,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -650,7 +650,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -686,7 +686,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1949,6 +1949,20 @@ int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHa
 	return 0;
 }
 
+int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_USER_CONSTRAINT);
+	b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
+	
+	command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK;
+	command->m_userConstraintArguments.m_gearAuxLink = gearAuxLink;
+
+	return 0;
+}
+
+
 b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
 {
 	 PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -107,7 +107,7 @@ int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHan
 int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
 int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
 int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
-
+int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink);
 
 b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6450,6 +6450,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
 								}
+								if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
+								{
+									userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink);
+								}
 
 							}
 							if (userConstraintPtr->m_rbConstraint)

examples/SharedMemory/SharedMemoryCommands.h

@@ -645,6 +645,8 @@ enum EnumUserConstraintFlags
 	USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
 	USER_CONSTRAINT_REQUEST_INFO=64,
 	USER_CONSTRAINT_CHANGE_GEAR_RATIO=128,	
+	USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,	
+
 };
 
 enum EnumBodyChangeFlags

examples/SharedMemory/SharedMemoryPublic.h

@@ -235,6 +235,7 @@ struct b3UserConstraint
     double m_maxAppliedForce;
     int m_userConstraintUniqueId;
 	double m_gearRatio;
+	int m_gearAuxLink;
 
 };
 

examples/pybullet/examples/racecar_differential.py

@@ -12,10 +12,10 @@ useRealTimeSim = 1
 #for video recording (works best on Mac and Linux, not well on Windows)
 #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
 p.setRealTimeSimulation(useRealTimeSim) # either this
-#p.loadURDF("plane.urdf")
+p.loadURDF("plane.urdf")
-p.loadSDF("stadium.sdf")
+#p.loadSDF("stadium.sdf")
 
-car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True)
+car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
 for i in range (p.getNumJoints(car)):
 	print (p.getJointInfo(car,i))
 for wheel in range(p.getNumJoints(car)):
@@ -26,7 +26,6 @@ wheels = [8,15]
 print("----------------")
 
 #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
-
 c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
 p.changeConstraint(c,gearRatio=1, maxForce=10000)
 
@@ -39,12 +38,17 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000)
 c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
 p.changeConstraint(c,gearRatio=1, maxForce=10000)
 
-c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
-p.changeConstraint(c,gearRatio=-1, maxForce=10000)
 
 c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
 p.changeConstraint(c,gearRatio=-1, maxForce=10000)
 
+c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000)
+c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
 
 
 steering = [0,2]

examples/pybullet/pybullet.c

@@ -4804,11 +4804,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 
 static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
 {
-	static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL};
+	static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL};
 	int userConstraintUniqueId = -1;
 	b3SharedMemoryCommandHandle commandHandle;
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;
+	int gearAuxLink = -1;
 	int physicsClientId = 0;
 	b3PhysicsClientHandle sm = 0;
 	PyObject* jointChildPivotObj = 0;
@@ -4817,7 +4818,7 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
 	double jointChildFrameOrn[4];
 	double maxForce = -1;
 	double gearRatio = 0;
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -4847,6 +4848,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
 	{
 		b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio);
 	}
+	if (gearAuxLink>=0)
+	{
+		b3InitChangeUserConstraintSetGearAuxLink(commandHandle,gearAuxLink);
+	}
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 	statusType = b3GetStatusType(statusHandle);
 	Py_INCREF(Py_None);

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

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

src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

@@ -22,7 +22,8 @@ subject to the following restrictions:
 
 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)
+	m_gearRatio(1),
+	m_gearAuxLink(-1)
 {
    
 }
@@ -121,11 +122,18 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
         int dof = 0;
         btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
         btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
-
+		btScalar auxVel = 0;
+		
+		if (m_gearAuxLink>=0)
+		{
+			auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
+		}
+		currentVelocity += auxVel;
+			
 		//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
         //btScalar velocityError = (m_desiredVelocity - currentVelocity);
 
-        btScalar desiredRelativeVelocity =   0;
+        btScalar desiredRelativeVelocity =   auxVel;
     
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
 

src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h

@@ -31,10 +31,11 @@ protected:
   btMatrix3x3     m_frameInA;
   btMatrix3x3     m_frameInB;
 	btScalar			m_gearRatio;
+	int			m_gearAuxLink;
 	
 public:
 
-	btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	//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();
@@ -97,6 +98,10 @@ public:
 	{
 		m_gearRatio = gearRatio;
 	}
+	virtual void setGearAuxLink(int gearAuxLink)
+	{
+		m_gearAuxLink = gearAuxLink;
+	}
 
 };