Merge pull request #3351 from erwincoumans/master

PyBullet: expose useMultiBody argument for loadMJCF command, expose sleeping threshold

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -33,6 +33,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
 {
 	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
 	rbci.m_startWorldTransform = initialWorldTrans;
+	btScalar sleep_threshold = btScalar(0.22360679775);//sqrt(0.05) to be similar to btMultiBody (SLEEP_THRESHOLD)
+	rbci.m_angularSleepingThreshold = sleep_threshold;
+	rbci.m_linearSleepingThreshold = sleep_threshold;
+	
 	btRigidBody* body = new btRigidBody(rbci);
 	if (m_rigidBody == 0)
 	{

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -260,6 +260,17 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
 	}
 }
 
+B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_LOAD_MJCF);
+	if (command->m_type == CMD_LOAD_MJCF)
+	{
+		command->m_updateFlags |= URDF_ARGS_USE_MULTIBODY;
+		command->m_mjcfArguments.m_useMultiBody = useMultiBody;
+	}
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -3296,6 +3307,18 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHa
 	return 0;
 }
 
+B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_sleepThreshold = sleepThreshold;
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD;
+	return 0;
+}
+
+
+
 B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -157,6 +157,7 @@ extern "C"
 	B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce);
 	B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType);
 
+	B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold);
 	
 	B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
 	B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
@@ -415,6 +416,8 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
 	B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
+	B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
+	
 
 	///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
 	B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -9761,6 +9761,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 				mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
 			}
 
+			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD)
+			{
+				mb->setSleepThreshold(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold);
+			}
+
 			if (linkIndex == -1)
 			{
 				if (mb->getBaseCollider())
@@ -10045,6 +10050,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 			btRigidBody* rb = 0;
 			if (body && body->m_rigidBody)
 			{
+
 				if (linkIndex == -1)
 				{
 					rb = body->m_rigidBody;
@@ -10180,6 +10186,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 						m_data->m_dynamicsWorld->addCollisionObject(rb, collisionFilterGroup, collisionFilterMask);
 					}
 				}
+
+				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD)
+				{
+					btScalar threshold2 = btSqrt(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold);
+					rb->setSleepingThresholds(threshold2,threshold2);
+				}
+
 			}
 		}
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -14612,7 +14625,12 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
 			{
 				m_data->m_pickedBody = body;
 				m_data->m_savedActivationState = body->getActivationState();
+				if (m_data->m_savedActivationState==ISLAND_SLEEPING)
+				{
+					m_data->m_savedActivationState = ACTIVE_TAG;
+				}
 				m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
+				m_data->m_pickedBody->setDeactivationTime(0);
 				//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
 				btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
 				btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);

examples/SharedMemory/SharedMemoryCommands.h

@@ -174,6 +174,7 @@ enum EnumChangeDynamicsInfoFlags
 	CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS = 1 << 18,
 	CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE = 1 << 19,
 	CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE = 1 << 20,
+	CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD = 1 << 21,
 };
 
 struct ChangeDynamicsInfoArgs
@@ -205,6 +206,9 @@ struct ChangeDynamicsInfoArgs
 	double m_jointLimitForce;
 	
 	int m_dynamicType;
+
+	double m_sleepThreshold;
+
 };
 
 struct GetDynamicsInfoArgs

examples/pybullet/gym/pybullet_examples/sleeptest.py

@@ -0,0 +1,53 @@
+import pybullet as p
+import pybullet_data as pd
+import time
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+objects=[]
+useMaximalCoordinates=False
+
+if 0:
+  collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_SPHERE, radius = 0.1)
+  batchPositions = []
+  index = 1
+  for x in range(2):
+    for y in range(1):
+      for z in range(1):
+        batchPositions.append(arr[index])
+        index=index+1
+  bodyUids = p.createMultiBody(baseMass=10,
+                              baseInertialFramePosition=[0, 0, 0],
+                              baseCollisionShapeIndex=collisionShapeId,
+                              baseVisualShapeIndex=-1,
+                              basePosition=[0, 0, 2],
+                              batchPositions=batchPositions,
+                              useMaximalCoordinates=useMaximalCoordinates)
+  for b in bodyUids:
+    objects.append(b)
+
+else:
+  for i in range(2):
+    for j in range(1):
+      for k in range(1):
+        ob = p.loadURDF("sphere_1cm.urdf", [0.210050 * i, 0.210050 * j, 1 + 0.210050 * k],globalScaling=20,
+                        useMaximalCoordinates=useMaximalCoordinates)
+        objects.append(ob)
+        p.changeDynamics(ob, -1, activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING, linearDamping=0, angularDamping=0, sleepThreshold=0.05)
+        if (i==0):
+          p.resetBaseVelocity(ob, [0,0,0], [0,0,0.22])
+        
+        
+timeid = p.addUserDebugText("t=", [0,0,2])
+lvelid = p.addUserDebugText("lvel", [0,0,1.8])
+avelid = p.addUserDebugText("avel", [0,0,1.6])
+t=0
+dt=1./240.
+while p.isConnected():
+  p.stepSimulation()
+  t+=dt
+  txtid = p.addUserDebugText("t="+str(t), [0,0,2],replaceItemUniqueId=timeid)
+  lin, ang = p.getBaseVelocity(ob)
+  txtid = p.addUserDebugText("lvel="+"{:.4f}".format(lin[0])+","+"{:.4f}".format(lin[1])+","+"{:.4f}".format(lin[2]), [0,0,1.8],replaceItemUniqueId=lvelid)
+  txtid = p.addUserDebugText("avel="+"{:.4f}".format(ang[0])+","+"{:.4f}".format(ang[1])+","+"{:.4f}".format(ang[2]), [0,0,1.6],replaceItemUniqueId=avelid)
+  time.sleep(dt)
+  

