Merge pull request #1561 from erwincoumans/master

add HelloPyBullet notebook, random block stacking gym env test

Extras/Serialize/CMakeLists.txt

@@ -1,6 +1,6 @@
 
 IF (BUILD_BLEND_DEMO OR INTERNAL_UPDATE_SERIALIZATION_STRUCTURES)
-	SUBDIRS(BlenderSerialize )
+	#SUBDIRS(BlenderSerialize )
 ENDIF()
 
 

Extras/Serialize/HeaderGenerator/CMakeLists.txt

@@ -13,7 +13,7 @@ SET(includes
 
 
 LINK_LIBRARIES(
-	 BulletFileLoader BlenderSerialize LinearMath
+	 BulletFileLoader  LinearMath
 )
 
 INCLUDE_DIRECTORIES(${includes})

Extras/Serialize/makesdna/makesdna.cpp

@@ -86,10 +86,12 @@ typedef unsigned __int64 uint64_t;
 
 	/* Linux-i386, Linux-Alpha, Linux-ppc */
 #include <stdint.h>
+typedef intptr_t btintptr_t;
 
 #elif defined (__APPLE__)
 
 #include <inttypes.h>
+typedef intptr_t btintptr_t;
 
 #elif defined(FREE_WINDOWS)
 

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -2,6 +2,9 @@
 #include "LinearMath/btTransform.h"
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+
+
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
@@ -502,6 +505,15 @@ void ConvertURDF2BulletInternal(
 
                 col->setCollisionShape(compoundShape);
 
+				if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+				{
+					btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)compoundShape;
+					if (trimeshShape->getTriangleInfoMap())
+					{
+						col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+					}
+				}
+
                 btTransform tr;
                 tr.setIdentity();
                 tr = linkTransformInWorldSpace;

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -566,6 +566,14 @@ B3_SHARED_API	int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMem
 	return 0;
 }
 
+B3_SHARED_API	int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_physSimParamArgs.m_allowedCcdPenetration = allowedCcdPenetration;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION ;
+	return 0;
+}
 
 B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
 {
@@ -2308,6 +2316,17 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHan
 	return 0;
 }
 
+B3_SHARED_API	int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
+	command->m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius = ccdSweptSphereRadius;
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS;
+	return 0;
+}
+
 
 
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)

examples/SharedMemory/PhysicsClientC_API.h

@@ -125,6 +125,7 @@ B3_SHARED_API	int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHand
 B3_SHARED_API	int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping);
 B3_SHARED_API	int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping);
 B3_SHARED_API	int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor);
+B3_SHARED_API	int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius);
 
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
 
@@ -291,9 +292,7 @@ B3_SHARED_API	int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle
 B3_SHARED_API	int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
 B3_SHARED_API	int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction);
 B3_SHARED_API	int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs);
-
-
-
+B3_SHARED_API	int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration);
 
 
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6,6 +6,8 @@
 #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
 #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
 
+#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
+
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -1727,6 +1729,7 @@ void logCallback(btDynamicsWorld *world, btScalar timeStep)
 
 bool MyContactAddedCallback(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
 {
+	btAdjustInternalEdgeContacts(cp, colObj1Wrap, colObj0Wrap, partId1,index1);
 	return true;
 }
 
@@ -2253,12 +2256,13 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	isPreTick = true;
 	m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback,this,isPreTick);
 
+	gContactAddedCallback = MyContactAddedCallback;
 
 #ifdef B3_ENABLE_TINY_AUDIO
 	m_data->m_soundEngine.init(16,true);
 
 //we don't use those callbacks (yet), experimental
-//	gContactAddedCallback = MyContactAddedCallback;
+
 //	gContactDestroyedCallback = MyContactDestroyedCallback;
 //	gContactProcessedCallback = MyContactProcessedCallback;
 //	gContactStartedCallback = MyContactStartedCallback;
