From 5436b8f048887b8f2c86059442a1041d076ded4b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 28 May 2017 17:05:18 -0700 Subject: [PATCH] print better error warning, in case the physics client/server version mismatch. fix in b3HashString remove many unused dependencies from kuka_grasp_block_playback.py (time,math, datetime ,numpy,pylab ,sys, os, fnmatch,argparse were not used!) move block_grasp_log.bin from Bullet3/data to Bullet3/examples/pybullet/examples/data folder. PhysicsServerCommandProcessor, derive from CommandProcessorInterface to prepare for different back-end implementation --- .../PhysicsCommandProcessorInterface.h | 25 +++++++++++++++++- examples/SharedMemory/PhysicsServer.h | 14 +++++----- .../PhysicsServerCommandProcessor.h | 12 ++++----- .../SharedMemory/PhysicsServerExample.cpp | 10 +++---- .../PhysicsServerSharedMemory.cpp | 9 ++++--- .../SharedMemory/PhysicsServerSharedMemory.h | 1 - .../SharedMemoryCommandProcessor.cpp | 10 ++++++- .../examples/data}/block_grasp_log.bin | Bin .../examples/kuka_grasp_block_playback.py | 10 +------ src/Bullet3Common/b3HashMap.h | 4 +-- 10 files changed, 59 insertions(+), 36 deletions(-) rename {data => examples/pybullet/examples/data}/block_grasp_log.bin (100%) diff --git a/examples/SharedMemory/PhysicsCommandProcessorInterface.h b/examples/SharedMemory/PhysicsCommandProcessorInterface.h index 70ed2d044..4bfebf9f3 100644 --- a/examples/SharedMemory/PhysicsCommandProcessorInterface.h +++ b/examples/SharedMemory/PhysicsCommandProcessorInterface.h @@ -1,7 +1,7 @@ #ifndef PHYSICS_COMMAND_PROCESSOR_INTERFACE_H #define PHYSICS_COMMAND_PROCESSOR_INTERFACE_H -enum PhysicsCOmmandRenderFlags +enum PhysicsCommandRenderFlags { COV_DISABLE_SYNC_RENDERING=1 }; @@ -29,4 +29,27 @@ public: }; + +class btVector3; + +class CommandProcessorInterface : public PhysicsCommandProcessorInterface +{ + +public: + virtual ~CommandProcessorInterface(){} + + virtual void syncPhysicsToGraphics()=0; + virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)=0; + virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0; + + virtual void enableCommandLogging(bool enable, const char* fileName)=0; + virtual void replayFromLogFile(const char* fileName)=0; + virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes )=0; + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; + virtual void removePickingConstraint()=0; + +}; + #endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h index 7b75a36cf..423d228b5 100644 --- a/examples/SharedMemory/PhysicsServer.h +++ b/examples/SharedMemory/PhysicsServer.h @@ -25,18 +25,18 @@ public: //@todo(erwincoumans) Should we have shared memory commands for picking objects? ///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise - virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; - virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; - virtual void removePickingConstraint()=0; + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;} + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;} + virtual void removePickingConstraint(){} //for physicsDebugDraw and renderScene are mainly for debugging purposes //and for physics visualization. The idea is that physicsDebugDraw can also send wireframe //to a physics client, over shared memory - virtual void physicsDebugDraw(int debugDrawFlags)=0; - virtual void renderScene(int renderFlags)=0; + virtual void physicsDebugDraw(int debugDrawFlags){} + virtual void renderScene(int renderFlags){} - virtual void enableCommandLogging(bool enable, const char* fileName)=0; - virtual void replayFromLogFile(const char* fileName)=0; + virtual void enableCommandLogging(bool enable, const char* fileName){} + virtual void replayFromLogFile(const char* fileName){} }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 6b0f7a58f..304740d49 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -15,7 +15,7 @@ struct SharedMemLines ///todo: naming. Perhaps PhysicsSdkCommandprocessor? -class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface +class PhysicsServerCommandProcessor : public CommandProcessorInterface { struct PhysicsServerCommandProcessorInternalData* m_data; @@ -91,16 +91,16 @@ public: virtual void removePickingConstraint(); //logging /playback the shared memory commands - void enableCommandLogging(bool enable, const char* fileName); - void replayFromLogFile(const char* fileName); - void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); + virtual void enableCommandLogging(bool enable, const char* fileName); + virtual void replayFromLogFile(const char* fileName); + virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); //logging of object states (position etc) void logObjectStates(btScalar timeStep); void processCollisionForces(btScalar timeStep); - void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents); - void enableRealTimeSimulation(bool enableRealTimeSim); + virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents); + virtual void enableRealTimeSimulation(bool enableRealTimeSim); void applyJointDamping(int bodyUniqueId); virtual void setTimeOut(double timeOutInSeconds); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 772dbd6d4..9c70e07da 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -600,7 +600,7 @@ struct ColorWidth ATTRIBUTE_ALIGNED16( class )MultithreadedDebugDrawer : public btIDebugDraw { - class GUIHelperInterface* m_guiHelper; + struct GUIHelperInterface* m_guiHelper; int m_debugMode; btAlignedObjectArray< btAlignedObjectArray > m_sortedIndices; @@ -1776,22 +1776,22 @@ void PhysicsServerExample::updateGraphics() if (flag==COV_ENABLE_VR_TELEPORTING) { - gEnableTeleporting = enable; + gEnableTeleporting = (enable!=0); } if (flag == COV_ENABLE_VR_PICKING) { - gEnablePicking = enable; + gEnablePicking = (enable!=0); } if (flag ==COV_ENABLE_SYNC_RENDERING_INTERNAL) { - gEnableSyncPhysicsRendering = enable; + gEnableSyncPhysicsRendering = (enable!=0); } if (flag == COV_ENABLE_RENDERING) { - gEnableRendering = enable; + gEnableRendering = (enable!=0); } m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag,m_multiThreadedHelper->m_visualizerEnable); diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 225076910..3e9e42dc1 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -30,7 +30,7 @@ struct PhysicsServerSharedMemoryInternalData int m_sharedMemoryKey; bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS]; bool m_verboseOutput; - PhysicsServerCommandProcessor* m_commandProcessor; + CommandProcessorInterface* m_commandProcessor; PhysicsServerSharedMemoryInternalData() :m_sharedMemory(0), @@ -89,7 +89,7 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar PhysicsServerSharedMemory::~PhysicsServerSharedMemory() { - m_data->m_commandProcessor->deleteDynamicsWorld(); + //m_data->m_commandProcessor->deleteDynamicsWorld(); delete m_data->m_commandProcessor; if (m_data->m_sharedMemory) @@ -109,11 +109,12 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory() delete m_data; } -void PhysicsServerSharedMemory::resetDynamicsWorld() +/*void PhysicsServerSharedMemory::resetDynamicsWorld() { m_data->m_commandProcessor->deleteDynamicsWorld(); m_data->m_commandProcessor ->createEmptyDynamicsWorld(); } +*/ void PhysicsServerSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; @@ -188,7 +189,7 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) { - m_data->m_commandProcessor->deleteDynamicsWorld(); + //m_data->m_commandProcessor->deleteDynamicsWorld(); m_data->m_commandProcessor->setGuiHelper(0); diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index e3dc7facf..68fa376bb 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -48,7 +48,6 @@ public: void enableCommandLogging(bool enable, const char* fileName); void replayFromLogFile(const char* fileName); - void resetDynamicsWorld(); }; diff --git a/examples/SharedMemory/SharedMemoryCommandProcessor.cpp b/examples/SharedMemory/SharedMemoryCommandProcessor.cpp index abd4879a2..29c2eb865 100644 --- a/examples/SharedMemory/SharedMemoryCommandProcessor.cpp +++ b/examples/SharedMemory/SharedMemoryCommandProcessor.cpp @@ -73,7 +73,15 @@ bool SharedMemoryCommandProcessor::connect() if (m_data->m_testBlock1) { if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { - b3Error("Error: please start server before client\n"); + + if ((m_data->m_testBlock1->m_magicId < 211705023) && + (m_data->m_testBlock1->m_magicId >=201705023)) + { + b3Error("Error: physics server version mismatch (expected %d got %d)\n",SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId); + } else + { + b3Error("Error connecting to shared memory: please start server before client\n"); + } m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); m_data->m_testBlock1 = 0; diff --git a/data/block_grasp_log.bin b/examples/pybullet/examples/data/block_grasp_log.bin similarity index 100% rename from data/block_grasp_log.bin rename to examples/pybullet/examples/data/block_grasp_log.bin diff --git a/examples/pybullet/examples/kuka_grasp_block_playback.py b/examples/pybullet/examples/kuka_grasp_block_playback.py index d22335975..6a786ad0d 100644 --- a/examples/pybullet/examples/kuka_grasp_block_playback.py +++ b/examples/pybullet/examples/kuka_grasp_block_playback.py @@ -1,14 +1,6 @@ import pybullet as p -import time -import math -from datetime import datetime -from numpy import * -from pylab import * import struct -import sys -import os, fnmatch -import argparse -from time import sleep + def readLogFile(filename, verbose = True): f = open(filename, 'rb') diff --git a/src/Bullet3Common/b3HashMap.h b/src/Bullet3Common/b3HashMap.h index ceeb838e2..24a59d9ba 100644 --- a/src/Bullet3Common/b3HashMap.h +++ b/src/Bullet3Common/b3HashMap.h @@ -44,8 +44,8 @@ struct b3HashString /* Fowler / Noll / Vo (FNV) Hash */ unsigned int hash = InitialFNV; - - for(int i = 0; m_string[i]; i++) + int len = m_string.length(); + for(int i = 0; i