From 9fef6c1d6612ec4f593d28ea68c4c04253413d23 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 May 2017 22:35:33 -0700 Subject: [PATCH] Add C++ version VRGloveSimulatorMain example, using the serial library. First run the App_PhysicsServer_SharedMemory_VR_vs2010.exe to run the VR server, then run App_VRGloveHandSimulator. You likely need to tune the minV/maxV for each finger (check values using Arduino IDE Serial Monitor) --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 1 - examples/RobotSimulator/MinitaurSetup.cpp | 1 - .../RobotSimulator/RobotSimulatorMain.cpp | 23 +- .../RobotSimulator/VRGloveSimulatorMain.cpp | 295 ++++++++++++++++++ .../b3RobotSimulatorClientAPI.cpp | 71 ++++- .../b3RobotSimulatorClientAPI.h | 11 +- examples/RobotSimulator/premake4.lua | 209 +++++++++---- .../PhysicsServerCommandProcessor.cpp | 5 +- examples/SharedMemory/SharedMemoryPublic.h | 14 +- examples/TinyAudio/b3AudioListener.cpp | 17 +- examples/TinyAudio/b3SoundEngine.cpp | 1 - examples/Utils/b3HashString.h | 59 ---- src/Bullet3Common/b3HashMap.h | 56 ++++ 13 files changed, 610 insertions(+), 153 deletions(-) create mode 100644 examples/RobotSimulator/VRGloveSimulatorMain.cpp delete mode 100644 examples/Utils/b3HashString.h diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 4751c7be4b..9bbcafb645 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -48,7 +48,6 @@ //quick test for file import, @todo(erwincoumans) make it more general and add other file formats #include "../Importers/ImportURDFDemo/ImportURDFSetup.h" #include "../Importers/ImportBullet/SerializeSetup.h" -#include "../Utils/b3HashString.h" #include "Bullet3Common/b3HashMap.h" struct GL3TexLoader : public MyTextureLoader diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index 9f78c02836..e867d05837 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -2,7 +2,6 @@ #include "b3RobotSimulatorClientAPI.h" #include "Bullet3Common/b3HashMap.h" -#include "../Utils/b3HashString.h" struct MinitaurSetupInternalData { diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index fb1072fd0b..b84cb60a6d 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -16,7 +16,7 @@ int main(int argc, char* argv[]) //sim->connect(eCONNECT_UDP, "localhost", 1234); sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); // sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME - sim->setTimeOut(12345); + sim->setTimeOut(10); //syncBodies is only needed when connecting to an existing physics server that has already some bodies sim->syncBodies(); b3Scalar fixedTimeStep = 1./240.; @@ -33,18 +33,14 @@ int main(int argc, char* argv[]) //b3BodyInfo bodyInfo; //sim->getBodyInfo(blockId,&bodyInfo); - sim->loadURDF("plane_with_collision_audio.urdf"); + sim->loadURDF("plane.urdf"); - b3RobotSimulatorLoadUrdfFileArgs args; - args.m_startPosition.setValue(0,0,2); - for (int i=0;i<10;i++) - { - args.m_startPosition[0] = 0.5*i; - args.m_startPosition[1] = 0.5*i; - args.m_startPosition[2] = 2+i*2; - int r2d2 = sim->loadURDF("cube.urdf",args); - } + MinitaurSetup minitaur; + int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3)); + + //b3RobotSimulatorLoadUrdfFileArgs args; + //args.m_startPosition.setValue(2,0,1); //int r2d2 = sim->loadURDF("r2d2.urdf",args); //b3RobotSimulatorLoadFileResults sdfResults; @@ -127,8 +123,8 @@ int main(int argc, char* argv[]) yaw+=0.1; b3Vector3 basePos; b3Quaternion baseOrn; -// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn); - // sim->resetDebugVisualizerCamera(distance,yaw,20,basePos); + sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn); + sim->resetDebugVisualizerCamera(distance,yaw,20,basePos); } b3Clock::usleep(1000.*1000.*fixedTimeStep); } @@ -144,4 +140,3 @@ int main(int argc, char* argv[]) } - diff --git a/examples/RobotSimulator/VRGloveSimulatorMain.cpp b/examples/RobotSimulator/VRGloveSimulatorMain.cpp new file mode 100644 index 0000000000..00c3a26d2d --- /dev/null +++ b/examples/RobotSimulator/VRGloveSimulatorMain.cpp @@ -0,0 +1,295 @@ +//VR Glove hand simulator is a C++ conversion from the Python pybullet vrhand_vive_tracker.py +//For more details about the VR glove, see also https://docs.google.com/document/d/1_qwXJRBTGKmhktdBtVQ6zdOdxwud1K30jt0G5dkAr10/edit + +#include "b3RobotSimulatorClientAPI.h" +#include "../Utils/b3Clock.h" +#include "Bullet3Common/b3CommandLineArgs.h" +#include +#include +#include +#include "serial/serial.h" +#include "../Importers/ImportURDFDemo/urdfStringSplit.h" + +double convertSensor(int inputV, int minV, int maxV) +{ + b3Clamp(inputV,minV,maxV); + double outVal =(double)inputV; + double b = (outVal-(double)minV)/float(maxV-minV); + return (b); +} + +void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int linkIndex, double targetPosition) +{ + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); + controlArgs.m_maxTorqueValue = 50.; + controlArgs.m_targetPosition = targetPosition; + controlArgs.m_targetVelocity = 0; + sim->setJointMotorControl(obUid,linkIndex,controlArgs); +} + +int main(int argc, char* argv[]) +{ + b3CommandLineArgs args(argc,argv); + std::string port="COM10"; + args.GetCmdLineArgument("port",port); + int baud = 115200; + args.GetCmdLineArgument("speed",baud); + std::string mode = "SHARED_MEMORY"; + args.GetCmdLineArgument("mode",mode); + int disableGui = 0; + args.GetCmdLineArgument("disableGui",disableGui); + int disableShadows = 0; + args.GetCmdLineArgument("disableShadows",disableShadows); + + printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud,mode.c_str()); + + // port, baudrate, timeout in milliseconds + serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1)); + if (!my_serial.isOpen()) + { + printf("Cannot open serial port\n"); + return -1; + } + + b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI(); + + //Can also use eCONNECT_UDP,eCONNECT_TCP, for example: sim->connect(eCONNECT_UDP, "localhost", 1234); + if (mode=="GUI") + { + sim->connect(eCONNECT_GUI); + } else + { + if (mode=="DIRECT") + { + sim->connect(eCONNECT_DIRECT); + } else + { + sim->connect(eCONNECT_SHARED_MEMORY); + } + } + + sim->setRealTimeSimulation(true); + sim->setInternalSimFlags(0); + sim->resetSimulation(); + + if (disableGui) + { + sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); + } + + if (disableShadows) + { + sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0); + } + + sim->setTimeOut(12345); + //syncBodies is only needed when connecting to an existing physics server that has already some bodies + sim->syncBodies(); + b3Scalar fixedTimeStep = 1./240.; + + sim->setTimeStep(fixedTimeStep); + + b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3)); + b3Vector3 rpy; + rpy = sim->getEulerFromQuaternion(q); + + sim->setGravity(b3MakeVector3(0,0,-9.8)); + sim->setContactBreakingThreshold(0.001); + + sim->loadURDF("plane_with_collision_audio.urdf"); + + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107))); + sim->loadURDF("table/table.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.200000,0.000000),b3Quaternion(0.000000,0.000000,0.707107,0.707107))); + sim->loadURDF("cube_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3( 0.950000,-0.100000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107))); + sim->loadURDF("sphere_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000,-0.400000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107))); + + int handUid = -1; + + b3RobotSimulatorLoadFileResults mjcfResults; + const char* mjcfFileName = "MPL/mpl2.xml"; + if (sim->loadMJCF(mjcfFileName,mjcfResults)) + { + printf("mjcfResults = %d\n", mjcfResults.m_uniqueObjectIds.size()); + if (mjcfResults.m_uniqueObjectIds.size()==1) + { + handUid = mjcfResults.m_uniqueObjectIds[0]; + } + } + if (handUid<0) + { + printf("Cannot load MJCF file %s\n", mjcfFileName); + } + + b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14,-3.14/2,0)); + b3Vector3 handPos = b3MakeVector3(-0.05,0,0.02); + + b3JointInfo jointInfo; + jointInfo.m_jointType = eFixedType; + b3Vector3 handStartPosWorld = b3MakeVector3(0.500000,0.300006,0.900000); + b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity(); + + jointInfo.m_childFrame[0] = handStartPosWorld[0]; + jointInfo.m_childFrame[1] = handStartPosWorld[1]; + jointInfo.m_childFrame[2] = handStartPosWorld[2]; + jointInfo.m_childFrame[3] = handStartOrnWorld[0]; + jointInfo.m_childFrame[4] = handStartOrnWorld[1]; + jointInfo.m_childFrame[5] = handStartOrnWorld[2]; + jointInfo.m_childFrame[6] = handStartOrnWorld[3]; + + jointInfo.m_parentFrame[0] = handPos[0]; + jointInfo.m_parentFrame[1] = handPos[1]; + jointInfo.m_parentFrame[2] = handPos[2]; + jointInfo.m_parentFrame[3] = handOrn[0]; + jointInfo.m_parentFrame[4] = handOrn[1]; + jointInfo.m_parentFrame[5] = handOrn[2]; + jointInfo.m_parentFrame[6] = handOrn[3]; + + int handConstraintId = sim->createConstraint(handUid,-1,-1,-1,&jointInfo); + double maxFingerForce = 50; + + for (int j=0; jgetNumJoints(handUid);j++) + { + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); + controlArgs.m_maxTorqueValue = maxFingerForce ; + controlArgs.m_targetPosition = 0; + controlArgs.m_targetVelocity = 0; + sim->setJointMotorControl(handUid,j,controlArgs); + } + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double simWallClockSeconds = 20.; + + int vidLogId = -1; + int minitaurLogId = -1; + int rotateCamera = 0; + + while (sim->canSubmitCommand()) + { + b3VREventsData vrEvents; + int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER; + + sim->getVREvents(&vrEvents, deviceTypeFilter); + //instead of iterating over all vr events, we just take the most up-to-date one + if (vrEvents.m_numControllerEvents) + { + int i = vrEvents.m_numControllerEvents - 1; + b3VRControllerEvent& e = vrEvents.m_controllerEvents[i]; +// printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]); + b3JointInfo changeConstraintInfo; + changeConstraintInfo.m_flags = 0; + changeConstraintInfo.m_jointMaxForce = 50; + changeConstraintInfo.m_flags |= eJointChangeMaxForce; + + changeConstraintInfo.m_childFrame[0] = e.m_pos[0]; + changeConstraintInfo.m_childFrame[1] = e.m_pos[1]; + changeConstraintInfo.m_childFrame[2] = e.m_pos[2]; + changeConstraintInfo.m_flags |= eJointChangeChildFramePosition; + + changeConstraintInfo.m_childFrame[3] = e.m_orn[0]; + changeConstraintInfo.m_childFrame[4] = e.m_orn[1]; + changeConstraintInfo.m_childFrame[5] = e.m_orn[2]; + changeConstraintInfo.m_childFrame[6] = e.m_orn[3]; + changeConstraintInfo.m_flags |= eJointChangeChildFrameOrientation; + + sim->changeConstraint(handConstraintId, &changeConstraintInfo); + } + + //read the serial output from the hand, extract into parts + std::string result = my_serial.readline(); + //my_serial.flush(); + if (result.length()) + { + int res = result.find("\n"); + while (res<0) + { + result += my_serial.readline(); + res = result.find("\n"); + } + btAlignedObjectArray pieces; + btAlignedObjectArray separators; + separators.push_back(","); + urdfStringSplit( pieces, result, separators); + + //printf("serial: %s\n", result.c_str()); + if (pieces.size()==6) + { + double pinkTarget = 0; + double middleTarget = 0; + double indexTarget = 0; + double thumbTarget = 0; + { + int pink = atoi(pieces[1].c_str()); + int middle = atoi(pieces[2].c_str()); + int index = atoi(pieces[3].c_str()); + int thumb = atoi(pieces[4].c_str()); + + pinkTarget = convertSensor(pink,250,400); + middleTarget = convertSensor(middle,250,400); + indexTarget = convertSensor(index,250,400); + thumbTarget = convertSensor(thumb,250,400); + } + + //printf("pink = %d, middle=%d, index=%d, thumb=%d\n", pink,middle,index,thumb); + + setJointMotorPositionControl(sim,handUid,5,1.3); + setJointMotorPositionControl(sim,handUid,7,thumbTarget); + setJointMotorPositionControl(sim,handUid,9,thumbTarget); + setJointMotorPositionControl(sim,handUid,11,thumbTarget); + + setJointMotorPositionControl(sim,handUid,15,indexTarget); + setJointMotorPositionControl(sim,handUid,17,indexTarget); + setJointMotorPositionControl(sim,handUid,19,indexTarget); + + setJointMotorPositionControl(sim,handUid,22,middleTarget); + setJointMotorPositionControl(sim,handUid,24,middleTarget); + setJointMotorPositionControl(sim,handUid,27,middleTarget); + + double ringTarget = 0.5f*(pinkTarget+middleTarget); + setJointMotorPositionControl(sim,handUid,30,ringTarget); + setJointMotorPositionControl(sim,handUid,32,ringTarget); + setJointMotorPositionControl(sim,handUid,34,ringTarget); + + setJointMotorPositionControl(sim,handUid,38,pinkTarget); + setJointMotorPositionControl(sim,handUid,40,pinkTarget); + setJointMotorPositionControl(sim,handUid,42,pinkTarget); + } + } + + b3KeyboardEventsData keyEvents; + sim->getKeyboardEvents(&keyEvents); + + //sim->stepSimulation(); + + if (rotateCamera) + { + static double yaw=0; + double distance = 1; + yaw+=0.1; + b3Vector3 basePos; + b3Quaternion baseOrn; +// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn); +// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos); + } + //b3Clock::usleep(1000.*1000.*fixedTimeStep); + } + + printf("serial.close()\n"); + my_serial.close(); + + printf("sim->disconnect\n"); + sim->disconnect(); + + printf("delete sim\n"); + delete sim; + + printf("exit\n"); + +} diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index ebf515e924..17a4806cca 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -560,21 +560,70 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); } -void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) +int b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) { if (!isConnected()) { b3Warning("Not connected"); - return; + return -1; } b3SharedMemoryStatusHandle statusHandle; b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); + int statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_CONSTRAINT_COMPLETED) + { + int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); + return userConstraintUid; + } + } + return -1; +} + +int b3RobotSimulatorClientAPI::changeConstraint(int constraintId, b3JointInfo* jointInfo) +{ + + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); + + if (jointInfo->m_flags & eJointChangeMaxForce) + { + b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce); + } + + if (jointInfo->m_flags & eJointChangeChildFramePosition) + { + b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]); + } + if (jointInfo->m_flags & eJointChangeChildFrameOrientation) + { + b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]); + } + + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + int statusType = b3GetStatusType(statusHandle); + return statusType; +} + +void b3RobotSimulatorClientAPI::removeConstraint(int constraintId) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; } + b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + int statusType = b3GetStatusType(statusHandle); } + bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) { if (!isConnected()) @@ -685,6 +734,21 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } +void b3RobotSimulatorClientAPI::setContactBreakingThreshold(double threshold) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetContactBreakingThreshold(command,threshold); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + + void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) { if (!isConnected()) @@ -815,7 +879,7 @@ void b3RobotSimulatorClientAPI::configureDebugVisualizer(b3ConfigureDebugVisuali b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } -void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData) +void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter) { vrEventsData->m_numControllerEvents = 0; vrEventsData->m_controllerEvents = 0; @@ -826,6 +890,7 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData) } b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); + b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); } diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index 876c7048b3..f92fd6497e 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -132,6 +132,8 @@ class b3RobotSimulatorClientAPI struct b3RobotSimulatorClientAPI_InternalData* m_data; public: + + b3RobotSimulatorClientAPI(); virtual ~b3RobotSimulatorClientAPI(); @@ -167,7 +169,11 @@ class b3RobotSimulatorClientAPI bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); - void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + + int changeConstraint(int constraintId, b3JointInfo* jointInfo); + + void removeConstraint(int constraintId); bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); @@ -188,6 +194,7 @@ class b3RobotSimulatorClientAPI void setTimeStep(double timeStepInSeconds); void setNumSimulationSubSteps(int numSubSteps); void setNumSolverIterations(int numIterations); + void setContactBreakingThreshold(double threshold); bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); @@ -201,7 +208,7 @@ class b3RobotSimulatorClientAPI int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds=b3AlignedObjectArray(), int maxLogDof = -1); void stopStateLogging(int stateLoggerUniqueId); - void getVREvents(b3VREventsData* vrEventsData); + void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); }; diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index 88acdeaefe..df4cf88a58 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -1,3 +1,67 @@ + +myfiles = +{ + "../../examples/SharedMemory/IKTrajectoryHelper.cpp", + "../../examples/SharedMemory/IKTrajectoryHelper.h", + "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", + "../../examples/SharedMemory/InProcessMemory.cpp", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsServer.cpp", + "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.h", + "../../examples/SharedMemory/PhysicsDirect.cpp", + "../../examples/SharedMemory/PhysicsDirect.h", + "../../examples/SharedMemory/PhysicsDirectC_API.cpp", + "../../examples/SharedMemory/PhysicsDirectC_API.h", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + "../../examples/Utils/RobotLoggingUtil.cpp", + "../../examples/Utils/RobotLoggingUtil.h", + "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", + "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", + "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", + "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", + "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + "../../examples/MultiThreading/b3PosixThreadSupport.cpp", + "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", + "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", +} + project ("App_RobotSimulator") language "C++" @@ -128,66 +192,91 @@ if not _OPTIONS["no-enet"] then "b3RobotSimulatorClientAPI.h", "MinitaurSetup.cpp", "MinitaurSetup.h", - "../../examples/SharedMemory/IKTrajectoryHelper.cpp", - "../../examples/SharedMemory/IKTrajectoryHelper.h", - "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", - "../../examples/SharedMemory/InProcessMemory.cpp", - "../../examples/SharedMemory/PhysicsClient.cpp", - "../../examples/SharedMemory/PhysicsClient.h", - "../../examples/SharedMemory/PhysicsServer.cpp", - "../../examples/SharedMemory/PhysicsServer.h", - "../../examples/SharedMemory/PhysicsServerExample.cpp", - "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", - "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", - "../../examples/SharedMemory/PhysicsServerSharedMemory.h", - "../../examples/SharedMemory/PhysicsDirect.cpp", - "../../examples/SharedMemory/PhysicsDirect.h", - "../../examples/SharedMemory/PhysicsDirectC_API.cpp", - "../../examples/SharedMemory/PhysicsDirectC_API.h", - "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", - "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", - "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", - "../../examples/SharedMemory/PhysicsClientSharedMemory.h", - "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", - "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h", - "../../examples/SharedMemory/PhysicsClientC_API.cpp", - "../../examples/SharedMemory/PhysicsClientC_API.h", - "../../examples/SharedMemory/Win32SharedMemory.cpp", - "../../examples/SharedMemory/Win32SharedMemory.h", - "../../examples/SharedMemory/PosixSharedMemory.cpp", - "../../examples/SharedMemory/PosixSharedMemory.h", - "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", - "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", - "../../examples/TinyRenderer/geometry.cpp", - "../../examples/TinyRenderer/model.cpp", - "../../examples/TinyRenderer/tgaimage.cpp", - "../../examples/TinyRenderer/our_gl.cpp", - "../../examples/TinyRenderer/TinyRenderer.cpp", - "../../examples/Utils/b3ResourcePath.cpp", - "../../examples/Utils/b3ResourcePath.h", - "../../examples/Utils/RobotLoggingUtil.cpp", - "../../examples/Utils/RobotLoggingUtil.h", - "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", - "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", - "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", - "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", - "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", - "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", - "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", - "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", - "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", - "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", - "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", - "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", - "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", - "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", - "../../examples/MultiThreading/b3PosixThreadSupport.cpp", - "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", - "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", - "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", - } + myfiles + } + if os.is("Linux") then + initX11() + end + + + +project ("App_VRGloveHandSimulator") + + language "C++" + kind "ConsoleApp" + + includedirs {"../../src", "../../examples", + "../../examples/ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + + hasCL = findOpenCL("clew") + + links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"} + initOpenGL() + initGlew() + + includedirs { + ".", + "../../src", + "../ThirdPartyLibs", + } + + + if os.is("MacOSX") then + links{"Cocoa.framework"} + end + + if (hasCL) then + links { + "Bullet3OpenCL_clew", + "Bullet3Dynamics", + "Bullet3Collision", + "Bullet3Geometry", + "Bullet3Common", + } + end + + if _OPTIONS["audio"] then + files { + "../TinyAudio/b3ADSR.cpp", + "../TinyAudio/b3AudioListener.cpp", + "../TinyAudio/b3ReadWavFile.cpp", + "../TinyAudio/b3SoundEngine.cpp", + "../TinyAudio/b3SoundSource.cpp", + "../TinyAudio/b3WriteWavFile.cpp", + "../TinyAudio/RtAudio.cpp", + } + defines {"B3_ENABLE_TINY_AUDIO"} + + if _OPTIONS["serial"] then + defines{"B3_ENABLE_SERIAL"} + includedirs {"../../examples/ThirdPartyLibs/serial/include"} + links {"serial"} + end + + if os.is("Windows") then + links {"winmm","Wsock32","dsound"} + defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"} + end + + if os.is("Linux") then initX11() + defines {"__OS_LINUX__","__LINUX_ALSA__"} + links {"asound","pthread"} + end + + + if os.is("MacOSX") then + links{"Cocoa.framework"} + links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"} + defines {"__OS_MACOSX__","__MACOSX_CORE__"} + end + end + files { + "VRGloveSimulatorMain.cpp", + "b3RobotSimulatorClientAPI.cpp", + "b3RobotSimulatorClientAPI.h", + myfiles + } if os.is("Linux") then initX11() end diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1fcba6111b..762491389a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2196,9 +2196,10 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } } } - } #endif - std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + } + + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_data->m_strings.push_back(baseName); mb->setBaseName(baseName->c_str()); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 466f3dcb3f..f54aef52cf 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -167,6 +167,14 @@ enum JointType { ePoint2PointType = 5, }; + +enum b3JointInfoFlags +{ + eJointChangeMaxForce = 1, + eJointChangeChildFramePosition = 2, + eJointChangeChildFrameOrientation = 4, +}; + struct b3JointInfo { char* m_linkName; @@ -317,11 +325,7 @@ struct b3VREventsData { int m_numControllerEvents; struct b3VRControllerEvent* m_controllerEvents; - int m_numHmdEvents; - struct b3VRMoveEvent* m_hmdEvents; - - int m_numGenericTrackerEvents; - struct b3VRMoveEvent* m_genericTrackerEvents; + }; diff --git a/examples/TinyAudio/b3AudioListener.cpp b/examples/TinyAudio/b3AudioListener.cpp index 206df87fa4..7c4238595f 100644 --- a/examples/TinyAudio/b3AudioListener.cpp +++ b/examples/TinyAudio/b3AudioListener.cpp @@ -20,10 +20,12 @@ struct b3AudioListenerInternalData b3WriteWavFile m_wavOut2; + bool m_writeWavOut; b3AudioListenerInternalData() :m_numControlTicks(64), - m_sampleRate(B3_SAMPLE_RATE) + m_sampleRate(B3_SAMPLE_RATE), + m_writeWavOut(false) { for (int i=0;im_wavOut2.setWavFile("bulletAudio2.wav",B3_SAMPLE_RATE,2,false); + if (m_data->m_writeWavOut) + { + m_data->m_wavOut2.setWavFile("bulletAudio2.wav",B3_SAMPLE_RATE,2,false); + } } b3AudioListener::~b3AudioListener() { - m_data->m_wavOut2.closeWavFile(); + if (m_data->m_writeWavOut) + { + m_data->m_wavOut2.closeWavFile(); + } delete m_data; } @@ -154,7 +161,7 @@ int b3AudioListener::tick(void *outputBuffer,void *inputBuffer1,unsigned int nBu } //logging to wav file - if (numSamples) + if (data->m_writeWavOut && numSamples) { data->m_wavOut2.tick( (double *)outputBuffer,numSamples); } diff --git a/examples/TinyAudio/b3SoundEngine.cpp b/examples/TinyAudio/b3SoundEngine.cpp index 478e0efc84..34f92b876d 100644 --- a/examples/TinyAudio/b3SoundEngine.cpp +++ b/examples/TinyAudio/b3SoundEngine.cpp @@ -7,7 +7,6 @@ #include "Bullet3Common/b3AlignedObjectArray.h" #include "b3ReadWavFile.h" #include "../Utils/b3ResourcePath.h" -#include "../Utils/b3HashString.h" #include "Bullet3Common/b3HashMap.h" diff --git a/examples/Utils/b3HashString.h b/examples/Utils/b3HashString.h deleted file mode 100644 index 9cae98c6de..0000000000 --- a/examples/Utils/b3HashString.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef B3_HASH_STRING_H -#define B3_HASH_STRING_H - -#include - -///very basic hashable string implementation, compatible with b3HashMap -struct b3HashString -{ - std::string m_string; - unsigned int m_hash; - - B3_FORCE_INLINE unsigned int getHash()const - { - return m_hash; - } - - - b3HashString(const char* name) - :m_string(name) - { - - /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */ - static const unsigned int InitialFNV = 2166136261u; - static const unsigned int FNVMultiple = 16777619u; - - /* Fowler / Noll / Vo (FNV) Hash */ - unsigned int hash = InitialFNV; - - for(int i = 0; m_string[i]; i++) - { - hash = hash ^ (m_string[i]); /* xor the low 8 bits */ - hash = hash * FNVMultiple; /* multiply by the magic number */ - } - m_hash = hash; - } - - int portableStringCompare(const char* src, const char* dst) const - { - int ret = 0 ; - - while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst) - ++src, ++dst; - - if ( ret < 0 ) - ret = -1 ; - else if ( ret > 0 ) - ret = 1 ; - - return( ret ); - } - - bool equals(const b3HashString& other) const - { - return (m_string == other.m_string); - } - -}; - -#endif //B3_HASH_STRING_H diff --git a/src/Bullet3Common/b3HashMap.h b/src/Bullet3Common/b3HashMap.h index 15281ed904..dc35791179 100644 --- a/src/Bullet3Common/b3HashMap.h +++ b/src/Bullet3Common/b3HashMap.h @@ -20,6 +20,62 @@ subject to the following restrictions: #include "b3AlignedObjectArray.h" +#include + +///very basic hashable string implementation, compatible with b3HashMap +struct b3HashString +{ + std::string m_string; + unsigned int m_hash; + + B3_FORCE_INLINE unsigned int getHash()const + { + return m_hash; + } + + + b3HashString(const char* name) + :m_string(name) + { + + /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */ + static const unsigned int InitialFNV = 2166136261u; + static const unsigned int FNVMultiple = 16777619u; + + /* Fowler / Noll / Vo (FNV) Hash */ + unsigned int hash = InitialFNV; + + for(int i = 0; m_string[i]; i++) + { + hash = hash ^ (m_string[i]); /* xor the low 8 bits */ + hash = hash * FNVMultiple; /* multiply by the magic number */ + } + m_hash = hash; + } + + int portableStringCompare(const char* src, const char* dst) const + { + int ret = 0 ; + + while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst) + ++src, ++dst; + + if ( ret < 0 ) + ret = -1 ; + else if ( ret > 0 ) + ret = 1 ; + + return( ret ); + } + + bool equals(const b3HashString& other) const + { + return (m_string == other.m_string); + } + +}; + + const int B3_HASH_NULL=0xffffffff;