Merge remote-tracking branch 'upstream/master'

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -143,6 +143,34 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
 
 }
 
+void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisionObject* col)
+{
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION) != 0)
+	{
+		col->setFriction(contactInfo.m_lateralFriction);
+	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_RESTITUTION) != 0)
+	{
+		col->setRestitution(contactInfo.m_restitution);
+	}
+
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION) != 0)
+	{
+		col->setRollingFriction(contactInfo.m_rollingFriction);
+	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION) != 0)
+	{
+		col->setSpinningFriction(contactInfo.m_spinningFriction);
+	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING) != 0)
+	{
+		col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping);
+	}
+}
+
+
+
+
 void ConvertURDF2BulletInternal(
     const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
     URDF2BulletCachedData& cache, int urdfLinkIndex,
@@ -258,11 +286,18 @@ void ConvertURDF2BulletInternal(
 
             world1->addRigidBody(body);
 
+
             compoundShape->setUserIndex(graphicsIndex);
 
+			URDFLinkContactInfo contactInfo;
+			u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
+
+			processContactParameters(contactInfo, body);
             creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
             cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
             
+
+
             //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
         } else
         {
@@ -413,22 +448,7 @@ void ConvertURDF2BulletInternal(
 				URDFLinkContactInfo contactInfo;
 				u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
 
-				if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
+				processContactParameters(contactInfo, col);
-				{
-					col->setFriction(contactInfo.m_lateralFriction);
-				}
-				if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0)
-				{
-					col->setRollingFriction(contactInfo.m_rollingFriction);
-				}
-                if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION)!=0)
-                {
-                    col->setSpinningFriction(contactInfo.m_spinningFriction);
-                }
-				if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0)
-				{
-				    col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping);
-				}
 
                 if (mbLinkIndex>=0) //???? double-check +/- 1
                 {

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -22,6 +22,7 @@ enum URDF_LinkContactFlags
 	URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
     URDF_CONTACT_HAS_ROLLING_FRICTION=32,
     URDF_CONTACT_HAS_SPINNING_FRICTION=64,
+	URDF_CONTACT_HAS_RESTITUTION=128,
 
 };
 
@@ -30,6 +31,7 @@ struct URDFLinkContactInfo
 	btScalar m_lateralFriction;
 	btScalar m_rollingFriction;
     btScalar m_spinningFriction;
+	btScalar m_restitution;
     btScalar m_inertiaScaling;
 	btScalar m_contactCfm;
 	btScalar m_contactErp;
@@ -42,6 +44,7 @@ struct URDFLinkContactInfo
 		:m_lateralFriction(0.5),
 		m_rollingFriction(0),
         m_spinningFriction(0),
+		m_restitution(0),
         m_inertiaScaling(1),
 		m_contactCfm(0),
 		m_contactErp(0),

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -672,6 +672,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
                   }
               }
           }
+
+		  {
+			  TiXmlElement *restitution_xml = ci->FirstChildElement("restitution");
+			  if (restitution_xml)
+			  {
+				  if (m_parseSDF)
+				  {
+					  link.m_contactInfo.m_restitution = urdfLexicalCast<double>(restitution_xml->GetText());
+					  link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION;
+				  }
+				  else
+				  {
+					  if (!restitution_xml->Attribute("value"))
+					  {
+						  logger->reportError("Link/contact: restitution element must have value attribute");
+						  return false;
+					  }
+
+					  link.m_contactInfo.m_restitution = urdfLexicalCast<double>(restitution_xml->Attribute("value"));
+					  link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION;
+
+				  }
+			  }
+		  }
+
           {
               TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction");
               if (spinning_xml)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -317,6 +317,26 @@ int     b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHan
 	return 0;
 }
 
+int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+
+	command->m_physSimParamArgs.m_useSplitImpulse = useSplitImpulse;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE;
+	return 0;
+}
+
+int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+
+	command->m_physSimParamArgs.m_splitImpulsePenetrationThreshold = splitImpulsePenetrationThreshold;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD;
+	return 0;
+}
+
 int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -163,6 +163,9 @@ int	b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle
 int	b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
 int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
 int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
+int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
+int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
+
 
 //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
 //Use at own risk: magic things may or my not happen when calling this API

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2338,6 +2338,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					{
 						m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
 					}
