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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
YunfeiBai
2016-08-19 12:02:48 -07:00
parent 4611f5f097 e5626b954f
commit 94161246d9
20 changed files with 412 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/CommonInterfaces/CommonGraphicsAppInterface.h
View File

@@ -66,9 +66,9 @@ struct CommonGraphicsApp
m_mouseYpos(0.f),
m_mouseInitialized(false)
{
m_backgroundColorRGB[0] = 0.9;
m_backgroundColorRGB[1] = 0.9;
m_backgroundColorRGB[2] = 1;
m_backgroundColorRGB[0] = 0.7;
m_backgroundColorRGB[1] = 0.7;
m_backgroundColorRGB[2] = 0.8;
}
virtual ~CommonGraphicsApp()
{

52
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -123,6 +123,7 @@ static bool enable_experimental_opencl = false;
int gDebugDrawFlags = 0;
static bool pauseSimulation=false;
static bool singleStepSimulation = false;
int midiBaseIndex = 176;
extern bool gDisableDeactivation;
@@ -227,6 +228,12 @@ void MyKeyboardCallback(int key, int state)
{
pauseSimulation = !pauseSimulation;
}
if (key == 'o' && state)
{
singleStepSimulation = true;
}
#ifndef NO_OPENGL3
if (key=='s' && state)
{
@@ -490,7 +497,7 @@ void MyComboBoxCallback(int comboId, const char* item)
void MyGuiPrintf(const char* msg)
{
printf("b3Printf: %s\n",msg);
if (gui2)
if (!gDisableDemoSelection)
{
gui2->textOutput(msg);
gui2->forceUpdateScrollBars();
@@ -502,7 +509,7 @@ void MyGuiPrintf(const char* msg)
void MyStatusBarPrintf(const char* msg)
{
printf("b3Printf: %s\n", msg);
if (gui2)
if (!gDisableDemoSelection)
{
bool isLeft = true;
gui2->setStatusBarMessage(msg,isLeft);
@@ -513,7 +520,7 @@ void MyStatusBarPrintf(const char* msg)
void MyStatusBarError(const char* msg)
{
printf("Warning: %s\n", msg);
if (gui2)
if (!gDisableDemoSelection)
{
bool isLeft = false;
gui2->setStatusBarMessage(msg,isLeft);
@@ -1103,15 +1110,33 @@ void OpenGLExampleBrowser::update(float deltaTime)
s_app->drawText(bla,10,10);
}
if (gPngFileName)
{
static int skip = 0;
skip--;
if (skip<0)
{
skip=gPngSkipFrames;
//printf("gPngFileName=%s\n",gPngFileName);
static int s_frameCount = 100;
sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
//b3Printf("Made screenshot %s",staticPngFileName);
s_app->dumpNextFrameToPng(staticPngFileName);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
}
if (sCurrentDemo)
{
if (!pauseSimulation)
if (!pauseSimulation || singleStepSimulation)
{
singleStepSimulation = false;
//printf("---------------------------------------------------\n");
//printf("Framecount = %d\n",frameCount);
if (gFixedTimeStep>0)
{
sCurrentDemo->stepSimulation(gFixedTimeStep);
@@ -1145,23 +1170,6 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
}
if (gPngFileName)
{
static int skip = 0;
skip--;
if (skip<0)
{
skip=gPngSkipFrames;
//printf("gPngFileName=%s\n",gPngFileName);
static int s_frameCount = 100;
sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
//b3Printf("Made screenshot %s",staticPngFileName);
s_app->dumpNextFrameToPng(staticPngFileName);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
}
{

5
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -84,7 +84,10 @@ public:
virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
{
drawLine(PointOnB,PointOnB+normalOnB,color);
drawLine(PointOnB,PointOnB+normalOnB*distance,color);
btVector3 red(0.3, 1., 0.3);
drawLine(PointOnB, PointOnB + normalOnB*0.01, red);
}

29
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -36,7 +36,8 @@ class ImportUrdfSetup : public CommonMultiBodyBase
struct ImportUrdfInternalData* m_data;
bool m_useMultiBody;
btAlignedObjectArray<std::string* > m_nameMemory;
btScalar m_grav;
int m_upAxis;
public:
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportUrdfSetup();
@@ -87,7 +88,9 @@ struct ImportUrdfInternalData
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper)
:CommonMultiBodyBase(helper),
m_grav(0),
m_upAxis(2)
{
m_data = new ImportUrdfInternalData;
@@ -186,8 +189,8 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
void ImportUrdfSetup::initPhysics()
{
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
m_guiHelper->setUpAxis(m_upAxis);
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
@@ -199,10 +202,14 @@ void ImportUrdfSetup::initPhysics()
);//+btIDebugDraw::DBG_DrawConstraintLimits);
btVector3 gravity(0,0,0);
gravity[upAxis]=-9.8;
if (m_guiHelper->getParameterInterface())
{
SliderParams slider("Gravity", &m_grav);
slider.m_minVal = -10;
slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_dynamicsWorld->setGravity(gravity);
BulletURDFImporter u2b(m_guiHelper, 0);
@@ -350,7 +357,7 @@ void ImportUrdfSetup::initPhysics()
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
groundHalfExtents[m_upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(box);
box->initializePolyhedralFeatures();
@@ -358,7 +365,7 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-2.5;
groundOrigin[m_upAxis]=-2.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body);
@@ -388,6 +395,10 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
btVector3 gravity(0, 0, 0);
gravity[m_upAxis] = m_grav;
m_dynamicsWorld->setGravity(gravity);
for (int i=0;i<m_data->m_numMotors;i++)
{
if (m_data->m_jointMotors[i])

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

@@ -299,7 +299,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
//b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
if (enableConstraints)
world1->addConstraint(dof6,true);
@@ -324,7 +324,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else
{
btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
if (enableConstraints)
world1->addConstraint(dof6,true);
@@ -350,7 +350,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else
{
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints)
@@ -455,7 +455,9 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
mb->setBaseWorldTransform(rootTransformInWorldSpace);
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m);

24
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -58,7 +58,7 @@ public:
virtual void physicsDebugDraw(int debugDrawMode)
{
m_robotSim.debugDraw(debugDrawMode);
}
virtual void initPhysics()
{
@@ -81,6 +81,15 @@ public:
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
}
{
b3RobotSimLoadFileArgs args("");
@@ -129,15 +138,7 @@ public:
}
}
}
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0,0,.107);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = false;
m_robotSim.loadFile(args,results);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
@@ -179,13 +180,14 @@ public:
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
m_robotSim.stepSimulation();
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
//m_app->m_renderer->renderScene();
}

201
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -7,15 +7,15 @@
//#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/PhysicsDirectC_API.h"
#include "../SharedMemory/PhysicsDirect.h"
#include <string>
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
@@ -406,24 +406,27 @@ public:
struct b3RobotSimAPI_InternalData
{
//GUIHelperInterface* m_guiHelper;
PhysicsServerSharedMemory m_physicsServer;
b3PhysicsClientHandle m_physicsClient;
b3ThreadSupportInterface* m_threadSupport;
RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper;
PhysicsDirect* m_clientServerDirect;
bool m_useMultiThreading;
bool m_connected;
b3RobotSimAPI_InternalData()
:m_multiThreadedHelper(0),
:m_threadSupport(0),
m_multiThreadedHelper(0),
m_clientServerDirect(0),
m_physicsClient(0),
m_useMultiThreading(false),
m_connected(false)
{
}
@@ -464,6 +467,9 @@ b3RobotSimAPI::~b3RobotSimAPI()
void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
{
if (0==m_data->m_multiThreadedHelper)
return;
switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
{
case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject:
@@ -563,27 +569,6 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
}
}
#if 0
if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
while (rtc.getTimeMilliseconds()<endTime)
{
m_physicsServer.processClientCommands();
}
} else
{
//for (int i=0;i<10;i++)
m_physicsServer.processClientCommands();
}
#endif
}
b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle)
@@ -733,94 +718,124 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS
bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
{
m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);
MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);
m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
for (int i=0;i<m_data->m_threadSupport->getNumTasks();i++)
if (m_data->m_useMultiThreading)
{
RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i);
b3Assert(storage);
storage->threadId = i;
//storage->m_sharedMem = data->m_sharedMem;
}
m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
for (int w=0;w<MAX_ROBOT_NUM_THREADS;w++)
{
m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized);
int numMoving = 0;
m_data->m_args[w].m_positions.resize(numMoving);
m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
int index = 0;
m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w);
while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized)
for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++)
{
RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i);
b3Assert(storage);
storage->threadId = i;
//storage->m_sharedMem = data->m_sharedMem;
}
for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++)
{
m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized);
int numMoving = 0;
m_data->m_args[w].m_positions.resize(numMoving);
m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
int index = 0;
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w);
while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized)
{
}
}
m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper);
b3Assert(serverConnected);
m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
}
else
{
m_data->m_clientServerDirect = new PhysicsDirect();
bool connected = m_data->m_clientServerDirect->connect(guiHelper);
m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect;
}
m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
//client connected
m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient);
m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper);
b3Assert(m_data->m_connected);
m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient);
b3Assert(canSubmit);
return m_data->m_connected && canSubmit;
b3Assert(m_data->m_connected);
return m_data->m_connected && m_data->m_connected;
}
void b3RobotSimAPI::disconnect()
{
for (int i=0;i<MAX_ROBOT_NUM_THREADS;i++)
if (m_data->m_useMultiThreading)
{
m_data->m_args[i].m_cs->lock();
m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim);
m_data->m_args[i].m_cs->unlock();
for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++)
{
m_data->m_args[i].m_cs->lock();
m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim);
m_data->m_args[i].m_cs->unlock();
}
int numActiveThreads = MAX_ROBOT_NUM_THREADS;
while (numActiveThreads)
{
int arg0, arg1;
if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0))
{
numActiveThreads--;
printf("numActiveThreads = %d\n", numActiveThreads);
}
else
{
}
};
printf("stopping threads\n");
delete m_data->m_threadSupport;
m_data->m_threadSupport = 0;
}
int numActiveThreads = MAX_ROBOT_NUM_THREADS;
while (numActiveThreads)
{
int arg0,arg1;
if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
{
numActiveThreads--;
printf("numActiveThreads = %d\n",numActiveThreads);
} else
{
}
};
printf("stopping threads\n");
delete m_data->m_threadSupport;
m_data->m_threadSupport = 0;
b3DisconnectSharedMemory(m_data->m_physicsClient);
m_data->m_physicsServer.disconnectSharedMemory(true);
m_data->m_connected = false;
}
void b3RobotSimAPI::debugDraw(int debugDrawMode)
{
if (m_data->m_clientServerDirect)
{
m_data->m_clientServerDirect->debugDraw(debugDrawMode);
}
}
void b3RobotSimAPI::renderScene()
{
if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
if (m_data->m_useMultiThreading)
{
m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
{
m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
}
}
if (m_data->m_clientServerDirect)
{
m_data->m_clientServerDirect->renderScene();
}
m_data->m_physicsServer.renderScene();
}