@@ -2411,6 +2415,17 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 	for (int j = 0; j<m_data->m_collisionShapes.size(); j++)
 	{
 		btCollisionShape* shape = m_data->m_collisionShapes[j];
+
+
+		//check for internal edge utility, delete memory
+		if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+		{
+			btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) shape;
+			if (trimesh->getTriangleInfoMap())
+			{
+				delete trimesh->getTriangleInfoMap();
+			}
+		}
 		delete shape;
 	}
 	for (int j=0;j<m_data->m_meshInterfaces.size();j++)
@@ -3823,10 +3838,17 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 							meshInterface->addTriangle(v0, v1, v2);
 						}
 					}
+
 					{
 						BT_PROFILE("create btBvhTriangleMeshShape");
 						btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
 						m_data->m_collisionShapes.push_back(trimesh);
+
+						if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
+						{
+							btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
+							btGenerateInternalEdgeInfo(trimesh, triangleInfoMap);
+						}
 						//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
 						shape = trimesh;
 						if (compound)
@@ -6377,6 +6399,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 					body->m_rigidBody->setMassProps(mass,newLocalInertiaDiagonal);
 				}
 			}
+
+			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
+			{
+				body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
+				//for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
+				body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius/2.);
+			}
 		}
 	}
 					
@@ -6534,6 +6563,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 	{
 		m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0);
 	}
+
+	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION)
+	{
+		m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration;
+	}
+
 	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
 	{
 		m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;

examples/SharedMemory/SharedMemoryCommands.h

@@ -159,7 +159,7 @@ enum EnumChangeDynamicsInfoFlags
 	CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256,
 	CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512,
 	CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024,
-
+	CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048,
 };
 
 struct ChangeDynamicsInfoArgs
@@ -178,6 +178,7 @@ struct ChangeDynamicsInfoArgs
 	double m_contactDamping;
 	double m_localInertiaDiagonal[3];
 	int m_frictionAnchor;
+	double m_ccdSweptSphereRadius;
 };
 
 struct GetDynamicsInfoArgs
@@ -438,6 +439,7 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384,
 	SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
 	SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536,
+	SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072,
 };
 
 enum EnumLoadSoftBodyUpdateFlags

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -21,15 +21,15 @@ public:
     
     InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[], bool useInProcessMemory)
     {
-        int newargc = argc+2;
+        int newargc = argc+3;
         char** newargv = (char**)malloc(sizeof(void*)*newargc);
-        for (int i=0;i<argc;i++)
-            newargv[i] = argv[i];
+		char* t0 = (char*)"--unused";
+        newargv[0] = t0;
+		for (int i=0;i<argc;i++)
+            newargv[i+1] = argv[i];
+        newargv[argc+1]=(char*)"--logtostderr";
+		newargv[argc+2]=(char*)"--start_demo_name=Physics Server";
         
-        char* t0 = (char*)"--logtostderr";
-        char* t1 = (char*)"--start_demo_name=Physics Server";
-        newargv[argc] = t0;
-        newargv[argc+1] = t1;
         m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv, useInProcessMemory);
         SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data);
         

examples/SharedMemory/SharedMemoryPublic.h

@@ -664,6 +664,7 @@ enum eCONNECT_METHOD {
   eCONNECT_TCP = 5,
   eCONNECT_EXISTING_EXAMPLE_BROWSER=6,
   eCONNECT_GUI_SERVER=7,
+  eCONNECT_GUI_MAIN_THREAD=8,
 };
 
 enum eURDF_Flags
@@ -692,6 +693,7 @@ enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
 enum eUrdfCollisionFlags
 {
 	GEOM_FORCE_CONCAVE_TRIMESH=1,
+	GEOM_CONCAVE_INTERNAL_EDGE=2,
 };
 
 enum eUrdfVisualFlags
@@ -739,6 +741,7 @@ struct b3PhysicsSimulationParameters
 	double m_frictionERP;
 	int m_enableConeFriction;
 	int m_deterministicOverlappingPairs;
+	double m_allowedCcdPenetration;
 };
 
 

examples/pybullet/examples/experimentalCcdSphereRadius.py