examples/pybullet/pybullet.c

@@ -1354,9 +1354,10 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
 	PyObject* pylist = 0;
 	int physicsClientId = 0;
 	int flags = -1;
+	int useMultiBody = -1;
 
-	static char* kwlist[] = {"mjcfFileName", "flags", "physicsClientId", NULL};
+	static char* kwlist[] = {"mjcfFileName", "flags", "useMultiBody", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &mjcfFileName, &flags, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|iii", kwlist, &mjcfFileName, &flags, &useMultiBody, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1372,6 +1373,11 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
 	{
 		b3LoadMJCFCommandSetFlags(command, flags);
 	}
+	if (useMultiBody>=0)
+	{
+		b3LoadMJCFCommandSetUseMultiBody(command, useMultiBody);
+	}
+
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	statusType = b3GetStatusType(statusHandle);
 	if (statusType != CMD_MJCF_LOADING_COMPLETED)
@@ -1430,11 +1436,14 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	double jointUpperLimit = -1;
 	double jointLimitForce = -1;
 
+	double sleepThreshold = -1;
+	
+
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity",  "collisionMargin", "jointLowerLimit","jointUpperLimit", "jointLimitForce",  "physicsClientId", NULL};
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity",  "collisionMargin", "jointLowerLimit","jointUpperLimit", "jointLimitForce", "sleepThreshold", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdddddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &jointLowerLimit , &jointUpperLimit , &jointLimitForce , &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOddddddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &jointLowerLimit , &jointUpperLimit , &jointLimitForce , &sleepThreshold,  &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1460,6 +1469,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 		{
 			b3ChangeDynamicsInfoSetJointLimitForce(command, bodyUniqueId, linkIndex, jointLimitForce);
 		}
+		if (sleepThreshold >=0)
+		{
+			b3ChangeDynamicsInfoSetSleepThreshold(command, bodyUniqueId, sleepThreshold);
+		}
 
 		if (jointLowerLimit <= jointUpperLimit)
 		{

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -33,8 +33,8 @@
 
 namespace
 {
-const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
+const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
-const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2);     // in seconds
 }  // namespace
 
 void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
@@ -110,6 +110,9 @@ btMultiBody::btMultiBody(int n_links,
 	  m_canSleep(canSleep),
 	  m_canWakeup(true),
 	  m_sleepTimer(0),
+      m_sleepEpsilon(INITIAL_SLEEP_EPSILON),
+	  m_sleepTimeout(INITIAL_SLEEP_TIMEOUT),
+
 	  m_userObjectPointer(0),
 	  m_userIndex2(-1),
 	  m_userIndex(-1),
@@ -2104,10 +2107,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 			motion += m_realBuf[i] * m_realBuf[i];
 	}
 
-	if (motion < SLEEP_EPSILON)
+	if (motion < m_sleepEpsilon)
 	{
 		m_sleepTimer += timestep;
-		if (m_sleepTimer > SLEEP_TIMEOUT)
+		if (m_sleepTimer > m_sleepTimeout)
 		{
 			goToSleep();
 		}

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -726,6 +726,17 @@ public:
 
 	bool isLinkAndAllAncestorsKinematic(const int i) const;
 
+	void setSleepThreshold(btScalar sleepThreshold)
+	{
+		m_sleepEpsilon = sleepThreshold;
+	}
+
+	void setSleepTimeout(btScalar sleepTimeout)
+	{
+		this->m_sleepTimeout = sleepTimeout;
+	}
+
+
 private:
 	btMultiBody(const btMultiBody &);     // not implemented
 	void operator=(const btMultiBody &);  // not implemented
@@ -801,6 +812,8 @@ private:
 	bool m_canSleep;
 	bool m_canWakeup;
 	btScalar m_sleepTimer;
+	btScalar m_sleepEpsilon;
+	btScalar m_sleepTimeout;
 
 	void *m_userObjectPointer;
 	int m_userIndex2;