diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0138697c5..f02e332a4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; - command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q; - command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q; - + if ((qIndex>=0) && (qIndex < MAX_DEGREE_OF_FREEDOM)) + { + 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) + if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM)) { 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) + if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM)) { 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) + if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM)) { 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) + if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM)) { 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) + if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM)) { 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]; - 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]; + 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_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex]; + 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; diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 1bd307b4e..b4933aeb4 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/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(); } - - btUpdateInProcessExampleBrowserMainThread(m_data); - return PhysicsClientSharedMemory::processServerStatus(); + unsigned long int ms = m_clock.getTimeMilliseconds(); + if (ms>20) + { + m_clock.reset(); + btUpdateInProcessExampleBrowserMainThread(m_data); + } else + { + //b3Clock::usleep(100); + } + return PhysicsClientSharedMemory::processServerStatus(); } diff --git a/examples/ThirdPartyLibs/Gwen/Utility.cpp b/examples/ThirdPartyLibs/Gwen/Utility.cpp index 2da302d1b..fcf134a87 100644 --- a/examples/ThirdPartyLibs/Gwen/Utility.cpp +++ b/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 ); diff --git a/examples/pybullet/minitaur.py b/examples/pybullet/minitaur.py new file mode 100644 index 000000000..39827071c --- /dev/null +++ b/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]) diff --git a/examples/pybullet/minitaur_test.py b/examples/pybullet/minitaur_test.py new file mode 100644 index 000000000..8f4591086 --- /dev/null +++ b/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) diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py index a85ae0b1c..0d8f32b47 100644 --- a/examples/pybullet/quadruped.py +++ b/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 -speed = 10 +p_gain =1 +speed = 10 amplitude = 1.3 #stand still -t_end = time.time() + 2 -while time.time() < t_end: - p.stepSimulation() +p.setRealTimeSimulation(useRealTime) + + +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 -while time.time() < t_end: - t = time.time() +ref_time = 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) - - p.stepSimulation() + if (useRealTime==0): + p.stepSimulation() #hop forward -t_end = time.time() + 30 +t_end = 20 i=0 -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)*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) - - p.stepSimulation() + if (useRealTime==0): + p.stepSimulation() #walk -t_end = time.time() + 120 +t_end = 100 i=0 -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*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) diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 8dd7e4403..d4a1aa78e 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/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 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;im_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + //if (m_solverInfo->m_minimumSolverBatchSize<=1) + //{ + // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + //} else { for (i=0;i