+
+					if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE)
+					{
+						m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse;
+					}
+					if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD)
+					{
+						m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold;
+					}
+					
+
                     if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
                     {
                         m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
@@ -2363,27 +2374,50 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId;
 					InteralBodyData* body = m_data->getHandle(bodyUniqueId);
 
+					btVector3 baseLinVel(0, 0, 0);
+					btVector3 baseAngVel(0, 0, 0);
+
+					if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
+					{
+						baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
+							clientCmd.m_initPoseArgs.m_initialStateQdot[1],
+							clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
+					}
+					if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
+					{
+						baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
+							clientCmd.m_initPoseArgs.m_initialStateQdot[4],
+							clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
+					}
+					btVector3 basePos(0, 0, 0);
+					if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
+					{
+						basePos = btVector3(
+							clientCmd.m_initPoseArgs.m_initialStateQ[0],
+							clientCmd.m_initPoseArgs.m_initialStateQ[1],
+							clientCmd.m_initPoseArgs.m_initialStateQ[2]);
+					}
+					btQuaternion baseOrn(0, 0, 0, 1);
+					if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
+					{
+						baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3],
+							clientCmd.m_initPoseArgs.m_initialStateQ[4],
+							clientCmd.m_initPoseArgs.m_initialStateQ[5],
+							clientCmd.m_initPoseArgs.m_initialStateQ[6]);
+					}
 					if (body && body->m_multiBody)
 					{
 						btMultiBody* mb = body->m_multiBody;
-						btVector3 baseLinVel(0, 0, 0);
+						
-						btVector3 baseAngVel(0, 0, 0);
 
 
 						if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
 						{
-							baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
-								clientCmd.m_initPoseArgs.m_initialStateQdot[1],
-								clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
 							mb->setBaseVel(baseLinVel);
-
 						}
 
 						if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
 						{
-							baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
-								clientCmd.m_initPoseArgs.m_initialStateQdot[4],
-								clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
 							mb->setBaseOmega(baseAngVel);
 						}
 
@@ -2396,10 +2430,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                     clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
 
 							mb->setBaseVel(baseLinVel);
-							mb->setBasePos(btVector3(
+							mb->setBasePos(basePos);
-									clientCmd.m_initPoseArgs.m_initialStateQ[0],
-									clientCmd.m_initPoseArgs.m_initialStateQ[1],
-									clientCmd.m_initPoseArgs.m_initialStateQ[2]));
 						}
 						if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
 						{
@@ -2409,10 +2440,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                     clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
 
 							mb->setBaseOmega(baseAngVel);
-							btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
+							btQuaternion invOrn(baseOrn);
-									clientCmd.m_initPoseArgs.m_initialStateQ[4],
-									clientCmd.m_initPoseArgs.m_initialStateQ[5],
-									clientCmd.m_initPoseArgs.m_initialStateQ[6]);
 
 							mb->setWorldToBaseRot(invOrn.inverse());
 						}
@@ -2441,6 +2469,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                         mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
 
                         
+					}
+
+					if (body && body->m_rigidBody)
+					{
+						if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
+						{
+							body->m_rigidBody->setLinearVelocity(baseLinVel);
+						}
+						if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
+						{
+							body->m_rigidBody->setAngularVelocity(baseAngVel);
+						}
+						
+						if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
+						{
+							body->m_rigidBody->getWorldTransform().setOrigin(basePos);
+							body->m_rigidBody->setLinearVelocity(baseLinVel);
+						}
+
+						if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
+						{
+							body->m_rigidBody->getWorldTransform().setRotation(baseOrn);
+							body->m_rigidBody->setAngularVelocity(baseAngVel);
+						}
+
 					}
 
 					SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -3114,11 +3167,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
                     {
                         InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
+						bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0);
+
                         if (body && body->m_multiBody)
                         {
                             btMultiBody* mb = body->m_multiBody;
-                            bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
+                         
-
                             if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
                             {
                                 btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
@@ -3166,6 +3220,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 }
                             }
                         }
+
+						if (body && body->m_rigidBody)
+						{
+							btRigidBody* rb = body->m_rigidBody;
+							if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
+							{
+								btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
+									clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
+									clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
+								btVector3 positionLocal(
+									clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
+									clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
+									clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);
+
+								btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis()*forceLocal;
+								btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis()*positionLocal;
+								rb->applyForce(forceWorld, relPosWorld);
+
+							}
+
+							if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0)
+							{
+								btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
+									clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
+									clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
+
+								btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis()*torqueLocal;
+								rb->applyTorque(torqueWorld);
+							}
+						}
                     }
 
                     SharedMemoryStatus& serverCmd =serverStatusOut;

