diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 28d069505..884da0072 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -219,6 +219,7 @@ void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data) mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->finalizeMultiDof(); mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); break; } @@ -230,6 +231,7 @@ void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data) mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision); mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->finalizeMultiDof(); mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); break; } @@ -242,6 +244,7 @@ void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data) btScalar jointPos[4] = {(btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2], (btScalar)mbd->m_links[i].m_jointPos[3]}; btScalar jointVel[3] = {(btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2]}; mb->setJointPosMultiDof(i, jointPos); + mb->finalizeMultiDof(); mb->setJointVelMultiDof(i, jointVel); break; diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index c34ead64b..cd821da1f 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -13,10 +13,10 @@ struct DrawGridData int upAxis; float gridColor[4]; - DrawGridData(int upAxis = 1) + DrawGridData(int upAx = 1) : gridSize(10), upOffset(0.001f), - upAxis(upAxis) + upAxis(upAx) { gridColor[0] = 0.6f; gridColor[1] = 0.6f; diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h index 05e8f2504..09f8b57d3 100644 --- a/examples/CommonInterfaces/CommonRigidBodyBase.h +++ b/examples/CommonInterfaces/CommonRigidBodyBase.h @@ -10,6 +10,7 @@ #include "CommonGraphicsAppInterface.h" #include "CommonWindowInterface.h" +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" struct CommonRigidBodyBase : public CommonExampleInterface { @@ -317,6 +318,7 @@ struct CommonRigidBodyBase : public CommonExampleInterface btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld); + rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback); if (rayCallback.hasHit()) { diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index baa607bef..d3176cfe5 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -78,7 +78,7 @@ struct SimpleInternalData m_ffmpegFile(0), m_renderTexture(0), m_userPointer(0), - m_upAxis(0), + m_upAxis(1), m_customViewPortWidth(-1), m_customViewPortHeight(-1) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 651dde11b..1f4242cb9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10753,6 +10753,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld); MyResultCallback rayCallback(rayFromWorld, rayToWorld); + rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; m_data->m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback); if (rayCallback.hasHit()) { diff --git a/examples/pybullet/tensorflow/humanoid_running.py b/examples/pybullet/tensorflow/humanoid_running.py index b64310429..66a130cf2 100644 --- a/examples/pybullet/tensorflow/humanoid_running.py +++ b/examples/pybullet/tensorflow/humanoid_running.py @@ -10,7 +10,7 @@ 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) +p.setPhysicsEngineParameter(fixedTimeStep=1.0/60.,solverResidualThreshold=1-10, numSolverIterations=50, numSubSteps=4) #this mp4 recording requires ffmpeg installed #mp4log = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4,"humanoid.mp4") @@ -180,11 +180,14 @@ def demo_run(): forces = [0.] * len(motors) for m in range(len(motors)): - forces[m] = motor_power[m]*actions[m]*0.082 + limit=15 + ac = np.clip(actions[m],-limit,limit) + #print (ac) + forces[m] = motor_power[m]*ac*0.082 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) diff --git a/setup.py b/setup.py index 86b07206a..ce61fb7d2 100644 --- a/setup.py +++ b/setup.py @@ -582,7 +582,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.3.3', + version='2.3.4', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement 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',