Merge remote-tracking branch 'upstream/master'

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -444,10 +444,12 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
-    command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
+	if ((qIndex>=0) && (qIndex < MAX_DEGREE_OF_FREEDOM))
-	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+	{
-    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
+		command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
-
+		command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+		command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
+	}
     return 0;
 }
 
@@ -455,8 +457,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
-	b3Assert(dofIndex>=0);
+	if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
-	if (dofIndex>=0)
 	{
 		command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
 		command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
@@ -469,8 +470,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
-	b3Assert(dofIndex>=0);
+	if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
-	if (dofIndex>=0)
 	{
 		command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
 		command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
@@ -483,8 +483,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
-	b3Assert(dofIndex>=0);
+	if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
-	if (dofIndex>=0)
 	{
 		command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
 		command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
@@ -498,8 +497,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
-	b3Assert(dofIndex>=0);
+	if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
-	if (dofIndex>=0)
 	{
 		command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
 		command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
@@ -512,8 +510,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
-	b3Assert(dofIndex>=0);
+	if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
-	if (dofIndex>=0)
 	{
 		command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
 		command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
@@ -547,13 +544,16 @@ int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
 	  bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
 	  if (result)
 	  {
-		  state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
+		  if ((info.m_qIndex>=0) && (info.m_uIndex>=0)  && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
-		  state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
+		  {
-		  for (int ii(0); ii < 6; ++ii) {
+			  state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
-			state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
+			  state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
+			  for (int ii(0); ii < 6; ++ii) {
+				state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
+			  }
+			  state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
+			  return 1;
 		  }
-		  state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
-		  return 1;
 	  }
   }
   return 0;

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -1,5 +1,6 @@
 
 #include "SharedMemoryInProcessPhysicsC_API.h"
+#include "../Utils/b3Clock.h"
 
 #include "PhysicsClientSharedMemory.h"
 #include"../ExampleBrowser/InProcessExampleBrowser.h"
@@ -8,7 +9,8 @@
 class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
 {
     btInProcessExampleBrowserMainThreadInternalData* m_data;
-    
+   b3Clock m_clock;
+ 
 public:
     
     InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[])
@@ -41,9 +43,16 @@ public:
         {
             PhysicsClientSharedMemory::disconnectSharedMemory();
         }
-        
+    	unsigned long int ms = m_clock.getTimeMilliseconds();
-        btUpdateInProcessExampleBrowserMainThread(m_data);
+	if (ms>20)
-        return PhysicsClientSharedMemory::processServerStatus();
+	{ 
+		m_clock.reset(); 
+        	btUpdateInProcessExampleBrowserMainThread(m_data);
+        } else
+	{
+		//b3Clock::usleep(100);
+	}
+	return PhysicsClientSharedMemory::processServerStatus();
         
         
     }

examples/ThirdPartyLibs/Gwen/Utility.cpp

@@ -16,7 +16,7 @@ using namespace Gwen;
 
 UnicodeString Gwen::Utility::Format( const wchar_t* fmt, ... )
 {
-	wchar_t strOut[ 4096 ];
+	wchar_t strOut[ 2048 ];
 
 	va_list s;
 	va_start( s, fmt ); 

examples/pybullet/minitaur.py

@@ -0,0 +1,83 @@
+import pybullet as p
+
+class Minitaur:
+  def __init__(self):
+    self.reset()
+
+  def reset(self):
+    self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
+    self.kp = 1
+    self.kd = 0.1
+    self.maxForce = 100
+    nJoints = p.getNumJoints(self.quadruped)
+    self.jointNameToId = {}
+    for i in range(nJoints):
+      jointInfo = p.getJointInfo(self.quadruped, i)
+      self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+    self.resetPose()
+    for i in range(100):
+      p.stepSimulation()
+
+  def disableAllMotors(self):
+    nJoints = p.getNumJoints(self.quadruped)
+    for i in range(nJoints):
+      p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
+
+  def setMotorAngleByName(self, motorName, desiredAngle):
+    p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=self.jointNameToId[motorName], controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
+
+  def resetPose(self):
+    #right front leg
+    self.disableAllMotors()
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
+    self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
+    self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
+
+    #left front leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
+    self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
+    self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
+
+    #right back leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
+    self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
+    self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
+
+    #left back leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
+    self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
+    self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
+
+  def getBasePosition(self):
+    position, orientation = p.getBasePositionAndOrientation(self.quadruped)
+    return position
+
+  def getBaseOrientation(self):
+    position, orientation = p.getBasePositionAndOrientation(self.quadruped)
+    return orientation
+
+  def applyAction(self, motorCommands):
+    self.setMotorAngleByName('motor_front_rightR_joint', motorCommands[0])
+    self.setMotorAngleByName('motor_front_rightL_joint', motorCommands[1])
+    self.setMotorAngleByName('motor_front_leftR_joint', motorCommands[2])
+    self.setMotorAngleByName('motor_front_leftL_joint', motorCommands[3])
+    self.setMotorAngleByName('motor_back_rightR_joint', motorCommands[4])
+    self.setMotorAngleByName('motor_back_rightL_joint', motorCommands[5])
+    self.setMotorAngleByName('motor_back_leftR_joint', motorCommands[6])
+    self.setMotorAngleByName('motor_back_leftL_joint', motorCommands[7])

examples/pybullet/minitaur_test.py

@@ -0,0 +1,33 @@
+import pybullet as p
+
+from minitaur import Minitaur
+import time
+import math
+import numpy as np
+
+def main(unused_args):
+  timeStep = 0.01
+  c = p.connect(p.SHARED_MEMORY)
+  if (c<0):
+      c = p.connect(p.GUI)
+  p.resetSimulation()
+  p.setTimeStep(timeStep)
+  p.loadURDF("plane.urdf")
+  p.setGravity(0,0,-10)
+
+  minitaur = Minitaur()
+  amplitude = 0.24795664427
+  speed = 0.2860877729434
+  for i in range(1000):
+    a1 = math.sin(i*speed)*amplitude+1.57
+    a2 = math.sin(i*speed+3.14)*amplitude+1.57
+    joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
+    minitaur.applyAction(joint_values)
+
+    p.stepSimulation()
+#    print(minitaur.getBasePosition())
+    time.sleep(timeStep)
+  final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
+  print(final_distance)
+
+main(0)

examples/pybullet/quadruped.py

@@ -2,6 +2,8 @@ import pybullet as p
 import time
 import math
 
+useRealTime = 0
+fixedTimeStep = 0.001
 
 physId = p.connect(p.SHARED_MEMORY)
 if (physId<0):
@@ -9,8 +11,10 @@ if (physId<0):
 
 p.loadURDF("plane.urdf")
 p.setGravity(0,0,-1)
+p.setTimeStep(fixedTimeStep)
+
 p.setRealTimeSimulation(0)
-quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,2)
+quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,1)
 #p.getNumJoints(1)
 #right front leg
 p.resetJointState(quadruped,0,1.57)
@@ -71,24 +75,41 @@ p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
 
 
 
-p_gain = 2
+p_gain =1 
-speed = 10
+speed = 10 
 amplitude = 1.3
 
 #stand still
-t_end = time.time() + 2
+p.setRealTimeSimulation(useRealTime)
-while time.time() < t_end:
+
-	p.stepSimulation()
+
+t=0.0
+ref_time = time.time()
+t_end = t + 4
+while t < t_end:
+	if (useRealTime==0):
+		t = t+fixedTimeStep
+		p.stepSimulation()
+	else:
+		t = time.time()-ref_time
+		p.setGravity(0,0,-1)
+
 p.setGravity(0,0,-10)
 
 jump_amp = 0.5
 
 #jump
-t_end = time.time() + 10
+t = 0.0
+t_end = t + 10
 i=0
-t=0
+ref_time = time.time()
-while time.time() < t_end:
+
-	t = time.time()
+while t < t_end:
+	if (useRealTime):
+		t = time.time()-ref_time
+	else:
+		t = t+fixedTimeStep
+
 	p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
 	p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
 	p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
@@ -97,15 +118,19 @@ while time.time() < t_end:
 	p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
 	p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
 	p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
-	
+	if (useRealTime==0):	
-	p.stepSimulation()
+		p.stepSimulation()
 
 
 #hop forward
-t_end = time.time() + 30
+t_end = 20
 i=0
-while time.time() < t_end:
+while t < t_end:
-	t = time.time()
+	if (useRealTime):
+		t = time.time()-ref_time
+	else:
+		t = t+fixedTimeStep
+
 	p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
 	p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,p_gain)
 	p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
@@ -114,14 +139,18 @@ while time.time() < t_end:
 	p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,p_gain)
 	p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain)
 	p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,p_gain)