examples/SharedMemory/SharedMemoryCommands.h

@@ -287,7 +287,9 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
 	SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
 	SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32,
-	SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64
+	SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64,
+	SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE=128,
+	SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256,
 };
 
 enum EnumLoadBunnyUpdateFlags
@@ -312,6 +314,8 @@ struct SendPhysicsSimulationParameters
 	int m_numSimulationSubSteps;
 	int m_numSolverIterations;
 	bool m_allowRealTimeSimulation;
+	int m_useSplitImpulse;
+	double m_splitImpulsePenetrationThreshold;
 	int m_internalSimFlags;
 	double m_defaultContactERP;
 };

examples/pybullet/pybullet.c

@@ -419,6 +419,62 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
 	return Py_None;
 }
 
+static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject *keywds)
+{
+	double fixedTimeStep = -1;
+	int numSolverIterations = -1;
+	int useSplitImpulse = -1;
+	double splitImpulsePenetrationThreshold = -1;
+
+	if (0 == sm) {
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", NULL };
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diid", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold))
+	{
+		return NULL;
+	}
+	{
+		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+		b3SharedMemoryStatusHandle statusHandle;
+
+		if (numSolverIterations >= 0)
+		{
+			b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
+		}
+		if (fixedTimeStep >= 0)
+		{
+			b3PhysicsParamSetTimeStep(command, fixedTimeStep);
+		}
+		if (useSplitImpulse >= 0)
+		{
+			b3PhysicsParamSetUseSplitImpulse(command,useSplitImpulse);
+		}
+		if (splitImpulsePenetrationThreshold >= 0)
+		{
+			b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
+		}
+
+		//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
+
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	}
+#if 0
+	b3SharedMemoryCommandHandle	b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
+	int	b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx, double gravy, double gravz);
+	int	b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
+	int	b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
+	int	b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
+	int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
+	int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
+#endif
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
 
 
 // Load a robot from a URDF file (universal robot description format)
@@ -426,8 +482,14 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
 // to position (0,0,1) with orientation(0,0,0,1)
 // els(x,y,z) or
 // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w)
-static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
+static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *keywds) 
+{
   int size = PySequence_Size(args);
+  static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase", NULL };
+
+  static char *kwlistBackwardCompatible4[] = { "fileName", "startPosX", "startPosY", "startPosZ", NULL };
+  static char *kwlistBackwardCompatible8[] = { "fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY","startOrnZ","startOrnW", NULL };
+
 
   int bodyIndex = -1;
   const char* urdfFileName = "";
@@ -439,24 +501,79 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
   double startOrnY = 0.0;
   double startOrnZ = 0.0;
   double startOrnW = 1.0;
+  int useMaximalCoordinates = 0;
+  int useFixedBase = 0;
+
+  int backwardsCompatibilityArgs = 0;
 
   if (0 == sm) {
     PyErr_SetString(SpamError, "Not connected to physics server.");
     return NULL;
   }
-  if (size == 1) {
+
-    if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL;
+  if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX,
+	  &startPosY, &startPosZ))
+  {
+	  backwardsCompatibilityArgs = 1;
   }
-  if (size == 4) {
+  else
-    if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY,
+  {
-                          &startPosZ))
+	  PyErr_Clear();
-      return NULL;
   }