@@ -0,0 +1,51 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
+
+
+terrain_mass=0
+terrain_visual_shape_id=-1
+terrain_position=[0,0,0]
+terrain_orientation=[0,0,0,1]
+terrain_collision_shape_id = p.createCollisionShape(
+	shapeType=p.GEOM_MESH,
+  fileName="terrain.obj",
+  flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE,
+  meshScale=[0.5, 0.5, 0.5])
+p.createMultiBody(
+	terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
+	terrain_position, terrain_orientation)
+            
+            
+useMaximalCoordinates = True
+sphereRadius = 0.005
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+
+for i in range (5):
+	for j in range (5):
+		for k in range (5):
+			#if (k&2):
+			sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*5*sphereRadius,j*5*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
+			#else:
+			#	sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
+			p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
+			p.changeDynamics(sphereUid,-1,ccdSweptSphereRadius=0.002)
+			
+
+
+p.setGravity(0,0,-10)
+
+pts = p.getContactPoints()
+print("num points=",len(pts))
+print(pts)
+while (p.isConnected()):
+	time.sleep(1./240.)
+	p.stepSimulation()
+	

examples/pybullet/examples/internalEdge.py

@@ -0,0 +1,42 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+
+if (1):
+	box_collision_shape_id = p.createCollisionShape(
+		shapeType=p.GEOM_BOX,
+		halfExtents=[0.01,0.01,0.055])
+	box_mass=0.1
+	box_visual_shape_id = -1
+	box_position=[0,0.1,1]
+	box_orientation=[0,0,0,1]
+
+	p.createMultiBody(
+		box_mass, box_collision_shape_id, box_visual_shape_id,
+		box_position, box_orientation, useMaximalCoordinates=True)
+	
+
+terrain_mass=0
+terrain_visual_shape_id=-1
+terrain_position=[0,0,0]
+terrain_orientation=[0,0,0,1]
+terrain_collision_shape_id = p.createCollisionShape(
+	shapeType=p.GEOM_MESH,
+  fileName="terrain.obj",
+  flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE,
+  meshScale=[0.5, 0.5, 0.5])
+p.createMultiBody(
+	terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
+	terrain_position, terrain_orientation)
+            
+
+p.setGravity(0,0,-10)
+
+pts = p.getContactPoints()
+print("num points=",len(pts))
+print(pts)
+while (p.isConnected()):
+	time.sleep(1./240.)
+	p.stepSimulation()
+	

examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py

@@ -6,11 +6,12 @@ import pybullet
 class BulletClient(object):
   """A wrapper for pybullet to manage different clients."""
 
-  def __init__(self, connection_mode=pybullet.DIRECT):
+  def __init__(self, connection_mode=pybullet.DIRECT, options=""):
     """Create a simulation and connect to it."""
     self._client = pybullet.connect(pybullet.SHARED_MEMORY)
     if(self._client<0):
-      self._client = pybullet.connect(connection_mode)
+      print("options=",options)
+      self._client = pybullet.connect(connection_mode,options=options)
     self._shapes = {}
 
   def __del__(self):

examples/pybullet/gym/pybullet_envs/prediction/boxstack_pybullet_sim.py

