Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add C++ version VRGloveSimulatorMain example, using the serial library.

Browse Source 
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)
This commit is contained in:
Erwin Coumans
2017-05-01 22:35:33 -07:00
parent 305725e52e
commit 9fef6c1d66
13 changed files with 610 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -48,7 +48,6 @@
//quick test for file import, @todo(erwincoumans) make it more general and add other file formats //quick test for file import, @todo(erwincoumans) make it more general and add other file formats
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h" #include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportBullet/SerializeSetup.h" #include "../Importers/ImportBullet/SerializeSetup.h"
#include "../Utils/b3HashString.h"
#include "Bullet3Common/b3HashMap.h" #include "Bullet3Common/b3HashMap.h"
struct GL3TexLoader : public MyTextureLoader struct GL3TexLoader : public MyTextureLoader

1
examples/RobotSimulator/MinitaurSetup.cpp
View File

@@ -2,7 +2,6 @@
#include "b3RobotSimulatorClientAPI.h" #include "b3RobotSimulatorClientAPI.h"
#include "Bullet3Common/b3HashMap.h" #include "Bullet3Common/b3HashMap.h"
#include "../Utils/b3HashString.h"
struct MinitaurSetupInternalData struct MinitaurSetupInternalData
{ {

23
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

@@ -16,7 +16,7 @@ int main(int argc, char* argv[])
//sim->connect(eCONNECT_UDP, "localhost", 1234); //sim->connect(eCONNECT_UDP, "localhost", 1234);
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME // 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 //syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies(); sim->syncBodies();
b3Scalar fixedTimeStep = 1./240.; b3Scalar fixedTimeStep = 1./240.;
@@ -33,18 +33,14 @@ int main(int argc, char* argv[])
//b3BodyInfo bodyInfo; //b3BodyInfo bodyInfo;
//sim->getBodyInfo(blockId,&bodyInfo); //sim->getBodyInfo(blockId,&bodyInfo);
sim->loadURDF("plane_with_collision_audio.urdf"); sim->loadURDF("plane.urdf");
b3RobotSimulatorLoadUrdfFileArgs args; MinitaurSetup minitaur;
args.m_startPosition.setValue(0,0,2); int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
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);
}
//b3RobotSimulatorLoadUrdfFileArgs args;
//args.m_startPosition.setValue(2,0,1);
//int r2d2 = sim->loadURDF("r2d2.urdf",args); //int r2d2 = sim->loadURDF("r2d2.urdf",args);
//b3RobotSimulatorLoadFileResults sdfResults; //b3RobotSimulatorLoadFileResults sdfResults;
@@ -127,8 +123,8 @@ int main(int argc, char* argv[])
yaw+=0.1; yaw+=0.1;
b3Vector3 basePos; b3Vector3 basePos;
b3Quaternion baseOrn; b3Quaternion baseOrn;
// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn); sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos); sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
} }
b3Clock::usleep(1000.*1000.*fixedTimeStep); b3Clock::usleep(1000.*1000.*fixedTimeStep);
} }
@@ -144,4 +140,3 @@ int main(int argc, char* argv[])
} }

295
examples/RobotSimulator/VRGloveSimulatorMain.cpp Normal file
View File

@@ -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 <string.h>
#include <stdio.h>
#include <assert.h>
#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; j<sim->getNumJoints(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<std::string> pieces;
btAlignedObjectArray<std::string> 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");
}

71
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -560,20 +560,69 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); 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()) if (!isConnected())
{ {
b3Warning("Not connected"); b3Warning("Not connected");
return; return -1;
} }
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
if (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)); 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) bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{ {
@@ -685,6 +734,21 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); 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) void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
{ {
if (!isConnected()) if (!isConnected())
@@ -815,7 +879,7 @@ void b3RobotSimulatorClientAPI::configureDebugVisualizer(b3ConfigureDebugVisuali
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
} }
void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData) void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter)
{ {
vrEventsData->m_numControllerEvents = 0; vrEventsData->m_numControllerEvents = 0;
vrEventsData->m_controllerEvents = 0; vrEventsData->m_controllerEvents = 0;
@@ -826,6 +890,7 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
} }
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
} }

11
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -132,6 +132,8 @@ class b3RobotSimulatorClientAPI
struct b3RobotSimulatorClientAPI_InternalData* m_data; struct b3RobotSimulatorClientAPI_InternalData* m_data;
public: public:
b3RobotSimulatorClientAPI(); b3RobotSimulatorClientAPI();
virtual ~b3RobotSimulatorClientAPI(); virtual ~b3RobotSimulatorClientAPI();
@@ -167,7 +169,11 @@ public:
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); 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); bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
@@ -188,6 +194,7 @@ public:
void setTimeStep(double timeStepInSeconds); void setTimeStep(double timeStepInSeconds);
void setNumSimulationSubSteps(int numSubSteps); void setNumSimulationSubSteps(int numSubSteps);
void setNumSolverIterations(int numIterations); void setNumSolverIterations(int numIterations);
void setContactBreakingThreshold(double threshold);
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
@@ -201,7 +208,7 @@ public:
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds=b3AlignedObjectArray<int>(), int maxLogDof = -1); int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds=b3AlignedObjectArray<int>(), int maxLogDof = -1);
void stopStateLogging(int stateLoggerUniqueId); void stopStateLogging(int stateLoggerUniqueId);
void getVREvents(b3VREventsData* vrEventsData); void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter);
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
}; };

207
examples/RobotSimulator/premake4.lua
View File

@@ -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") project ("App_RobotSimulator")
language "C++" language "C++"
@@ -128,65 +192,90 @@ if not _OPTIONS["no-enet"] then
"b3RobotSimulatorClientAPI.h", "b3RobotSimulatorClientAPI.h",
"MinitaurSetup.cpp", "MinitaurSetup.cpp",
"MinitaurSetup.h", "MinitaurSetup.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp", myfiles
"../../examples/SharedMemory/IKTrajectoryHelper.h", }
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", if os.is("Linux") then
"../../examples/SharedMemory/InProcessMemory.cpp", initX11()
"../../examples/SharedMemory/PhysicsClient.cpp", end
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp", project ("App_VRGloveHandSimulator")
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", language "C++"
"../../examples/SharedMemory/PhysicsServerSharedMemory.h", kind "ConsoleApp"
"../../examples/SharedMemory/PhysicsDirect.cpp",
"../../examples/SharedMemory/PhysicsDirect.h", includedirs {"../../src", "../../examples",
"../../examples/SharedMemory/PhysicsDirectC_API.cpp", "../../examples/ThirdPartyLibs"}
"../../examples/SharedMemory/PhysicsDirectC_API.h", defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h", hasCL = findOpenCL("clew")
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h", links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"}
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", initOpenGL()
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h", initGlew()
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
"../../examples/SharedMemory/PhysicsClientC_API.h", includedirs {
"../../examples/SharedMemory/Win32SharedMemory.cpp", ".",
"../../examples/SharedMemory/Win32SharedMemory.h", "../../src",
"../../examples/SharedMemory/PosixSharedMemory.cpp", "../ThirdPartyLibs",
"../../examples/SharedMemory/PosixSharedMemory.h", }
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
"../../examples/TinyRenderer/geometry.cpp", if os.is("MacOSX") then
"../../examples/TinyRenderer/model.cpp", links{"Cocoa.framework"}
"../../examples/TinyRenderer/tgaimage.cpp", end
"../../examples/TinyRenderer/our_gl.cpp",
"../../examples/TinyRenderer/TinyRenderer.cpp", if (hasCL) then
"../../examples/Utils/b3ResourcePath.cpp", links {
"../../examples/Utils/b3ResourcePath.h", "Bullet3OpenCL_clew",
"../../examples/Utils/RobotLoggingUtil.cpp", "Bullet3Dynamics",
"../../examples/Utils/RobotLoggingUtil.h", "Bullet3Collision",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", "Bullet3Geometry",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", "Bullet3Common",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", }
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", end
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", if _OPTIONS["audio"] then
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", files {
"../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../TinyAudio/b3ADSR.cpp",
"../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", "../TinyAudio/b3AudioListener.cpp",
"../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", "../TinyAudio/b3ReadWavFile.cpp",
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../TinyAudio/b3SoundEngine.cpp",
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../TinyAudio/b3SoundSource.cpp",
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../TinyAudio/b3WriteWavFile.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", "../TinyAudio/RtAudio.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", }
"../../examples/MultiThreading/b3PosixThreadSupport.cpp", defines {"B3_ENABLE_TINY_AUDIO"}
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp", if _OPTIONS["serial"] then
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", defines{"B3_ENABLE_SERIAL"}
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", 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 if os.is("Linux") then
initX11() initX11()

3
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2196,8 +2196,9 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
} }
} }
} }
}
#endif #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); m_data->m_strings.push_back(baseName);

12
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -167,6 +167,14 @@ enum JointType {
ePoint2PointType = 5, ePoint2PointType = 5,
}; };
enum b3JointInfoFlags
{
eJointChangeMaxForce = 1,
eJointChangeChildFramePosition = 2,
eJointChangeChildFrameOrientation = 4,
};
struct b3JointInfo struct b3JointInfo
{ {
char* m_linkName; char* m_linkName;
@@ -317,11 +325,7 @@ struct b3VREventsData
{ {
int m_numControllerEvents; int m_numControllerEvents;
struct b3VRControllerEvent* m_controllerEvents; struct b3VRControllerEvent* m_controllerEvents;
int m_numHmdEvents;
struct b3VRMoveEvent* m_hmdEvents;
int m_numGenericTrackerEvents;
struct b3VRMoveEvent* m_genericTrackerEvents;
}; };

13
examples/TinyAudio/b3AudioListener.cpp
View File

@@ -20,10 +20,12 @@ struct b3AudioListenerInternalData
b3WriteWavFile m_wavOut2; b3WriteWavFile m_wavOut2;
bool m_writeWavOut;
b3AudioListenerInternalData() b3AudioListenerInternalData()
:m_numControlTicks(64), :m_numControlTicks(64),
m_sampleRate(B3_SAMPLE_RATE) m_sampleRate(B3_SAMPLE_RATE),
m_writeWavOut(false)
{ {
for (int i=0;i<MAX_SOUND_SOURCES;i++) for (int i=0;i<MAX_SOUND_SOURCES;i++)
@@ -36,13 +38,18 @@ struct b3AudioListenerInternalData
b3AudioListener::b3AudioListener() b3AudioListener::b3AudioListener()
{ {
m_data = new b3AudioListenerInternalData(); m_data = new b3AudioListenerInternalData();
if (m_data->m_writeWavOut)
{
m_data->m_wavOut2.setWavFile("bulletAudio2.wav",B3_SAMPLE_RATE,2,false); m_data->m_wavOut2.setWavFile("bulletAudio2.wav",B3_SAMPLE_RATE,2,false);
} }
}
b3AudioListener::~b3AudioListener() b3AudioListener::~b3AudioListener()
{
if (m_data->m_writeWavOut)
{ {
m_data->m_wavOut2.closeWavFile(); m_data->m_wavOut2.closeWavFile();
}
delete m_data; delete m_data;
} }
@@ -154,7 +161,7 @@ int b3AudioListener::tick(void *outputBuffer,void *inputBuffer1,unsigned int nBu
} }
//logging to wav file //logging to wav file
if (numSamples) if (data->m_writeWavOut && numSamples)
{ {
data->m_wavOut2.tick( (double *)outputBuffer,numSamples); data->m_wavOut2.tick( (double *)outputBuffer,numSamples);
} }

1
examples/TinyAudio/b3SoundEngine.cpp
View File

@@ -7,7 +7,6 @@
#include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3AlignedObjectArray.h"
#include "b3ReadWavFile.h" #include "b3ReadWavFile.h"
#include "../Utils/b3ResourcePath.h" #include "../Utils/b3ResourcePath.h"
#include "../Utils/b3HashString.h"
#include "Bullet3Common/b3HashMap.h" #include "Bullet3Common/b3HashMap.h"

59
examples/Utils/b3HashString.h
View File

@@ -1,59 +0,0 @@
#ifndef B3_HASH_STRING_H
#define B3_HASH_STRING_H
#include <string>
///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

56
src/Bullet3Common/b3HashMap.h
View File

@@ -20,6 +20,62 @@ subject to the following restrictions:
#include "b3AlignedObjectArray.h" #include "b3AlignedObjectArray.h"
#include <string>
///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; const int B3_HASH_NULL=0xffffffff;