-	
+	if (useRealTime==0):	
-	p.stepSimulation()
+		p.stepSimulation()
 
 #walk
-t_end = time.time() + 120
+t_end = 100
 i=0
-while time.time() < t_end:
+while t < t_end:
-	t = time.time()
+	if (useRealTime):
+		t = time.time()-ref_time
+	else:
+		t = t+fixedTimeStep
+
 	p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*3)*.3+1.57,1)
 	p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
 	p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*3+0.5*3.14)*.3+1.57,1)

src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -24,6 +24,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
+//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT
+#define USE_LOCAL_STACK 1
 
 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
 
@@ -251,7 +253,12 @@ static inline void		MycollideTT(	const btDbvtNode* root0,
 			int								depth=1;
 			int								treshold=btDbvt::DOUBLE_STACKSIZE-4;
 			btAlignedObjectArray<btDbvt::sStkNN>	stkStack;
+#ifdef USE_LOCAL_STACK
+			ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
+			stkStack.initializeFromBuffer(&localStack,btDbvt::DOUBLE_STACKSIZE,btDbvt::DOUBLE_STACKSIZE);
+#else
 			stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
+#endif
 			stkStack[0]=btDbvt::sStkNN(root0,root1);
 			do	{
 				btDbvt::sStkNN	p=stkStack[--depth];
@@ -329,6 +336,10 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	{
 		int i;
 		btManifoldArray manifoldArray;
+#ifdef USE_LOCAL_STACK 
+		btPersistentManifold localManifolds[4];
+		manifoldArray.initializeFromBuffer(&localManifolds,0,4);
+#endif
 		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
 		for (i=0;i<pairs.size();i++)
 		{

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -332,10 +332,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 				}
 			}
 
-			if (m_solverInfo->m_minimumSolverBatchSize<=1)
+			//if (m_solverInfo->m_minimumSolverBatchSize<=1)
-			{
+			//{
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
+			//	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-			} else
+			//} else
 			{
 				
 				for (i=0;i<numBodies;i++)