-  if (size == 8) {
+
-    if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX,
+
-                          &startPosY, &startPosZ, &startOrnX, &startOrnY,
+  if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8,&urdfFileName, &startPosX,
-                          &startOrnZ, &startOrnW))
+	  &startPosY, &startPosZ, &startOrnX, &startOrnY,&startOrnZ, &startOrnW))
-      return NULL;
+  {
+	  backwardsCompatibilityArgs = 1;
+  }
+  else
+  {
+	  PyErr_Clear();
+  }
+
+  
+
+  if (!backwardsCompatibilityArgs)
+  {
+	  PyObject* basePosObj = 0;
+	  PyObject* baseOrnObj = 0;
+	  double basePos[3];
+	  double baseOrn[4];
+
+
+	  if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase))
+	  {
+
+		  return NULL;
+	  }
+	  else
+	  {
+		  if (basePosObj)
+		  {
+			  if (!pybullet_internalSetVectord(basePosObj, basePos))
+			  {
+				  PyErr_SetString(SpamError, "Cannot convert basePosition.");
+				  return NULL;
+			  }
+			  startPosX = basePos[0];
+			  startPosY = basePos[1];
+			  startPosZ = basePos[2];
+
+		  }
+		  if (baseOrnObj)
+		  {
+			  if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn))
+			  {
+				  PyErr_SetString(SpamError, "Cannot convert baseOrientation.");
+				  return NULL;
+			  }
+			  startOrnX = baseOrn[0];
+			  startOrnY = baseOrn[1];
+			  startOrnZ = baseOrn[2];
+			  startOrnW = baseOrn[3];
+		  }
+	  }
   }
 
   if (strlen(urdfFileName)) {
@@ -473,6 +590,15 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
     b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ);
     b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,
                                          startOrnZ, startOrnW);
+	if (useMaximalCoordinates)
+	{
+		b3LoadUrdfCommandSetUseMultiBody(command, 0);
+	}
+	if (useFixedBase)
+	{
+		b3LoadUrdfCommandSetUseFixedBase(command, 1);
+	}
+
     statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
     statusType = b3GetStatusType(statusHandle);
     if (statusType != CMD_URDF_LOADING_COMPLETED) {
@@ -3483,6 +3609,8 @@ static PyMethodDef SpamMethods[] = {
      "Set the amount of time to proceed at each call to stepSimulation. (unit "
      "is seconds, typically range is 0.01 or 0.001)"},
 
+
+
 	{"setDefaultContactERP",  pybullet_setDefaultContactERP, METH_VARARGS,
         "Set the amount of contact penetration Error Recovery Paramater "
         "(ERP) in each time step. \
@@ -3493,10 +3621,13 @@ static PyMethodDef SpamMethods[] = {
 	"Enable or disable real time simulation (using the real time clock,"
 	" RTC) in the physics server. Expects one integer argument, 0 or 1" },
 
+	{ "setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS,
+	"Set some internal physics engine parameter, such as cfm or erp etc." },
+
 	{ "setInternalSimFlags", pybullet_setInternalSimFlags, METH_VARARGS,
 	"This is for experimental purposes, use at own risk, magic may or not happen"},
 	
-    {"loadURDF", pybullet_loadURDF, METH_VARARGS,
+    {"loadURDF", (PyCFunction) pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS,
      "Create a multibody by loading a URDF file."},
 
     {"loadSDF", pybullet_loadSDF, METH_VARARGS,

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -378,7 +378,9 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         
         
         btScalar erp = infoGlobal.m_erp2;
-        if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		
+		//split impulse is not implemented yet for btMultiBody*
+		//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
         {
             erp = infoGlobal.m_erp;
         }
@@ -388,19 +390,23 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
         
-        if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		//split impulse is not implemented yet for btMultiBody*
+
+      //  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
         {
             //combine position and velocity into rhs
             solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
             solverConstraint.m_rhsPenetration = 0.f;
             
-        } else
+        } 
+		/*else
         {
             //split position and velocity into rhs and m_rhsPenetration
             solverConstraint.m_rhs = velocityImpulse;
             solverConstraint.m_rhsPenetration = penetrationImpulse;
         }
-        
+        */
+
         solverConstraint.m_cfm = 0.f;
         solverConstraint.m_lowerLimit = lowerLimit;
         solverConstraint.m_upperLimit = upperLimit;

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -528,19 +528,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		if(!isFriction)
 		{
-			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		//	if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				//combine position and velocity into rhs
 				solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
 				solverConstraint.m_rhsPenetration = 0.f;
 
-			} else
+			}
+		/*else
 			{
 				//split position and velocity into rhs and m_rhsPenetration
 				solverConstraint.m_rhs = velocityImpulse;
 				solverConstraint.m_rhsPenetration = penetrationImpulse;
 			}
-
+			*/
 			solverConstraint.m_lowerLimit = 0;
 			solverConstraint.m_upperLimit = 1e10f;
 		}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -384,7 +384,7 @@ btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBr
 	m_multiBodyConstraintSolver(constraintSolver)
 {
 	//split impulse is not yet supported for Featherstone hierarchies
-	getSolverInfo().m_splitImpulse = false;
+//	getSolverInfo().m_splitImpulse = false;
 	getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
 	m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
 }