From a4a6a3ce571697f4d80f01c9867ccfa7b88a30f9 Mon Sep 17 00:00:00 2001 From: Giovanni Petrantoni Date: Wed, 17 Aug 2016 14:19:58 +0900 Subject: [PATCH 01/12] Fixed btKinematicCharacterController runtime crashes coming from previous pull request (more testing is necessary) --- .../Character/btKinematicCharacterController.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 60e49b750..83fe4969f 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -134,6 +134,7 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up) { + m_ghostObject = ghostObject; m_up.setValue(0.0f, 0.0f, 1.0f); m_jumpAxis.setValue(0.0f, 0.0f, 1.0f); setUp(up); @@ -141,8 +142,7 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho m_addedMargin = 0.02; m_walkDirection.setValue(0.0,0.0,0.0); m_AngVel.setValue(0.0, 0.0, 0.0); - m_useGhostObjectSweepTest = true; - m_ghostObject = ghostObject; + m_useGhostObjectSweepTest = true; m_turnAngle = btScalar(0.0); m_convexShape=convexShape; m_useWalkDirection = true; // use walk direction by default, legacy behavior @@ -520,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + if (!callback.hasHit() && m_ghostObject->hasContactResponse()) { //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); @@ -529,7 +529,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + if (!callback.hasHit() && m_ghostObject->hasContactResponse()) { //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); @@ -537,11 +537,11 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld } btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; - bool has_hit = false; + bool has_hit; if (bounce_fix == true) has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); else - has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); + has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject); btScalar stepHeight = 0.0f; if (m_verticalVelocity < 0.0) @@ -556,7 +556,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld m_targetPosition = orig_position; downVelocity = stepHeight; - btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); + step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; runonce = true; continue; //re-run previous tests @@ -564,7 +564,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld break; } - if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true) { // we dropped a fraction of the height -> hit floor btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; From 463f3e59c812a01d12622cb45ec37b7ee788f4a9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 17 Aug 2016 10:18:33 -0700 Subject: [PATCH 02/12] minor tweak in F1/screenshot handling. Note you can use --png_skip_frames=x command-line argument --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 37 ++++++++++--------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 6a8c87355..a3238f358 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1103,6 +1103,24 @@ void OpenGLExampleBrowser::update(float deltaTime) s_app->drawText(bla,10,10); } + if (gPngFileName) + { + + static int skip = 0; + skip--; + if (skip<0) + { + skip=gPngSkipFrames; + //printf("gPngFileName=%s\n",gPngFileName); + static int s_frameCount = 100; + + sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++); + //b3Printf("Made screenshot %s",staticPngFileName); + s_app->dumpNextFrameToPng(staticPngFileName); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + } + } + if (sCurrentDemo) { @@ -1145,24 +1163,7 @@ void OpenGLExampleBrowser::update(float deltaTime) } } - if (gPngFileName) - { - - static int skip = 0; - skip--; - if (skip<0) - { - skip=gPngSkipFrames; - //printf("gPngFileName=%s\n",gPngFileName); - static int s_frameCount = 100; - - sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++); - //b3Printf("Made screenshot %s",staticPngFileName); - s_app->dumpNextFrameToPng(staticPngFileName); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - } - } - + { From 7c9441c3f5c336af08507a1eaff264bf41913161 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 17 Aug 2016 10:30:50 -0700 Subject: [PATCH 03/12] by default, have a strong force keep the robot together, instead of floppy joints. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5e2970b83..e95713c14 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -696,7 +696,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) if (supportsJointMotor(mb,mbLinkIndex)) { - float maxMotorImpulse = 0.f; + float maxMotorImpulse = 10000.f; int dof = 0; btScalar desiredVelocity = 0.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); From 17c16ccfa02c221ee95e1ae52d7d248578507dbb Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 17 Aug 2016 19:35:52 -0700 Subject: [PATCH 04/12] pybullet, deal with overflow of joints (maximum of 128 joints/links per multibody at the moment) increase from 64 to 128 joints in shared memory API/pybullet fix potential issue in tinyrenderer, related to missing segmentation mask buffer report error if CMD_REQUEST_ACTUAL_STATE command on a multibody that exceed the number of links, todo: stream data to allow arbitrary large number of links in shared memory API --- .../PhysicsServerCommandProcessor.cpp | 6 ++ examples/SharedMemory/SharedMemoryCommands.h | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 2 +- examples/pybullet/pybullet.c | 87 +++++++++++-------- 4 files changed, 58 insertions(+), 39 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e95713c14..712e4b9fe 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1602,6 +1602,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; + if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + break; + } //always add the base, even for static (non-moving objects) //so that we can easily move the 'fixed' base when needed diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f7a9812ca..5e2bd41c5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -27,7 +27,7 @@ #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) #define SHARED_MEMORY_SERVER_TEST_C -#define MAX_DEGREE_OF_FREEDOM 64 +#define MAX_DEGREE_OF_FREEDOM 128 #define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_SDF_FILENAME_LENGTH 1024 diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index b81ce57ed..3ed2de4b9 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -268,7 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) renderData.m_viewportMatrix = viewport(0,0,width, height); b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer; - int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0); + int* segmentationMaskBufferPtr = renderData.m_segmentationMaskBufferPtr?&renderData.m_segmentationMaskBufferPtr->at(0):0; TGAImage& frame = renderData.m_rgbColorBuffer; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 915de51f1..dfe4f292d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -552,7 +552,11 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); const int status_type = b3GetStatusType(status_handle); - + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return NULL; + } const double* actualStateQ; // const double* jointReactionForces[]; int i; @@ -848,42 +852,44 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - pyListJointInfo = PyTuple_New(jointInfoSize); - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - - // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", - // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, - // info.m_qIndex, - // info.m_uIndex); - // printf(" flags=%d jointDamping=%f jointFriction=%f\n", - // info.m_flags, - // info.m_jointDamping, - // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, - PyInt_FromLong(info.m_jointIndex)); - PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); - PyTuple_SetItem(pyListJointInfo, 2, - PyInt_FromLong(info.m_jointType)); - PyTuple_SetItem(pyListJointInfo, 3, - PyInt_FromLong(info.m_qIndex)); - PyTuple_SetItem(pyListJointInfo, 4, - PyInt_FromLong(info.m_uIndex)); - PyTuple_SetItem(pyListJointInfo, 5, - PyInt_FromLong(info.m_flags)); - PyTuple_SetItem(pyListJointInfo, 6, - PyFloat_FromDouble(info.m_jointDamping)); - PyTuple_SetItem(pyListJointInfo, 7, - PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; + if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) + { + + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + PyTuple_SetItem(pyListJointInfo, 0, + PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, + PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, + PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, + PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, + PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + return pyListJointInfo; + } + else + { + PyErr_SetString(SpamError, "GetJointInfo failed."); + return NULL; + } } } @@ -934,12 +940,19 @@ pybullet_getJointState(PyObject* self, PyObject* args) { if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { - + int status_type = 0; b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - + + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return NULL; + } + pyListJointState = PyTuple_New(sensorStateSize); pyListJointForceTorque = PyTuple_New(forceTorqueSize); From 8f21e2bca967f8e1b5bb31c0419370fdfaf26830 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 17 Aug 2016 20:01:45 -0700 Subject: [PATCH 05/12] fix compile issue --- examples/pybullet/pybullet.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index dfe4f292d..becc7436e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -532,7 +532,7 @@ pybullet_setTimeStep(PyObject* self, PyObject* args) // Internal function used to get the base position and orientation // Orientation is returned in quaternions -static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) +static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) { basePosition[0] = 0.; basePosition[1] = 0.; @@ -555,7 +555,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return NULL; + return 0; } const double* actualStateQ; // const double* jointReactionForces[]; @@ -583,6 +583,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double } } + return 1; } // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion @@ -610,7 +611,11 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) return NULL; } - pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation); + if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation)) + { + PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?)."); + return NULL; + } { From 6005e54aa1e72c56c65e323ad4c4b682debb61d4 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 18 Aug 2016 09:44:33 -0700 Subject: [PATCH 06/12] Fix some inconsistencies in URDF file handling between btRigidBody and btMultiBody (rotation order and application of root-inertial-frame offset) --- .../ImportURDFDemo/ImportURDFSetup.cpp | 33 ++++++++++++------- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 10 +++--- .../SharedMemory/PhysicsClientExample.cpp | 2 +- 3 files changed, 29 insertions(+), 16 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 46b69cc08..de25c1a66 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -36,7 +36,8 @@ class ImportUrdfSetup : public CommonMultiBodyBase struct ImportUrdfInternalData* m_data; bool m_useMultiBody; btAlignedObjectArray m_nameMemory; - + btScalar m_grav; + int m_upAxis; public: ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName); virtual ~ImportUrdfSetup(); @@ -87,7 +88,9 @@ struct ImportUrdfInternalData ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName) - :CommonMultiBodyBase(helper) + :CommonMultiBodyBase(helper), + m_grav(0), + m_upAxis(2) { m_data = new ImportUrdfInternalData; @@ -186,9 +189,9 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName) void ImportUrdfSetup::initPhysics() { - int upAxis = 2; - m_guiHelper->setUpAxis(upAxis); - + + m_guiHelper->setUpAxis(m_upAxis); + this->createEmptyDynamicsWorld(); //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -199,10 +202,14 @@ void ImportUrdfSetup::initPhysics() );//+btIDebugDraw::DBG_DrawConstraintLimits); - btVector3 gravity(0,0,0); - gravity[upAxis]=-9.8; - - m_dynamicsWorld->setGravity(gravity); + if (m_guiHelper->getParameterInterface()) + { + SliderParams slider("Gravity", &m_grav); + slider.m_minVal = -10; + slider.m_maxVal = 10; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + BulletURDFImporter u2b(m_guiHelper, 0); @@ -350,7 +357,7 @@ void ImportUrdfSetup::initPhysics() if (createGround) { btVector3 groundHalfExtents(20,20,20); - groundHalfExtents[upAxis]=1.f; + groundHalfExtents[m_upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); m_collisionShapes.push_back(box); box->initializePolyhedralFeatures(); @@ -358,7 +365,7 @@ void ImportUrdfSetup::initPhysics() m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-2.5; + groundOrigin[m_upAxis]=-2.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); //m_dynamicsWorld->removeRigidBody(body); @@ -388,6 +395,10 @@ void ImportUrdfSetup::stepSimulation(float deltaTime) { if (m_dynamicsWorld) { + btVector3 gravity(0, 0, 0); + gravity[m_upAxis] = m_grav; + m_dynamicsWorld->setGravity(gravity); + for (int i=0;im_numMotors;i++) { if (m_data->m_jointMotors[i]) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 367e674da..8503dd41d 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -299,7 +299,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { //b3Printf("Fixed joint\n"); - btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA); if (enableConstraints) world1->addConstraint(dof6,true); @@ -324,7 +324,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); if (enableConstraints) world1->addConstraint(dof6,true); @@ -350,7 +350,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); + btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); if (enableConstraints) @@ -455,7 +455,9 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter mb->setHasSelfCollision(false); mb->finalizeMultiDof(); - mb->setBaseWorldTransform(rootTransformInWorldSpace); + btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex]; + + mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot); btAlignedObjectArray scratch_q; btAlignedObjectArray scratch_m; mb->forwardKinematics(scratch_q,scratch_m); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 8dc2f962d..034215bd2 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -239,7 +239,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; - static double startPosY = 1; + static double startPosY = 0; double startPosZ = 0; b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); startPosY += 2.f; From 5b0253ed47c49e43d8a6a1dc41e2ebfa4b9973ae Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 18 Aug 2016 13:10:28 -0700 Subject: [PATCH 07/12] Remove undesired assert, it makes pybullet mistakes fail in c++ Shared memory api/pybullet: by default, set joint motors in position PD mode with target 0, to maintain 0 joint angle. pybullet: allow setJointControlMode(body, link, POSITION_CONTROL,targetPos etc. --- .../PhysicsClientSharedMemory.cpp | 1 - .../PhysicsServerCommandProcessor.cpp | 2 + examples/pybullet/pybullet.c | 119 ++++++++++++++---- 3 files changed, 94 insertions(+), 28 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 16b435f61..85c90bbd7 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -81,7 +81,6 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const return bodyJoints->m_jointInfo.size(); } - btAssert(0); return 0; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 712e4b9fe..335855513 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -700,6 +700,8 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) int dof = 0; btScalar desiredVelocity = 0.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); + motor->setPositionTarget(0, 0.1); + motor->setVelocityTarget(0, 1); //motor->setMaxAppliedImpulse(0); mb->getLink(mbLinkIndex).m_userPtr = motor; m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index becc7436e..fdc753443 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -296,10 +296,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { int size; int bodyIndex, jointIndex, controlMode; - double targetValue=0; + double targetPosition=0; double targetVelocity=0; double maxForce=100000; + double appliedForce = 0; double kp=0.1; double kd=1.0; int valid = 0; @@ -313,34 +314,107 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) size= PySequence_Size(args); if (size==4) { - // for CONTROL_MODE_VELOCITY targetValue -> velocity - // for CONTROL_MODE_TORQUE targetValue -> force torque + double targetValue = 0; + // see switch statement below for convertsions dependent on controlMode if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } - valid = 1; + valid = 1; + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + appliedForce = targetValue; + break; + } + default: + { + valid = 0; + } + } + } if (size==5) { - // for CONTROL_MODE_VELOCITY targetValue -> velocity - // for CONTROL_MODE_TORQUE targetValue -> force torque + double targetValue = 0; + //See switch statement for conversions if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } valid = 1; + + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } } if (size==6) { - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd)) + double gain; + double targetValue = 0; + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } - valid = 1; + valid = 1; + + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + kp = gain; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + kd = gain; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } } if (size==8) { @@ -353,16 +427,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) valid = 1; } - if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD) - { - PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD"); - return NULL; - } - if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8) - { - PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity"); - return NULL; - } + if (valid) { @@ -395,25 +460,25 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { case CONTROL_MODE_VELOCITY: { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); break; } case CONTROL_MODE_TORQUE: { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce); break; } case CONTROL_MODE_POSITION_VELOCITY_PD: { b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); break; } default: @@ -426,7 +491,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) Py_INCREF(Py_None); return Py_None; } - PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl."); + PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); return NULL; } From ab8f498d180f759b40609b70f3ce81c2516890c2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 18 Aug 2016 13:44:04 -0700 Subject: [PATCH 08/12] avoid infinite recursion in b3Clock::usleep --- examples/Utils/b3Clock.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index 7482c302d..8e1884526 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -239,7 +239,7 @@ void b3Clock::usleep(int microSeconds) Sleep(millis); #else - usleep(microSeconds); + ::usleep(microSeconds); //struct timeval tv; //tv.tv_sec = microSeconds/1000000L; //tv.tv_usec = microSeconds%1000000L; From 2c636b52f7725acd096e88c27a63d9bfb4ac4ade Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 18 Aug 2016 15:36:18 -0700 Subject: [PATCH 09/12] bugfix: in torque control mode, torque index starts at 6. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 335855513..14df8c094 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1418,7 +1418,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Using CONTROL_MODE_TORQUE"); } // mb->clearForcesAndTorques(); - int torqueIndex = 0; + int torqueIndex = 6; if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) { for (int link=0;linkgetNumLinks();link++) From 91839cb2746cd045eb4ddb1c746edad71e4380e4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 18 Aug 2016 16:48:14 -0700 Subject: [PATCH 10/12] revert change of default position control in shared memory api/pybullet --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 14df8c094..b0adbd12d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -700,7 +700,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) int dof = 0; btScalar desiredVelocity = 0.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); - motor->setPositionTarget(0, 0.1); + motor->setPositionTarget(0, 0); motor->setVelocityTarget(0, 1); //motor->setMaxAppliedImpulse(0); mb->getLink(mbLinkIndex).m_userPtr = motor; From a68c9ca845fd29b20c86cde400e94b378b7a5702 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 18 Aug 2016 21:43:43 -0700 Subject: [PATCH 11/12] avoid MT crash in VR/physics server due to printf from separate thread. add fps display in VR use 1./240. internal substep for real-time physics sim in VR/physics server for more accurate robotics sim. --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 6 +++--- .../PhysicsServerCommandProcessor.cpp | 2 +- examples/SharedMemory/PhysicsServerExample.cpp | 17 ++++++++++++++++- 3 files changed, 20 insertions(+), 5 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index a3238f358..be8d0f2e8 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -490,7 +490,7 @@ void MyComboBoxCallback(int comboId, const char* item) void MyGuiPrintf(const char* msg) { printf("b3Printf: %s\n",msg); - if (gui2) + if (!gDisableDemoSelection) { gui2->textOutput(msg); gui2->forceUpdateScrollBars(); @@ -502,7 +502,7 @@ void MyGuiPrintf(const char* msg) void MyStatusBarPrintf(const char* msg) { printf("b3Printf: %s\n", msg); - if (gui2) + if (!gDisableDemoSelection) { bool isLeft = true; gui2->setStatusBarMessage(msg,isLeft); @@ -513,7 +513,7 @@ void MyStatusBarPrintf(const char* msg) void MyStatusBarError(const char* msg) { printf("Warning: %s\n", msg); - if (gui2) + if (!gDisableDemoSelection) { bool isLeft = false; gui2->setStatusBarMessage(msg,isLeft); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b0adbd12d..96dc083a7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2445,7 +2445,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); } - m_data->m_dynamicsWorld->stepSimulation(dtInSec); + m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.); } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 59e81ac4c..489d9526f 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -872,7 +872,22 @@ void PhysicsServerExample::renderScene() static int frameCount=0; frameCount++; char bla[1024]; - sprintf(bla,"VR sub-title text test, frame %d", frameCount/2); + + static btScalar prevTime = m_clock.getTimeSeconds(); + btScalar curTime = m_clock.getTimeSeconds(); + static btScalar deltaTime = 0.f; + static int count = 10; + if (count-- < 0) + { + count = 10; + deltaTime = curTime - prevTime; + } + if (deltaTime == 0) + deltaTime = 1000; + + prevTime = curTime; + + sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2); float pos[4]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); btTransform viewTr; From 62d5b7c5c03317514cbb00054547a4a690a4d553 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 19 Aug 2016 10:30:02 -0700 Subject: [PATCH 12/12] add single step simulation, using 'o' key. use 'i' key to suspend simulation first default background color a bit darker, to show debug lines tweaked contact point rendering a bit --- .../CommonGraphicsAppInterface.h | 6 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 13 +- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 5 +- .../RoboticsLearning/GripperGraspExample.cpp | 28 +-- examples/RoboticsLearning/b3RobotSimAPI.cpp | 207 ++++++++++-------- examples/RoboticsLearning/b3RobotSimAPI.h | 1 + examples/SharedMemory/PhysicsDirect.cpp | 18 ++ examples/SharedMemory/PhysicsDirect.h | 12 +- src/LinearMath/btIDebugDraw.h | 6 +- 9 files changed, 174 insertions(+), 122 deletions(-) diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index 242c63ccd..674807de8 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -66,9 +66,9 @@ struct CommonGraphicsApp m_mouseYpos(0.f), m_mouseInitialized(false) { - m_backgroundColorRGB[0] = 0.9; - m_backgroundColorRGB[1] = 0.9; - m_backgroundColorRGB[2] = 1; + m_backgroundColorRGB[0] = 0.7; + m_backgroundColorRGB[1] = 0.7; + m_backgroundColorRGB[2] = 0.8; } virtual ~CommonGraphicsApp() { diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index be8d0f2e8..be280c877 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -123,6 +123,7 @@ static bool enable_experimental_opencl = false; int gDebugDrawFlags = 0; static bool pauseSimulation=false; +static bool singleStepSimulation = false; int midiBaseIndex = 176; extern bool gDisableDeactivation; @@ -227,6 +228,12 @@ void MyKeyboardCallback(int key, int state) { pauseSimulation = !pauseSimulation; } + if (key == 'o' && state) + { + singleStepSimulation = true; + } + + #ifndef NO_OPENGL3 if (key=='s' && state) { @@ -1124,12 +1131,12 @@ void OpenGLExampleBrowser::update(float deltaTime) if (sCurrentDemo) { - if (!pauseSimulation) + if (!pauseSimulation || singleStepSimulation) { + singleStepSimulation = false; //printf("---------------------------------------------------\n"); //printf("Framecount = %d\n",frameCount); - - + if (gFixedTimeStep>0) { sCurrentDemo->stepSimulation(gFixedTimeStep); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index d4aa246dc..e49369547 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -84,7 +84,10 @@ public: virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) { - drawLine(PointOnB,PointOnB+normalOnB,color); + drawLine(PointOnB,PointOnB+normalOnB*distance,color); + btVector3 red(0.3, 1., 0.3); + drawLine(PointOnB, PointOnB + normalOnB*0.01, red); + } diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 84b5fc7db..7db87b26a 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -58,7 +58,7 @@ public: virtual void physicsDebugDraw(int debugDrawMode) { - + m_robotSim.debugDraw(debugDrawMode); } virtual void initPhysics() { @@ -81,7 +81,16 @@ public: slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - + { + b3RobotSimLoadFileArgs args(""); + b3RobotSimLoadFileResults results; + args.m_fileName = "cube_small.urdf"; + args.m_startPosition.setValue(0, 0, .107); + args.m_startOrientation.setEulerZYX(0, 0, 0); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + } + { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; @@ -129,15 +138,7 @@ public: } } } - { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; - args.m_startPosition.setValue(0,0,.107); - args.m_startOrientation.setEulerZYX(0,0,0); - args.m_useMultiBody = false; - m_robotSim.loadFile(args,results); - } + if (1) { b3RobotSimLoadFileArgs args(""); @@ -179,19 +180,20 @@ public: m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } - m_robotSim.stepSimulation(); + m_robotSim.stepSimulation(); } virtual void renderScene() { m_robotSim.renderScene(); //m_app->m_renderer->renderScene(); + } virtual void physicsDebugDraw() { - + } virtual bool mouseMoveCallback(float x,float y) { diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 15cc08a21..4c825acab 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -7,15 +7,15 @@ //#include "../CommonInterfaces/CommonExampleInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" +#include "../SharedMemory/PhysicsDirectC_API.h" +#include "../SharedMemory/PhysicsDirect.h" + #include - - - - - - #include "../Utils/b3Clock.h" + + #include "../MultiThreading/b3ThreadSupportInterface.h" @@ -406,24 +406,27 @@ public: - - - struct b3RobotSimAPI_InternalData { //GUIHelperInterface* m_guiHelper; PhysicsServerSharedMemory m_physicsServer; b3PhysicsClientHandle m_physicsClient; + b3ThreadSupportInterface* m_threadSupport; RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper; + PhysicsDirect* m_clientServerDirect; + bool m_useMultiThreading; bool m_connected; b3RobotSimAPI_InternalData() - :m_multiThreadedHelper(0), + :m_threadSupport(0), + m_multiThreadedHelper(0), + m_clientServerDirect(0), m_physicsClient(0), + m_useMultiThreading(false), m_connected(false) { } @@ -464,6 +467,9 @@ b3RobotSimAPI::~b3RobotSimAPI() void b3RobotSimAPI::processMultiThreadedGraphicsRequests() { + if (0==m_data->m_multiThreadedHelper) + return; + switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) { case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject: @@ -563,27 +569,6 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests() } } - - - - #if 0 - if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) - { - btClock rtc; - btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); - - while (rtc.getTimeMilliseconds()m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - - MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - - - - - m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); - - for (int i=0;im_threadSupport->getNumTasks();i++) + if (m_data->m_useMultiThreading) { - RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i); - b3Assert(storage); - storage->threadId = i; - //storage->m_sharedMem = data->m_sharedMem; - } + m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper); + + MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper); - - for (int w=0;wm_args[w].m_cs = m_data->m_threadSupport->createCriticalSection(); - m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized); - int numMoving = 0; - m_data->m_args[w].m_positions.resize(numMoving); - m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer; - int index = 0; - - m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w); - while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized) + + m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); + + for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++) { + RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i); + b3Assert(storage); + storage->threadId = i; + //storage->m_sharedMem = data->m_sharedMem; } + + + + + for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++) + { + m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection(); + m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized); + int numMoving = 0; + m_data->m_args[w].m_positions.resize(numMoving); + m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer; + int index = 0; + + m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w); + while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized) + { + } + } + + m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); + + bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper); + b3Assert(serverConnected); + + m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + } + else + { + m_data->m_clientServerDirect = new PhysicsDirect(); + bool connected = m_data->m_clientServerDirect->connect(guiHelper); + m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect; + } - m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); - - m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper); - - b3Assert(m_data->m_connected); - - m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); - int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient); - b3Assert(canSubmit); - return m_data->m_connected && canSubmit; + //client connected + m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient); + + b3Assert(m_data->m_connected); + return m_data->m_connected && m_data->m_connected; } void b3RobotSimAPI::disconnect() { - - for (int i=0;im_useMultiThreading) { - m_data->m_args[i].m_cs->lock(); - m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim); - m_data->m_args[i].m_cs->unlock(); + for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++) + { + m_data->m_args[i].m_cs->lock(); + m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim); + m_data->m_args[i].m_cs->unlock(); + } + int numActiveThreads = MAX_ROBOT_NUM_THREADS; + + while (numActiveThreads) + { + int arg0, arg1; + if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0)) + { + numActiveThreads--; + printf("numActiveThreads = %d\n", numActiveThreads); + + } + else + { + } + }; + + printf("stopping threads\n"); + + delete m_data->m_threadSupport; + m_data->m_threadSupport = 0; } - int numActiveThreads = MAX_ROBOT_NUM_THREADS; - - while (numActiveThreads) - { - int arg0,arg1; - if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0)) - { - numActiveThreads--; - printf("numActiveThreads = %d\n",numActiveThreads); - - } else - { - } - }; - - printf("stopping threads\n"); - - delete m_data->m_threadSupport; - m_data->m_threadSupport = 0; - b3DisconnectSharedMemory(m_data->m_physicsClient); m_data->m_physicsServer.disconnectSharedMemory(true); + m_data->m_connected = false; } + +void b3RobotSimAPI::debugDraw(int debugDrawMode) +{ + if (m_data->m_clientServerDirect) + { + m_data->m_clientServerDirect->debugDraw(debugDrawMode); + } +} void b3RobotSimAPI::renderScene() { - if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + if (m_data->m_useMultiThreading) { - m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + } } + if (m_data->m_clientServerDirect) + { + m_data->m_clientServerDirect->renderScene(); + } m_data->m_physicsServer.renderScene(); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 9a4a14831..d435b2925 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -95,6 +95,7 @@ public: void setGravity(const b3Vector3& gravityAcceleration); void renderScene(); + void debugDraw(int debugDrawMode); }; #endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 05c8c0eed..948b90b90 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -69,9 +69,27 @@ PhysicsDirect::~PhysicsDirect() bool PhysicsDirect::connect() { m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + return true; } +// return true if connection succesfull, can also check 'isConnected' +bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper) +{ + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return true; +} + +void PhysicsDirect::renderScene() +{ + m_data->m_commandProcessor->renderScene(); +} +void PhysicsDirect::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + ////todo: rename to 'disconnect' void PhysicsDirect::disconnectSharedMemory() { diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index aaa2d8639..7e3ae2c94 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -29,9 +29,10 @@ public: virtual ~PhysicsDirect(); - // return true if connection succesfull, can also check 'isConnected' + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper virtual bool connect(); - + ////todo: rename to 'disconnect' virtual void disconnectSharedMemory(); @@ -62,7 +63,12 @@ public: virtual const float* getDebugLinesColor() const; virtual void getCachedCameraImage(b3CameraImageData* cameraData); - + + //those 2 APIs are for internal use for visualization + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + }; #endif //PHYSICS_DIRECT_H diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h index 58c9838c4..a020c3f4e 100644 --- a/src/LinearMath/btIDebugDraw.h +++ b/src/LinearMath/btIDebugDraw.h @@ -166,9 +166,9 @@ class btIDebugDraw virtual void drawTransform(const btTransform& transform, btScalar orthoLen) { btVector3 start = transform.getOrigin(); - drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); + drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f)); } virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,