remove pose frame from SDF,

allow plane collision shape in SDF
load the Roboschool stadium.sdf in humanoid_knee_position_control.py

data/stadium.sdf

@@ -48,12 +48,11 @@
 			</inertia>
 			</inertial>
 			<collision name='collision_1'>
-			<pose frame=''>0 0 -0.5 0 0 0</pose>
 			<geometry>
-				
+            <plane>
-            <box>
+              <normal>0 0 1</normal>
-              <size>100 100 1</size>
+              <size>100 100</size>
-            </box>
+            </plane>
           </geometry>
 			  </collision>
 			<visual name='visual'>

data/two_cubes.sdf

@@ -89,6 +89,7 @@
  <static>1</static>
       <pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
       <link name='unit_box_0::link'>
+      
         <inertial>
           <mass>1</mass>
           <inertia>
@@ -102,8 +103,6 @@
         </inertial>
         <collision name='collision'>
           <geometry>
-          <pose frame=''>0 0 -1 0 0 0</pose>
-      
             <box>
               <size>1 1 1</size>
             </box>

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -568,6 +568,15 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
     switch (collision->m_geometry.m_type)
     {
+	case URDF_GEOM_PLANE:
+		{
+			btVector3 planeNormal = collision->m_geometry.m_planeNormal;
+			btScalar planeConstant = 0;//not available?
+			btStaticPlaneShape* plane = new btStaticPlaneShape(planeNormal,planeConstant);
+			shape = plane;
+			shape ->setMargin(gUrdfDefaultCollisionMargin);
+			break;
+		}
 		case URDF_GEOM_CAPSULE:
         {
 			btScalar radius = collision->m_geometry.m_capsuleRadius;
@@ -789,7 +798,7 @@ upAxisMat.setIdentity();
 
         default:
 		b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
-		// for example, URDF_GEOM_PLANE
+		
 	}
 	return shape;
 }

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3939,6 +3939,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							}
 						}
 						else
+						{
+							if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
 							{
 								if (mb->getLinkCollider(linkIndex))
 								{
@@ -3971,7 +3973,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 										mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia);
 										mb->getLink(linkIndex).m_inertiaLocal = localInertia;
 									}
-
+								}
 							}
 						}
 					} else

examples/pybullet/examples/humanoid_knee_position_control.py

@@ -13,7 +13,7 @@ p.setRealTimeSimulation(useRealTime)
 
 p.setGravity(0,0,-10)
 
-p.loadURDF("plane.urdf")
+p.loadSDF("stadium.sdf")
 
 obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
 human = obUids[0]

examples/pybullet/tensorflow/humanoid_running.py

@@ -1,21 +1,30 @@
 import tensorflow as tf
 import sys
 import numpy as np
-
+import argparse
 import pybullet as p
 import time
-p.connect(p.GUI) #DIRECT is much faster, but GUI shows the running gait
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid<0):
+	cid = p.connect(p.GUI) #DIRECT is much faster, but GUI shows the running gait
+
 p.setGravity(0,0,-9.8)
 p.setPhysicsEngineParameter(fixedTimeStep=1.0/60., numSolverIterations=5, numSubSteps=2)
 #this mp4 recording requires ffmpeg installed
 #mp4log = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4,"humanoid.mp4")
 
+p.loadSDF("stadium.sdf")
+#p.loadURDF("plane.urdf")
 
-plane, human  = p.loadMJCF("mjcf/humanoid_symmetric.xml",flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
+objs = p.loadMJCF("mjcf/humanoid_symmetric_no_ground.xml",flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
-
+human  = objs[0]
 ordered_joints = []
 ordered_joint_indices = []
 
+parser = argparse.ArgumentParser()
+parser.add_argument('--profile')
+
 
 jdict = {}
 for j in range( p.getNumJoints(human) ):
@@ -175,7 +184,7 @@ def demo_run():
 		p.setJointMotorControlArray(human, motors,controlMode=p.TORQUE_CONTROL, forces=forces)
 
 		p.stepSimulation()
-		#time.sleep(0.01)
+		time.sleep(0.01)
 		distance=5
 		yaw = 0
 		humanPos, humanOrn = p.getBasePositionAndOrientation(human)

setup.py

@@ -418,7 +418,7 @@ else:
 
 setup(
 	name = 'pybullet',
-	version='1.1.0',
+	version='1.1.1',
 	description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -141,9 +141,13 @@ public:
 	}
 
 	btMultiBodyLinkCollider* getLinkCollider(int index)
+	{
+		if (index >= 0 && index < m_colliders.size())
 		{
 			return m_colliders[index];
 		}
+		return 0;
+	}
 
     //
     // get parent