@@ -0,0 +1,156 @@
+"""This file implements the functionalities of an example simulated system using pybullet.
+
+"""
+import copy
+import math
+import numpy as np
+import os
+import pybullet_data
+import random
+
+
+class BoxStackPyBulletSim(object):
+  """The ExamplePyBulletSim class that adds some objects to the scene, steps the sim and return a reward.
+
+  """
+
+  def __init__(self,
+               pybullet_client,
+               urdf_root= pybullet_data.getDataPath(),
+               time_step=0.01):
+    """Constructs an example simulation and reset it to the initial states.
+
+    Args:
+      pybullet_client: The instance of BulletClient to manage different
+        simulations.
+      urdf_root: The path to the urdf folder.
+      time_step: The time step of the simulation.
+    """
+    self._pybullet_client = pybullet_client
+    self._urdf_root = urdf_root
+    self.m_actions_taken_since_reset=0
+    self.time_step = time_step
+    self.stateId = -1
+    self.Reset(reload_urdf=True)
+    
+
+  def Reset(self, reload_urdf=False):
+    """Reset the minitaur to its initial states.
+
+    Args:
+      reload_urdf: Whether to reload the urdf file. If not, Reset() just place
+        the minitaur back to its starting position.
+    """
+    self.m_actions_taken_since_reset=0
+    xPosRange=0.025
+    yPosRange=0.025
+    boxHalfExtents = 0.025
+    
+    if reload_urdf:
+      camInfo = self._pybullet_client.getDebugVisualizerCamera()
+      cameraDistance=camInfo[10]
+      print("cameraYaw=",camInfo[8])
+      print("cameraPitch=",camInfo[9])
+      print("camtarget=",camInfo[11])
+      print("projectionMatrix=",camInfo[3])
+      self._pybullet_client.resetDebugVisualizerCamera(cameraDistance=0.3, cameraYaw=camInfo[8], cameraPitch=camInfo[9],cameraTargetPosition=camInfo[11])
+
+      plane = self._pybullet_client.loadURDF("plane.urdf")
+      texUid = self._pybullet_client.loadTexture("checker_blue.png")
+      self._pybullet_client.changeVisualShape(plane,-1, textureUniqueId = texUid)
+      
+      
+      self._numObjects=4 #random number?
+      
+      
+      self._cubes=[]
+      
+      red=[0.97,0.25,0.25,1]
+      green=[0.41,0.68,0.31,1]
+      yellow=[0.92,0.73,0,1]
+      blue=[0,0.55,0.81,1]
+      colors=[red,green,yellow,blue]
+      
+      for i in range (self._numObjects):
+          pos=[0,0,boxHalfExtents + i*2*boxHalfExtents]
+          orn = self._pybullet_client.getQuaternionFromEuler([0,0,0])
+          orn=[0,0,0,1]
+          cube = self._pybullet_client.loadURDF("cube_small.urdf",pos,orn)
+          self._pybullet_client.changeVisualShape(cube,-1,rgbaColor=colors[i])
+          self._cubes.append(cube)
+    
+      self._pybullet_client.setGravity(0, 0, -10)
+      self.stateId = self._pybullet_client.saveState()
+    else:
+      if (self.stateId>=0):
+        self._pybullet_client.restoreState(self.stateId)
+    index=0
+    for i in self._cubes:
+      posX = random.uniform(-xPosRange,xPosRange)
+      posY = random.uniform(-yPosRange,yPosRange)
+      yaw = random.uniform(-math.pi,math.pi)
+      pos=[posX,posY,boxHalfExtents + index*2*boxHalfExtents]
+      index+=1
+      orn = self._pybullet_client.getQuaternionFromEuler([0,0,yaw])
+      self._pybullet_client.resetBasePositionAndOrientation(i,pos,orn)
+
+  def GetActionDimension(self):
+    """Get the length of the action list.
+
+    Returns:
+      The length of the action list.
+    """
+    return 4#self.num_motors
+
+  def GetObservationUpperBound(self):
+    """Get the upper bound of the observation.
+
+    Returns:
+      The upper bound of an observation. See GetObservation() for the details
+        of each element of an observation.
+    """
+    upper_bound = np.array([0.0] * self.GetObservationDimension())
+    upper_bound[0:] = 1.0  # Quaternion of base orientation.
+    return upper_bound
+
+  def GetObservationLowerBound(self):
+    """Get the lower bound of the observation."""
+    return -self.GetObservationUpperBound()
+
+  def GetObservationDimension(self):
+    """Get the length of the observation list.
+
+    Returns:
+      The length of the observation list.
+    """
+    sz = len(self.GetObservation())
+    print("sz=",sz)
+    return sz
+
+  def GetObservation(self):
+    """Get the observations of minitaur.
+
+    Returns:
+      The observation list. observation[0:4] is the base orientation in quaternion form.
+    """
+    observation = []
+    for i in self._cubes:
+      pos,orn=self._pybullet_client.getBasePositionAndOrientation(i)
+      observation.extend(list(pos))
+      observation.extend(list(orn))
+    return observation
+
+  def ApplyAction(self, action):
+    """Set the desired action.
+    """
+    self.m_actions_taken_since_reset+=1
+    
+
+  def Termination(self):
+  	return self.m_actions_taken_since_reset>=200
+ 
+def CreateSim(pybullet_client,urdf_root,time_step):
+  sim = BoxStackPyBulletSim(pybullet_client=pybullet_client,
+          urdf_root=urdf_root,
+          time_step=time_step)
+  return sim

examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py

@@ -0,0 +1,219 @@
+"""This file implements the gym environment of example PyBullet simulation.
+
+"""
+
+import os, inspect
+currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+parentdir = os.path.dirname(os.path.dirname(currentdir))
+os.sys.path.insert(0,parentdir)
+
+
+import math
+import time
+import gym
+from gym import spaces
+from gym.utils import seeding
+import numpy as np
+import pybullet
+from pybullet_envs.bullet import bullet_client
+
+from pybullet_envs.prediction import boxstack_pybullet_sim
+
+import os
+import pybullet_data
+
+from pkg_resources import parse_version
+
+
+class PyBulletSimGymEnv(gym.Env):
+  """The gym environment to run pybullet simulations.
+
+
+  """
+  metadata = {
+      "render.modes": ["human", "rgb_array"],
+      "video.frames_per_second": 50
+  }
+
+  def __init__(self,pybullet_sim_factory = boxstack_pybullet_sim,
+               render=True,
+               render_sleep=False,
+               debug_visualization=True,
+               hard_reset = False,
+               render_width=240,
+               render_height=240,
+               action_repeat=1,
+               time_step = 1./240.,
+               num_bullet_solver_iterations=50,
+               urdf_root=pybullet_data.getDataPath()):
+    """Initialize the gym environment.
+
+    Args:
+      urdf_root: The path to the urdf data folder.
+    """
+    self._pybullet_sim_factory = pybullet_sim_factory
+    self._time_step = time_step
+    self._urdf_root = urdf_root
+    self._observation = []
+    self._action_repeat=action_repeat
+    self._num_bullet_solver_iterations = num_bullet_solver_iterations
+    self._env_step_counter = 0
+    self._is_render = render
+    self._debug_visualization = debug_visualization
+    self._render_sleep = render_sleep
+    self._render_width=render_width
+    self._render_height=render_height
+    self._cam_dist = .3
+    self._cam_yaw = 50
+    self._cam_pitch = -35
+    self._hard_reset = True
+    self._last_frame_time = 0.0
+    
+    optionstring='--width={} --height={}'.format(render_width,render_height)
+    
+    print("urdf_root=" + self._urdf_root)
+
+    if self._is_render:
+      self._pybullet_client = bullet_client.BulletClient(
+          connection_mode=pybullet.GUI, options=optionstring)
+    else:
+      self._pybullet_client = bullet_client.BulletClient()
+      
+    if (debug_visualization==False):
+      self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_GUI,enable=0)
+      self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_RGB_BUFFER_PREVIEW,enable=0)
+      self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_DEPTH_BUFFER_PREVIEW,enable=0)
+      self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,enable=0)
+
+
+    self._pybullet_client.setAdditionalSearchPath(urdf_root)
+    self._seed()
+    self.reset()
+
+    observation_high = (
+        self._example_sim.GetObservationUpperBound())
+    observation_low = (
+        self._example_sim.GetObservationLowerBound())
+
+    action_dim = self._example_sim.GetActionDimension()
+    self._action_bound = 1
+    action_high = np.array([self._action_bound] * action_dim)
+    self.action_space = spaces.Box(-action_high, action_high)
+    self.observation_space = spaces.Box(observation_low, observation_high)
+    
+    self.viewer = None
+    self._hard_reset = hard_reset  # This assignment need to be after reset()
+
+  
+  def configure(self, args):
+    self._args = args
+
+  def _reset(self):
+    if self._hard_reset:
+      self._pybullet_client.resetSimulation()
+      
+      self._pybullet_client.setPhysicsEngineParameter(
+          numSolverIterations=int(self._num_bullet_solver_iterations))
+      self._pybullet_client.setTimeStep(self._time_step)
+      
+      self._example_sim = self._pybullet_sim_factory.CreateSim(
+          pybullet_client=self._pybullet_client,
+          urdf_root=self._urdf_root,
+          time_step=self._time_step)
+    else:
+      self._example_sim.Reset(reload_urdf=False)
+
+    self._env_step_counter = 0
+    
+    #self._pybullet_client.resetDebugVisualizerCamera(
+    #    self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0])
+
+    return self._get_observation()
+
+  def _seed(self, seed=None):
+    self.np_random, seed = seeding.np_random(seed)
+    return [seed]
+
+  def _step(self, action):
+    """Step forward the simulation, given the action.
+
+    Args:
+      action: the predicted state
+
+    Returns:
+      observations: The actual state.
+      reward: The reward for how well the prediction matches the actual state.
+      done: Whether the episode has ended.
+      info: A dictionary that stores diagnostic information.
+
+    Raises:
+      ValueError: The action dimension is not the same as the number of motors.
+      ValueError: The magnitude of actions is out of bounds.
+    """
+    if self._render_sleep:
+      # Sleep, otherwise the computation takes less time than real time,
+      # which will make the visualization like a fast-forward video.
+      time_spent = time.time() - self._last_frame_time
+      self._last_frame_time = time.time()
+      time_to_sleep = self._action_repeat * self._time_step - time_spent
+      if time_to_sleep > 0:
+        time.sleep(time_to_sleep)
+        
+      #base_pos = self.minitaur.GetBasePosition()
+      #self._pybullet_client.resetDebugVisualizerCamera(
+      #    self._cam_dist, self._cam_yaw, self._cam_pitch, base_pos)
+    
+    
+    for _ in range(self._action_repeat):
+      self._example_sim.ApplyAction(action)
+      self._pybullet_client.stepSimulation()
+
+    self._env_step_counter += 1
+    reward = self._reward()
+    done = self._termination()
+    return np.array(self._get_observation()), reward, done, {}
+
+  def _render(self, mode="rgb_array", close=False):
+    if mode != "rgb_array":
+      return np.array([])
+    base_pos = [0,0,0]
+    view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
+        cameraTargetPosition=base_pos,
+        distance=self._cam_dist,
+        yaw=self._cam_yaw,
+        pitch=self._cam_pitch,
+        roll=0,
+        upAxisIndex=2)
+    proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
+        fov=60, aspect=float(self._render_width)/self._render_width,
+        nearVal=0.01, farVal=100.0)
+    proj_matrix=[1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
+    (_, _, px, _, _) = self._pybullet_client.getCameraImage(
+        width=self._render_width, height=self._render_height, viewMatrix=view_matrix,
+        projectionMatrix=proj_matrix, renderer=pybullet.ER_TINY_RENDERER)
+    rgb_array = np.array(px, dtype=np.uint8)
+    rgb_array = np.reshape(rgb_array, (self._render_height, self._render_width, 4))
+    rgb_array = rgb_array[:, :, :3]
+    return rgb_array
+
+  
+
+  def _termination(self):
+    terminate=self._example_sim.Termination()
+    return terminate
+
+  def _reward(self):
+    reward = 0
+    return reward
+
+  
+  def _get_observation(self):
+    self._observation = self._example_sim.GetObservation()
+    return self._observation
+
+  
+  if parse_version(gym.__version__)>=parse_version('0.9.6'):
+    render = _render
+    reset = _reset
+    seed = _seed
+    step = _step

examples/pybullet/gym/pybullet_envs/prediction/test_pybullet_sim_gym_env.py

@@ -0,0 +1,60 @@
+r"""An example to run of the minitaur gym environment with sine gaits.
+"""
+
+import os
+import inspect
+currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+parentdir = os.path.dirname(os.path.dirname(currentdir))
+os.sys.path.insert(0,parentdir)
+
+import math
+import numpy as np
+from pybullet_envs.prediction import pybullet_sim_gym_env
+import argparse
+import time
+
+from pybullet_envs.prediction import boxstack_pybullet_sim
+from gym.wrappers.monitoring import video_recorder
+
+def ResetPoseExample(steps):
+  """An example that the minitaur stands still using the reset pose."""
+  
+  
+  environment = pybullet_sim_gym_env.PyBulletSimGymEnv(
+      pybullet_sim_factory=boxstack_pybullet_sim,
+      debug_visualization=False,
+      render=True, action_repeat=30)
+  action = [math.pi / 2] * 8
+  
+  vid = video_recorder.VideoRecorder(env=environment,path="vid.mp4")
+  
+  for _ in range(steps):
+    print(_)
+    startsim = time.time()
+    _, _, done, _ = environment.step(action)
+    stopsim = time.time()
+    startrender = time.time()
+    #environment.render(mode='rgb_array')
+    vid.capture_frame()
+    stoprender = time.time()
+    print ("env.step " , (stopsim - startsim))
+    print ("env.render " , (stoprender - startrender))
+    if done:
+    	environment.reset()
+  
+
+
+
+def main():
+    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument('--env', help='environment ID (0==reset)',type=int,  default=0)
+    args = parser.parse_args()
+    print("--env=" + str(args.env))
+      
+    if (args.env == 0):
+      ResetPoseExample(steps = 1000)
+
+if __name__ == '__main__':
+    main()
+
+

examples/pybullet/pybullet.c

@@ -382,6 +382,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 #endif
 				break;
 			}