1
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -95,6 +95,7 @@ public:
void setGravity(const b3Vector3& gravityAcceleration);
void renderScene();
void debugDraw(int debugDrawMode);
};
#endif //B3_ROBOT_SIM_API_H

2
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -239,7 +239,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
static double startPosY = 1;
static double startPosY = 0;
double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
startPosY += 2.f;

1
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -81,7 +81,6 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
return bodyJoints->m_jointInfo.size();
}
btAssert(0);
return 0;
}

18
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -69,9 +69,27 @@ PhysicsDirect::~PhysicsDirect()
bool PhysicsDirect::connect()
{
m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx);
return true;
}
// return true if connection succesfull, can also check 'isConnected'
bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper)
{
m_data->m_commandProcessor->setGuiHelper(guiHelper);
return true;
}
void PhysicsDirect::renderScene()
{
m_data->m_commandProcessor->renderScene();
}
void PhysicsDirect::debugDraw(int debugDrawMode)
{
m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode);
}
////todo: rename to 'disconnect'
void PhysicsDirect::disconnectSharedMemory()
{

8
examples/SharedMemory/PhysicsDirect.h
View File

@@ -29,7 +29,8 @@ public:
virtual ~PhysicsDirect();
// return true if connection succesfull, can also check 'isConnected'
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
////todo: rename to 'disconnect'
@@ -63,6 +64,11 @@ public:
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();
virtual void debugDraw(int debugDrawMode);
};
#endif //PHYSICS_DIRECT_H

14
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -696,10 +696,12 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
if (supportsJointMotor(mb,mbLinkIndex))
{
float maxMotorImpulse = 0.f;
float maxMotorImpulse = 10000.f;
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
motor->setPositionTarget(0, 0);
motor->setVelocityTarget(0, 1);
//motor->setMaxAppliedImpulse(0);
mb->getLink(mbLinkIndex).m_userPtr = motor;
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
@@ -1416,7 +1418,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Using CONTROL_MODE_TORQUE");
}
// mb->clearForcesAndTorques();
int torqueIndex = 0;
int torqueIndex = 6;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
for (int link=0;link<mb->getNumLinks();link++)
@@ -1602,6 +1604,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM)
{
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
hasStatus = true;
break;
}
//always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
@@ -2437,7 +2445,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
}
m_data->m_dynamicsWorld->stepSimulation(dtInSec);
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.);
}
}

17
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -872,7 +872,22 @@ void PhysicsServerExample::renderScene()
static int frameCount=0;
frameCount++;
char bla[1024];
sprintf(bla,"VR sub-title text test, frame %d", frameCount/2);
static btScalar prevTime = m_clock.getTimeSeconds();
btScalar curTime = m_clock.getTimeSeconds();
static btScalar deltaTime = 0.f;
static int count = 10;
if (count-- < 0)
{
count = 10;
deltaTime = curTime - prevTime;
}
if (deltaTime == 0)
deltaTime = 1000;
prevTime = curTime;
sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2);
float pos[4];
m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
btTransform viewTr;

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -27,7 +27,7 @@
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 64
#define MAX_DEGREE_OF_FREEDOM 128
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_SDF_FILENAME_LENGTH 1024

2
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -268,7 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0);
int* segmentationMaskBufferPtr = renderData.m_segmentationMaskBufferPtr?&renderData.m_segmentationMaskBufferPtr->at(0):0;
TGAImage& frame = renderData.m_rgbColorBuffer;

2
examples/Utils/b3Clock.cpp
View File

@@ -239,7 +239,7 @@ void b3Clock::usleep(int microSeconds)
Sleep(millis);
#else
usleep(microSeconds);
::usleep(microSeconds);
//struct timeval tv;
//tv.tv_sec = microSeconds/1000000L;
//tv.tv_usec = microSeconds%1000000L;

211
examples/pybullet/pybullet.c
View File

@@ -296,10 +296,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double targetPosition=0;
double targetVelocity=0;
double maxForce=100000;
double appliedForce = 0;
double kp=0.1;
double kd=1.0;
int valid = 0;
@@ -313,34 +314,107 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
size= PySequence_Size(args);
if (size==4)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
double targetValue = 0;
// see switch statement below for convertsions dependent on controlMode
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
valid = 1;
switch (controlMode)
{
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
targetPosition = targetValue;
break;
}
case CONTROL_MODE_VELOCITY:
{
targetVelocity = targetValue;
break;
}
case CONTROL_MODE_TORQUE:
{
appliedForce = targetValue;
break;
}
default:
{
valid = 0;
}
}
}
if (size==5)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
double targetValue = 0;
//See switch statement for conversions
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
switch (controlMode)
{
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
targetPosition = targetValue;
break;
}
case CONTROL_MODE_VELOCITY:
{
targetVelocity = targetValue;
break;
}
case CONTROL_MODE_TORQUE:
{
valid = 0;
break;
}
default:
{
valid = 0;
}
}
}
if (size==6)
{
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
double gain;
double targetValue = 0;
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
valid = 1;
switch (controlMode)
{
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
targetPosition = targetValue;
kp = gain;
break;
}
case CONTROL_MODE_VELOCITY:
{
targetVelocity = targetValue;
kd = gain;
break;
}
case CONTROL_MODE_TORQUE:
{
valid = 0;
break;
}
default:
{
valid = 0;
}
}
}
if (size==8)
{
@@ -353,16 +427,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
valid = 1;
}
if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD)
{
PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD");
return NULL;
}
if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8)
{
PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity");
return NULL;
}
if (valid)
{
@@ -395,25 +460,25 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
default:
@@ -426,7 +491,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
return NULL;
}
@@ -532,7 +597,7 @@ pybullet_setTimeStep(PyObject* self, PyObject* args)
// Internal function used to get the base position and orientation
// Orientation is returned in quaternions
static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
{
basePosition[0] = 0.;
basePosition[1] = 0.;
@@ -552,7 +617,11 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
return 0;
}
const double* actualStateQ;
// const double* jointReactionForces[];
int i;
@@ -579,6 +648,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
}
}
return 1;
}
// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion
@@ -606,7 +676,11 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
return NULL;
}
pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation);
if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation))
{
PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?).");
return NULL;
}
{
@@ -848,42 +922,44 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
// printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex);
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
pyListJointInfo = PyTuple_New(jointInfoSize);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info))
{
// printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
// info.m_jointIndex,
// info.m_jointName,
// info.m_jointType,
// info.m_qIndex,
// info.m_uIndex);
// printf(" flags=%d jointDamping=%f jointFriction=%f\n",
// info.m_flags,
// info.m_jointDamping,
// info.m_jointFriction);
PyTuple_SetItem(pyListJointInfo, 0,
PyInt_FromLong(info.m_jointIndex));
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyTuple_SetItem(pyListJointInfo, 2,
PyInt_FromLong(info.m_jointType));
PyTuple_SetItem(pyListJointInfo, 3,
PyInt_FromLong(info.m_qIndex));
PyTuple_SetItem(pyListJointInfo, 4,
PyInt_FromLong(info.m_uIndex));
PyTuple_SetItem(pyListJointInfo, 5,
PyInt_FromLong(info.m_flags));
PyTuple_SetItem(pyListJointInfo, 6,
PyFloat_FromDouble(info.m_jointDamping));
PyTuple_SetItem(pyListJointInfo, 7,
PyFloat_FromDouble(info.m_jointFriction));
return pyListJointInfo;
// printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
// info.m_jointIndex,
// info.m_jointName,
// info.m_jointType,
// info.m_qIndex,
// info.m_uIndex);
// printf(" flags=%d jointDamping=%f jointFriction=%f\n",
// info.m_flags,
// info.m_jointDamping,
// info.m_jointFriction);
PyTuple_SetItem(pyListJointInfo, 0,
PyInt_FromLong(info.m_jointIndex));
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyTuple_SetItem(pyListJointInfo, 2,
PyInt_FromLong(info.m_jointType));
PyTuple_SetItem(pyListJointInfo, 3,
PyInt_FromLong(info.m_qIndex));
PyTuple_SetItem(pyListJointInfo, 4,
PyInt_FromLong(info.m_uIndex));
PyTuple_SetItem(pyListJointInfo, 5,
PyInt_FromLong(info.m_flags));
PyTuple_SetItem(pyListJointInfo, 6,
PyFloat_FromDouble(info.m_jointDamping));
PyTuple_SetItem(pyListJointInfo, 7,
PyFloat_FromDouble(info.m_jointFriction));
return pyListJointInfo;
}
else
{
PyErr_SetString(SpamError, "GetJointInfo failed.");
return NULL;
}
}
}
@@ -934,12 +1010,19 @@ pybullet_getJointState(PyObject* self, PyObject* args)
{
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
{
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
return NULL;
}
pyListJointState = PyTuple_New(sensorStateSize);
pyListJointForceTorque = PyTuple_New(forceTorqueSize);

14
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@@ -134,6 +134,7 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
{
m_ghostObject = ghostObject;
m_up.setValue(0.0f, 0.0f, 1.0f);
m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
setUp(up);
@@ -142,7 +143,6 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
m_walkDirection.setValue(0.0,0.0,0.0);
m_AngVel.setValue(0.0, 0.0, 0.0);
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_turnAngle = btScalar(0.0);
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
@@ -520,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
if (!callback.hasHit() && m_ghostObject->hasContactResponse())
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@@ -529,7 +529,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
if (!callback.hasHit() && m_ghostObject->hasContactResponse())
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@@ -537,11 +537,11 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
bool has_hit = false;
bool has_hit;
if (bounce_fix == true)
has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
else
has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
@@ -556,7 +556,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
m_targetPosition = orig_position;
downVelocity = stepHeight;
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
@@ -564,7 +564,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
break;
}
if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true)
{
// we dropped a fraction of the height -> hit floor
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;

6
src/LinearMath/btIDebugDraw.h
View File

@@ -166,9 +166,9 @@ class btIDebugDraw
virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
{
btVector3 start = transform.getOrigin();
drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3));
drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3));
drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f));
}
virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,