reduce stack usage (cause some crashes in low-stack tests)

fix crashing bug in changeVisualShape
add differential gear version of racecar (only 2 back wheels are powered)

examples/pybullet/examples/racecar_differential.py

@@ -0,0 +1,71 @@
+import pybullet as p
+import time
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid<0):
+	p.connect(p.GUI)
+	
+p.resetSimulation()
+p.setGravity(0,0,-10)
+p.setPhysicsEngineParameter(numSolverIterations=1000)
+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.loadSDF("stadium.sdf")
+
+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)):
+		p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
+		p.getJointInfo(car,wheel)	
+
+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)
+
+c = p.createConstraint(car,10,car,13,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,9,car,13,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,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)
+
+
+
+steering = [0,2]
+
+targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
+maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
+steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
+while (True):
+	maxForce = p.readUserDebugParameter(maxForceSlider)
+	targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
+	steeringAngle = p.readUserDebugParameter(steeringSlider)
+	#print(targetVelocity)
+	
+	for wheel in wheels:
+		p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce)
+		
+	for steer in steering:
+		p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle)
+		
+	steering
+	if (useRealTimeSim==0):
+		p.stepSimulation()
+	time.sleep(0.01)

examples/pybullet/gym/envs/bullet/kuka.py

@@ -72,9 +72,10 @@ class Kuka:
     state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
     pos = state[0]
     orn = state[1]
+    euler = p.getEulerFromQuaternion(orn)
         
     observation.extend(list(pos))
-    observation.extend(list(orn))
+    observation.extend(list(euler))
     
     return observation
 

examples/pybullet/gym/envs/bullet/kukaGymEnv.py

@@ -31,6 +31,7 @@ class KukaGymEnv(gym.Env):
     self._p = p
     if self._renders:
       p.connect(p.GUI)
+      p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
     else:
       p.connect(p.DIRECT)
     #timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
@@ -52,8 +53,7 @@ class KukaGymEnv(gym.Env):
     p.setPhysicsEngineParameter(numSolverIterations=150)
     p.setTimeStep(self._timeStep)
     p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
-    if self._renders:
-      p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
+    
     p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
     
     xpos = 0.5 +0.05*random.random()
@@ -78,14 +78,21 @@ class KukaGymEnv(gym.Env):
 
   def getExtendedObservation(self):
      self._observation = self._kuka.getObservation()
-     pos,orn = p.getBasePositionAndOrientation(self.blockUid)
-     self._observation.extend(list(pos))
-     self._observation.extend(list(orn))
-      
+     eeState  = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
+     endEffectorPos = eeState[0]
+     endEffectorOrn = eeState[1]
+     blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
+
+     invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
+     blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
+     blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
+     self._observation.extend(list(blockPosInEE))
+     self._observation.extend(list(blockEulerInEE))
+
      return self._observation
   
   def _step(self, action):
-    dv = 0.002
+    dv = 0.01
     dx = [0,-dv,dv,0,0,0,0][action]
     dy = [0,0,0,-dv,dv,0,0][action]
     da = [0,0,0,0,0,-0.1,0.1][action]