+			case eCONNECT_GUI_MAIN_THREAD:
+				{
+					sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
+					break;
+				}
 			case eCONNECT_GUI_SERVER:
 			{
 				
@@ -870,14 +875,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	double angularDamping = -1;
 	double contactStiffness = -1;
 	double contactDamping = -1;
+	double ccdSweptSphereRadius=-1;
 	int frictionAnchor = -1;
 	PyObject* localInertiaDiagonalObj=0;
 
 	b3PhysicsClientHandle sm = 0;
 	
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &physicsClientId))
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOdi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -937,6 +943,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 		{
 			b3ChangeDynamicsInfoSetFrictionAnchor(command,bodyUniqueId,linkIndex, frictionAnchor);
 		}
+		if (ccdSweptSphereRadius>=0)
+		{
+			b3ChangeDynamicsInfoSetCcdSweptSphereRadius(command,bodyUniqueId,linkIndex, ccdSweptSphereRadius);
+		}
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 	
@@ -1099,15 +1109,17 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	double erp = -1;
 	double contactERP = -1;
 	double frictionERP = -1;
+	double allowedCcdPenetration = -1;
+
 	int enableConeFriction = -1;
 	b3PhysicsClientHandle sm = 0;
 	int deterministicOverlappingPairs = -1;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "physicsClientId", NULL};
+	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
-									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1188,6 +1200,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 			b3PhysicsParameterSetDeterministicOverlappingPairs(command,deterministicOverlappingPairs);
 		}
 
+		if (allowedCcdPenetration>=0)
+		{
+			b3PhysicsParameterSetAllowedCcdPenetration(command,allowedCcdPenetration);
+			
+		}
+
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 
@@ -8776,6 +8794,7 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP);        // user read
 	PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP);        // user read
 	PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER);        // user read
+	PyModule_AddIntConstant(m, "GUI_MAIN_THREAD", eCONNECT_GUI_MAIN_THREAD);        // user read
 
 	PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType);        // user read
 	PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType);      // user read
@@ -8902,6 +8921,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
 
 	PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH);
+	PyModule_AddIntConstant(m, "GEOM_CONCAVE_INTERNAL_EDGE", GEOM_CONCAVE_INTERNAL_EDGE);
+	
 	
 	PyModule_AddIntConstant(m, "STATE_LOG_JOINT_MOTOR_TORQUES", STATE_LOG_JOINT_MOTOR_TORQUES);
 	PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES);