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

Merge remote-tracking branch 'bp/master' into pullRequest

Browse Source
This commit is contained in:
Jie Tan
2017-02-16 15:13:12 -08:00
parent 509b77054a 904a1b5f05
commit 83e5e816f5
48 changed files with 4372 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/CommonInterfaces/CommonCameraInterface.h
View File

@@ -36,6 +36,9 @@ struct CommonCameraInterface
virtual void setAspectRatio(float ratio) = 0;
virtual float getAspectRatio() const = 0;
virtual float getCameraFrustumFar() const = 0;
virtual float getCameraFrustumNear() const = 0;
};
#endif //COMMON_CAMERA_INTERFACE_H

1
examples/CommonInterfaces/CommonExampleInterface.h
View File

@@ -37,6 +37,7 @@ public:
virtual void initPhysics()=0;
virtual void exitPhysics()=0;
virtual void updateGraphics(){}
virtual void stepSimulation(float deltaTime)=0;
virtual void renderScene()=0;
virtual void physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h

1
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -266,6 +266,7 @@ void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
{
B3_PROFILE("clock.usleep");
clock.usleep(gMinUpdateTimeMicroSecs/10.);
exampleBrowser->updateGraphics();
} else
{
B3_PROFILE("exampleBrowser->update");

27
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -309,8 +309,11 @@ static void MyMouseMoveCallback( float x, float y)
bool handled = false;
if (sCurrentDemo)
handled = sCurrentDemo->mouseMoveCallback(x,y);
if (!handled && gui2)
handled = gui2->mouseMoveCallback(x,y);
if (renderGui)
{
if (!handled && gui2)
handled = gui2->mouseMoveCallback(x,y);
}
if (!handled)
{
if (prevMouseMoveCallback)
@@ -327,9 +330,11 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
if (sCurrentDemo)
handled = sCurrentDemo->mouseButtonCallback(button,state,x,y);
if (!handled && gui2)
handled = gui2->mouseButtonCallback(button,state,x,y);
if (renderGui)
{
if (!handled && gui2)
handled = gui2->mouseButtonCallback(button,state,x,y);
}
if (!handled)
{
if (prevMouseButtonCallback )
@@ -1125,6 +1130,18 @@ bool OpenGLExampleBrowser::requestedExit()
return s_window->requestedExit();
}
void OpenGLExampleBrowser::updateGraphics()
{
if (sCurrentDemo)
{
if (!pauseSimulation || singleStepSimulation)
{
B3_PROFILE("sCurrentDemo->updateGraphics");
sCurrentDemo->updateGraphics();
}
}
}
void OpenGLExampleBrowser::update(float deltaTime)
{
b3ChromeUtilsEnableProfiling();

2
examples/ExampleBrowser/OpenGLExampleBrowser.h
View File

@@ -19,6 +19,8 @@ public:
virtual void update(float deltaTime);
virtual void updateGraphics();
virtual bool requestedExit();
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);

7
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -437,10 +437,15 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
int bytesPerPixel = 4; //RGBA
int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
int sourceDepthIndex = xIndex+yIndex*sourceWidth;
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0];
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1];
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2];
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255;
m_data->m_depthBuffer1[i+j*destinationWidth] = sourceDepthBuffer[sourceDepthIndex];
}
}
}
@@ -456,7 +461,7 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
{
for (int i=0;i<numRequestedPixels;i++)
{
depthBuffer[i] = m_data->m_depthBuffer1[i];
depthBuffer[i] = m_data->m_depthBuffer1[i+startPixelIndex];
}
}
if (numPixelsCopied)

11
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -71,7 +71,7 @@ struct URDF2BulletCachedData
}
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
{
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
@@ -81,7 +81,7 @@ struct URDF2BulletCachedData
return m_urdfLink2rigidBodies[urdfLinkIndex];
}
void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
{
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
@@ -250,7 +250,12 @@ void ConvertURDF2BulletInternal(
btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
btCollisionShape* compoundShape = tmpShape;
if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0)==btTransform::getIdentity())
{
compoundShape = tmpShape->getChildShape(0);
}
int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);

10
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -383,3 +383,13 @@ float SimpleCamera::getAspectRatio() const
{
return m_data->m_aspect;
}
float SimpleCamera::getCameraFrustumFar() const
{
return m_data->m_frustumZFar;
}
float SimpleCamera::getCameraFrustumNear() const
{
return m_data->m_frustumZNear;
}

5
examples/OpenGLWindow/SimpleCamera.h
View File

@@ -47,6 +47,9 @@ struct SimpleCamera : public CommonCameraInterface
virtual void setAspectRatio(float ratio);
virtual float getAspectRatio() const;
virtual float getCameraFrustumFar() const;
virtual float getCameraFrustumNear() const;
};
#endif //SIMPLE_CAMERA_H
#endif //SIMPLE_CAMERA_H

2
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -787,7 +787,7 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
#ifdef _WIN32
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
"-threads 0 -y -b:v 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
//sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);

16
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -16,8 +16,8 @@
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
static btScalar sGripperVerticalVelocity = -0.2f;
static btScalar sGripperClosingTargetVelocity = 0.5f;
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = -0.7f;
class GripperGraspExample : public CommonExampleInterface
{
@@ -226,9 +226,9 @@ public:
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "sphere_small.urdf";
args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_fileName = "dinnerware/pan_tefal.urdf";
args.m_startPosition.setValue(0, -0.2, .47);
args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
}
@@ -492,7 +492,7 @@ public:
int fingerJointIndices[2]={0,1};
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
};
double maxTorqueValues[2]={50.0,50.0};
double maxTorqueValues[2]={800.0,800.0};
for (int i=0;i<2;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
@@ -550,8 +550,8 @@ public:
virtual void resetCamera()
{
float dist = 1.5;
float pitch = 12;
float yaw = -10;
float pitch = 18;
float yaw = 10;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{

10
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -338,6 +338,16 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl
return 0;
}
int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_contactBreakingThreshold = contactBreakingThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD;
return 0;
}
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -191,7 +191,7 @@ int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHand
int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
//b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
//Use at own risk: magic things may or my not happen when calling this API

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -12,6 +12,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "IKTrajectoryHelper.h"
@@ -798,7 +799,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 150;
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
// m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
@@ -2724,7 +2725,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
{
gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{
m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;

92
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -299,7 +299,6 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
double deltaTimeInSeconds = 0;
double sleepCounter = 0;
do
{
BT_PROFILE("loop");
@@ -310,27 +309,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
}
double dt = double(clock.getTimeMicroseconds())/1000000.;
clock.reset();
sleepCounter+=dt;
if (sleepCounter > sleepTimeThreshold)
{
BT_PROFILE("usleep(100)");
sleepCounter = 0;
b3Clock::usleep(100);
}
{
if (gEnableRealTimeSimVR)
{
BT_PROFILE("usleep(1000)");
b3Clock::usleep(1000);
}
}
deltaTimeInSeconds+= dt;
{
@@ -574,7 +553,7 @@ public:
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
{
b3Clock::usleep(100);
b3Clock::usleep(0);
}
}
@@ -961,6 +940,8 @@ public:
virtual void stepSimulation(float deltaTime);
virtual void updateGraphics();
void enableCommandLogging()
{
m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin");
@@ -1363,39 +1344,8 @@ bool PhysicsServerExample::wantsTermination()
return m_wantsShutdown;
}
void PhysicsServerExample::stepSimulation(float deltaTime)
void PhysicsServerExample::updateGraphics()
{
BT_PROFILE("PhysicsServerExample::stepSimulation");
//this->m_physicsServer.processClientCommands();
for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
{
if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
{
m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
{
m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
m_multiThreadedHelper->m_userDebugLines.pop_back();
}
}
}
for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
{
if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
{
m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
{
m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
m_multiThreadedHelper->m_userDebugText.pop_back();
}
}
}
//check if any graphics related tasks are requested
switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
@@ -1564,6 +1514,40 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
}
}
void PhysicsServerExample::stepSimulation(float deltaTime)
{
BT_PROFILE("PhysicsServerExample::stepSimulation");
//this->m_physicsServer.processClientCommands();
for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
{
if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
{
m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
{
m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
m_multiThreadedHelper->m_userDebugLines.pop_back();
}
}
}
for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
{
if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
{
m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
{
m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
m_multiThreadedHelper->m_userDebugText.pop_back();
}
}
}
updateGraphics();

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -313,7 +313,8 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64,
SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE=128,
SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256,
SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512
SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512,
SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024,
};
enum EnumLoadBunnyUpdateFlags
@@ -340,6 +341,7 @@ struct SendPhysicsSimulationParameters
bool m_allowRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;
double m_contactBreakingThreshold;
int m_internalSimFlags;
double m_defaultContactERP;
int m_collisionFilterMode;

17
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -44,16 +44,13 @@ public:
PhysicsClientSharedMemory::disconnectSharedMemory();
}
unsigned long int ms = m_clock.getTimeMilliseconds();
if (ms>20)
{
m_clock.reset();
btUpdateInProcessExampleBrowserMainThread(m_data);
} else
{
//b3Clock::usleep(100);
}
return PhysicsClientSharedMemory::processServerStatus();
if (ms>20)
{
m_clock.reset();
btUpdateInProcessExampleBrowserMainThread(m_data);
}
b3Clock::usleep(0);
return PhysicsClientSharedMemory::processServerStatus();
}

11
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -958,7 +958,16 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
{
if (depthBuffer)
{
depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
float distance = -m_data->m_depthBuffer[i+startPixelIndex];
float farPlane = m_data->m_camera.getCameraFrustumFar();
float nearPlane = m_data->m_camera.getCameraFrustumNear();
btClamp(distance,nearPlane,farPlane);
// the depth buffer value is between 0 and 1
float a = farPlane / (farPlane - nearPlane);
float b = farPlane * nearPlane / (nearPlane - farPlane);
depthBuffer[i] = a + b / distance;
}
if (segmentationMaskBuffer)
{

9
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -135,6 +135,8 @@ public:
void SetupRenderModelForTrackedDevice( vr::TrackedDeviceIndex_t unTrackedDeviceIndex );
CGLRenderModel *FindOrLoadRenderModel( const char *pchRenderModelName );
SimpleOpenGL3App* getApp() { return m_app;}
private:
bool m_bDebugOpenGL;
bool m_bVerbose;
@@ -2260,7 +2262,7 @@ int main(int argc, char *argv[])
b3ChromeUtilsEnableProfiling();
}
#ifdef BT_USE_CUSTOM_PROFILER
b3SetCustomEnterProfileZoneFunc(dcEnter);
b3SetCustomLeaveProfileZoneFunc(dcLeave);
@@ -2287,6 +2289,11 @@ int main(int argc, char *argv[])
}
char* gVideoFileName = 0;
args.GetCmdLineArgument("mp4",gVideoFileName);
if (gVideoFileName)
pMainApplication->getApp()->dumpFramesToVideo(gVideoFileName);
//request disable VSYNC
typedef bool (APIENTRY *PFNWGLSWAPINTERVALFARPROC)(int);
PFNWGLSWAPINTERVALFARPROC wglSwapIntervalEXT = 0;

253
examples/Utils/RobotLoggingUtil.cpp Normal file
View File

@@ -0,0 +1,253 @@
#include "RobotLoggingUtil.h"
#include <stdio.h>
#include "LinearMath/btAlignedObjectArray.h"
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
static bool readLine(FILE* file, btAlignedObjectArray<char>& line)
{
int c = 0;
for (c=fgetc(file);(c != EOF && c != '\n');c=fgetc(file))
{
line.push_back(c);
}
line.push_back(0);
return (c == EOF);
}
int readMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose)
{
int retVal = 0;
FILE* f = fopen(fileName,"rb");
if (f)
{
if (verbose)
{
printf("Opened file %s\n", fileName);
}
btAlignedObjectArray<char> line0Buf;
bool eof = readLine(f,line0Buf);
btAlignedObjectArray<char> line1Buf;
eof |= readLine(f,line1Buf);
std::string line0 = &line0Buf[0];
structTypes = &line1Buf[0];
btAlignedObjectArray<std::string> separators;
separators.push_back(",");
urdfStringSplit(structNames,line0,separators);
if (verbose)
{
printf("Num Fields = %d\n",structNames.size());
}
btAssert(structTypes.size() == structNames.size());
if (structTypes.size() != structNames.size())
{
retVal = eCorruptHeader;
}
int numStructsRead = 0;
if (structTypes.size() == structNames.size())
{
while (!eof)
{
unsigned char blaat[1024];
size_t s = fread(blaat,2,1,f);
if (s!=1)
{
eof=true;
retVal = eInvalidAABBAlignCheck;
break;
}
if ((blaat[0] != 0xaa) || (blaat[1] != 0xbb))
{
if (verbose)
{
printf("Expected 0xaa0xbb, terminating\n");
}
}
if (verbose)
{
printf("Reading structure %d\n",numStructsRead);
}
MinitaurLogRecord record;
for (int i=0;i<structNames.size();i++)
{
switch (structTypes[i])
{
case 'I':
{
size_t s = fread(blaat,sizeof(int),1,f);
if (s != 1)
{
eof = true;
retVal = eCorruptValue;
break;
}
int v = *(int*)blaat;
if (s==1)
{
if (verbose)
{
printf("%s = %d\n",structNames[i].c_str(),v);
}
record.m_values.push_back(v);
}
break;
}
case 'f':
{
float v;
size_t s = fread(&v,sizeof(float),1,f);
if (s != 1)
{
eof = true;
break;
}
if (s==1)
{
if (verbose)
{
printf("%s = %f\n",structNames[i].c_str(),v);
}
record.m_values.push_back(v);
}
break;
}
case 'B':
{
char v;
size_t s = fread(&v,sizeof(char),1,f);
if (s != 1)
{
eof = true;
break;
}
if (s==1)
{
if (verbose)
{
printf("%s = %d\n",structNames[i].c_str(),v);
}
record.m_values.push_back(v);
}
break;
}
default:
{
if (verbose)
{
printf("Unknown type\n");
}
retVal = eUnknownType;
btAssert(0);
}
}
}
logRecords.push_back(record);
numStructsRead++;
}
if (verbose)
{
printf("numStructsRead = %d\n",numStructsRead);
}
if (retVal==0)
{
retVal = numStructsRead;
}
}
//read header and
} else
{
if (verbose)
{
printf("Could not open file %s", fileName);
}
retVal = eMinitaurFileNotFound;
}
return retVal;
}
FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes)
{
FILE* f = fopen(fileName,"wb");
if (f)
{
for (int i=0;i<structNames.size();i++)
{
int len = strlen(structNames[i].c_str());
fwrite(structNames[i].c_str(),len,1,f);
if (i<structNames.size()-1)
{
fwrite(",",1,1,f);
}
}
int sz = sizeof("\n");
fwrite("\n",sz-1,1,f);
fwrite(structTypes.c_str(),strlen(structTypes.c_str()),1,f);
fwrite("\n",sz-1,1,f);
}
return f;
}
void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData)
{
if (f)
{
unsigned char buf[2] = {0xaa,0xbb};
fwrite(buf,2,1,f);
if (structTypes.length() == logData.m_values.size())
{
for (int i=0;i<logData.m_values.size();i++)
{
switch(structTypes[i])
{
case 'I':
{
fwrite(&logData.m_values[i].m_intVal,sizeof(int),1,f);
break;
}
case 'f':
{
fwrite(&logData.m_values[i].m_floatVal,sizeof(float),1,f);
break;
}
case 'B':
{
fwrite(&logData.m_values[i].m_charVal,sizeof(char),1,f);
break;
}
default:
{
}
}
}
}
}
}
void closeMinitaurLogFile(FILE* f)
{
if (f)
{
fclose(f);
}
}

54
examples/Utils/RobotLoggingUtil.h Normal file
View File

@@ -0,0 +1,54 @@
#ifndef ROBOT_LOGGING_UTIL_H
#define ROBOT_LOGGING_UTIL_H
#include "LinearMath/btAlignedObjectArray.h"
#include <string>
struct MinitaurLogValue
{
MinitaurLogValue()
:m_intVal(0xcdcdcdcd)
{
}
MinitaurLogValue(int iv)
:m_intVal(iv)
{
}
MinitaurLogValue(float fv)
:m_floatVal(fv)
{
}
MinitaurLogValue(char fv)
:m_charVal(fv)
{
}
union
{
char m_charVal;
int m_intVal;
float m_floatVal;
};
};
struct MinitaurLogRecord
{
btAlignedObjectArray<MinitaurLogValue> m_values;
};
enum MINITAUR_LOG_ERROR
{
eMinitaurFileNotFound = -1,
eCorruptHeader = -2,
eUnknownType = -3,
eCorruptValue = -4,
eInvalidAABBAlignCheck = -5,
};
int readMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose);
FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes);
void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData);
void closeMinitaurLogFile(FILE* f);
#endif //ROBOT_LOGGING_UTIL_H

25
examples/pybullet/manyspheres.py Normal file
View File

@@ -0,0 +1,25 @@
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf",useMaximalCoordinates=True)
p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
for i in range (10):
for j in range (10):
for k in range (5):
ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while True:
gravX = p.readUserDebugParameter(gravXid)
gravY = p.readUserDebugParameter(gravYid)
gravZ = p.readUserDebugParameter(gravZid)
p.setGravity(gravX,gravY,gravZ)
time.sleep(0.01)

13
examples/pybullet/pybullet.c
View File

@@ -583,14 +583,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
double splitImpulsePenetrationThreshold = -1;
int numSubSteps = -1;
int collisionFilterMode = -1;
double contactBreakingThreshold = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "physicsClientId", NULL };
static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
&collisionFilterMode, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &physicsClientId))
{
return NULL;
}
@@ -610,6 +611,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{
b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
}
if (collisionFilterMode>=0)
{
b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode);
@@ -630,6 +632,11 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{
b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
}
if (contactBreakingThreshold>=0)
{
b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold);
}
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);

26
examples/pybullet/rollPitchYaw.py Normal file
View File

@@ -0,0 +1,26 @@
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True)
rollId = p.addUserDebugParameter("roll",-1.5,1.5,0)
pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0)
yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0)
fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0)
fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0)
fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0)
while True:
roll = p.readUserDebugParameter(rollId)
pitch = p.readUserDebugParameter(pitchId)
yaw = p.readUserDebugParameter(yawId)
x = p.readUserDebugParameter(fwdxId)
y = p.readUserDebugParameter(fwdyId)
z = p.readUserDebugParameter(fwdzId)
orn = p.getQuaternionFromEuler([roll,pitch,yaw])
p.resetBasePositionAndOrientation(q,[x,y,z],orn)
#p.stepSimulation()#not really necessary for this demo, no physics used