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

Merge pull request #647 from erwincoumans/master

Browse Source 
Support the <static> field under <model> in SDF, fix premake build system
This commit is contained in:
erwincoumans
2016-06-07 17:03:41 -07:00
parent 68932b0c0a e0c3392cee
commit f5f4e1f3c0
33 changed files with 633 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/cube.mtl
View File

@@ -10,5 +10,5 @@ newmtl cube
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka cube.tga
map_Kd floor_diffuse.tga
map_Kd cube.png

1
data/two_cubes.sdf
View File

@@ -86,6 +86,7 @@
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='unit_box_0'>
<static>1</static>
<pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
<link name='unit_box_0::link'>
<inertial>

5
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -242,7 +242,10 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
#ifdef ENABLE_LUA

3
examples/ExampleBrowser/main.cpp
View File

@@ -13,6 +13,8 @@
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
int main(int argc, char* argv[])
@@ -29,6 +31,7 @@ int main(int argc, char* argv[])
exampleBrowser->registerFileImporter(".urdf",ImportURDFCreateFunc);
exampleBrowser->registerFileImporter(".sdf",ImportSDFCreateFunc);
exampleBrowser->registerFileImporter(".obj",ImportObjCreateFunc);
exampleBrowser->registerFileImporter(".stl",ImportSTLCreateFunc);
clock.reset();
if (init)

54
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -235,6 +235,60 @@ void ImportSDFSetup::initPhysics()
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
mb = creation.getBulletMultiBody();
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 10.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
}
}

31
examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
View File

@@ -13,9 +13,12 @@
class ImportSTLSetup : public CommonRigidBodyBase
{
const char* m_fileName;
btVector3 m_scaling;
public:
ImportSTLSetup(struct GUIHelperInterface* helper);
ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportSTLSetup();
virtual void initPhysics();
@@ -31,10 +34,19 @@ public:
};
ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName)
:CommonRigidBodyBase(helper),
m_scaling(btVector3(10,10,10))
{
if (fileName)
{
m_fileName = fileName;
m_scaling = btVector3(0.01,0.01,0.01);
} else
{
m_fileName = "l_finger_tip.stl";
}
}
ImportSTLSetup::~ImportSTLSetup()
@@ -51,17 +63,16 @@ void ImportSTLSetup::initPhysics()
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
const char* fileName = "l_finger_tip.stl";
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
{
b3Warning("Cannot find file %s\n", fileName);
b3Warning("Cannot find file %s\n", m_fileName);
return;
}
btVector3 shift(0,0,0);
btVector3 scaling(10,10,10);
// int index=10;
{
@@ -81,12 +92,12 @@ void ImportSTLSetup::initPhysics()
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,m_scaling);
}
}
class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options)
{
return new ImportSTLSetup(options.m_guiHelper);
return new ImportSTLSetup(options.m_guiHelper, options.m_fileName);
}

17
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -267,14 +267,25 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
mass = link->m_inertia.m_mass;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{
mass = 0.f;
localInertiaDiagonal.setValue(0,0,0);
}
else
{
mass = link->m_inertia.m_mass;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
inertialFrame = link->m_inertia.m_linkLocalFrame;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
else
{

14
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -529,6 +529,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
@@ -572,7 +574,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
@@ -1240,6 +1242,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
TiXmlElement* stat = robot_xml->FirstChildElement("static");
if (0!=stat)
{
int val = int(atof(stat->GetText()));
if (val==1)
{
localModel->m_overrideFixedBase = true;
}
}
// Get robot name
const char *name = robot_xml->Attribute("name");

7
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -134,6 +134,13 @@ struct UrdfModel
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
bool m_overrideFixedBase;
UrdfModel()
:m_overrideFixedBase(false)
{
m_rootTransformInWorld.setIdentity();
}
};

30
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -102,11 +102,11 @@ float loop;
}
-(void)drawRect:(NSRect)rect
{
if (([self frame].size.width != m_lastWidth) || ([self frame].size.height != m_lastHeight))
{
m_lastWidth = [self frame].size.width;
m_lastHeight = [self frame].size.height;
// Only needed on resize:
[m_context clearDrawable];
@@ -114,7 +114,6 @@ float loop;
float width = [self frame].size.width;
float height = [self frame].size.height;
// Get view dimensions in pixels
// glViewport(0,0,10,10);
@@ -209,16 +208,12 @@ struct MacOpenGLWindowInternalData
m_myview = 0;
m_pool = 0;
m_window = 0;
m_width = -1;
m_height = -1;
m_exitRequested = false;
}
NSApplication* m_myApp;
TestView* m_myview;
NSAutoreleasePool* m_pool;
NSWindow* m_window;
int m_width;
int m_height;
bool m_exitRequested;
};
@@ -294,8 +289,6 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
if (m_internalData)
closeWindow();
int width = ci.m_width;
int height = ci.m_height;
const char* windowTitle = ci.m_title;
@@ -303,9 +296,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
m_internalData = new MacOpenGLWindowInternalData;
m_internalData->m_width = width;
m_internalData->m_height = height;
m_internalData->m_pool = [NSAutoreleasePool new];
m_internalData->m_myApp = [NSApplication sharedApplication];
//myApp = [MyApp sharedApplication];
@@ -373,7 +364,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[newItem release];
*/
NSRect frame = NSMakeRect(0., 0., width, height);
NSRect frame = NSMakeRect(0., 0., ci.m_width, ci.m_height);
m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame
@@ -423,9 +414,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[m_internalData->m_window makeKeyAndOrderFront: nil];
[m_internalData->m_myview MakeCurrent];
m_internalData->m_width = m_internalData->m_myview.GetWindowWidth;
m_internalData->m_height = m_internalData->m_myview.GetWindowHeight;
[NSApp activateIgnoringOtherApps:YES];
@@ -1035,13 +1024,13 @@ void MacOpenGLWindow::startRendering()
float aspect;
//b3Vector3 extents;
if (m_internalData->m_width > m_internalData->m_height)
if (getWidth() > getHeight())
{
aspect = (float)m_internalData->m_width / (float)m_internalData->m_height;
aspect = (float)getWidth() / (float)getHeight();
//extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
aspect = (float)m_internalData->m_height / (float)m_internalData->m_width;
aspect = (float)getHeight() / (float)getWidth();
//extents.setValue(1.0f, aspect*1.f,0);
}
@@ -1082,7 +1071,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
NSOpenGLContext *foo = [NSOpenGLContext currentContext];
// get the url of a .txt file
NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",nil];
NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",@"stl",nil];
[zOpenPanel setAllowedFileTypes:zAryOfExtensions];
NSInteger zIntResult = [zOpenPanel runModal];
@@ -1136,6 +1125,7 @@ int MacOpenGLWindow::getWidth() const
{
if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowWidth)
return m_internalData->m_myview.GetWindowWidth;
return 0;
}
@@ -1152,7 +1142,7 @@ void MacOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback)
[m_internalData->m_myview setResizeCallback:resizeCallback];
if (resizeCallback)
{
(resizeCallback)(m_internalData->m_width,m_internalData->m_height);
(resizeCallback)(getWidth(), getHeight());
}
}

6
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -147,7 +147,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight());
m_app->m_renderer->enableBlend(true);
const char* fileName = "teddy.obj";//cube.obj";//textured_sphere_smooth.obj";//cube.obj";
const char* fileName = "textured_sphere_smooth.obj";
fileName = "cube.obj";
{
@@ -181,7 +183,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_guiHelper->getRenderInterface()->writeTransforms();
m_internalData->m_shapePtr.push_back(0);
TinyRenderObjectData* ob = new TinyRenderObjectData(m_internalData->m_width,m_internalData->m_height,
TinyRenderObjectData* ob = new TinyRenderObjectData(
m_internalData->m_rgbColorBuffer,
m_internalData->m_depthBuffer);
//ob->loadModel("cube.obj");

40
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -5,6 +5,29 @@
#include "SharedMemoryCommands.h"
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_SDF;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
@@ -146,6 +169,8 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
return 0;
}
@@ -154,6 +179,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
return 0;
}
@@ -162,6 +188,8 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
return 0;
}
@@ -170,6 +198,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
return 0;
}
@@ -179,6 +208,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
@@ -705,6 +735,16 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height )
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_pixelWidth = width;
command->m_requestPixelDataArguments.m_pixelHeight = height;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;
}
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -68,6 +68,7 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
@@ -88,6 +89,8 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///Set joint control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);

66
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -21,8 +21,8 @@ struct MyMotorInfo2
int m_qIndex;
};
int camVisualizerWidth = 640;//1024/3;
int camVisualizerHeight = 480;//768/3;
int camVisualizerWidth = 320;//1024/3;
int camVisualizerHeight = 240;//768/3;
#define MAX_NUM_MOTORS 128
@@ -77,10 +77,10 @@ protected:
virtual void resetCamera()
{
float dist = 5;
float dist = 1.1;
float pitch = 50;
float yaw = 35;
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
@@ -156,16 +156,12 @@ protected:
{
for (int i=0;i<m_numMotors;i++)
{
// btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
// int uIndex = m_motorTargetVelocities[i].m_uIndex;
// b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
int qIndex = m_motorTargetPositions[i].m_qIndex;
int uIndex = m_motorTargetPositions[i].m_uIndex;
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
b3JointControlSetKp(commandHandle, uIndex, 0.1);
b3JointControlSetKd(commandHandle, uIndex, 0.0);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
}
@@ -252,7 +248,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -430,6 +427,11 @@ PhysicsClientExample::~PhysicsClientExample()
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas && (m_canvasIndex>=0))
{
m_canvas->destroyCanvas(m_canvasIndex);
}
b3Printf("~PhysicsClientExample\n");
}
@@ -527,7 +529,6 @@ void PhysicsClientExample::initPhysics()
m_selectedBody = -1;
m_prevSelectedBody = -1;
if (m_options == eCLIENTEXAMPLE_SERVER)
{
m_canvas = m_guiHelper->get2dCanvasInterface();
if (m_canvas)
@@ -557,13 +558,22 @@ void PhysicsClientExample::initPhysics()
}
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
//m_physicsClientHandle = b3ConnectPhysicsDirect();
if (m_options == eCLIENTEXAMPLE_SERVER)
{
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
if (m_options == eCLIENTEXAMPLE_DIRECT)
{
m_physicsClientHandle = b3ConnectPhysicsDirect();
} else
{
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
}
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client");
@@ -603,25 +613,25 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
{
static int counter=0;
char msg[1024];
sprintf(msg,"Camera image %d OK\n",counter++);
// static int counter=0;
// char msg[1024];
// sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
{
for (int i=0;i<imageData.m_pixelWidth;i++)
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
for (int j=0;j<camVisualizerHeight;j++)
{
int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4;
int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,i,camVisualizerHeight-1-j,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
@@ -633,11 +643,11 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
m_canvas->refreshImageData(m_canvasIndex);
}
b3Printf(msg);
// b3Printf(msg);
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{
b3Printf("Camera image FAILED\n");
b3Warning("Camera image FAILED\n");
}
@@ -707,8 +717,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
b3Printf("Status after reset: %d",statusType);
//int statusType = b3GetStatusType(status);
//b3Printf("Status after reset: %d",statusType);
}
}
} else

2
examples/SharedMemory/PhysicsClientExample.h
View File

@@ -4,7 +4,7 @@
enum ClientExampleOptions
{
eCLIENTEXAMPLE_LOOPBACK=1,
eCLIENTEAXMPLE_DIRECT=2,
eCLIENTEXAMPLE_DIRECT=2,
eCLIENTEXAMPLE_SERVER=3,
};

11
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -448,8 +448,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
unsigned char* rgbaPixelsReceived =
(unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
printf("pixel = %d\n", rgbaPixelsReceived[0]);
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
@@ -461,7 +468,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_CAMERA_IMAGE_FAILED:
{
b3Printf("Camera image FAILED\n");
b3Warning("Camera image FAILED\n");
break;
}

94
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -33,6 +33,12 @@ struct PhysicsDirectInternalData
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
int m_cachedCameraPixelsWidth;
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
PhysicsServerCommandProcessor* m_commandProcessor;
PhysicsDirectInternalData()
@@ -167,6 +173,81 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED);
if (m_data->m_verboseOutput)
{
b3Printf("Camera image OK\n");
}
int numBytesPerPixel = 4;//RGBA
int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
m_data->m_cachedCameraPixelsWidth = 0;
m_data->m_cachedCameraPixelsHeight = 0;
int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
unsigned char* rgbaPixelsReceived =
(unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
= rgbaPixelsReceived[i];
}
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
{
// continue requesting remaining pixels
command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
command.m_requestPixelDataArguments.m_startPixelIndex =
serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
} else
{
m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
}
}
} while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
return m_data->m_hasStatus;
}
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
{
if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@@ -174,6 +255,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return processDebugLines(command);
}
if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
{
return processCamera(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
@@ -333,9 +419,9 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
{
if (cameraData)
{
cameraData->m_pixelHeight = 0;
cameraData->m_pixelWidth = 0;
cameraData->m_depthValues = 0;
cameraData->m_rgbColorData = 0;
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
}
}

2
examples/SharedMemory/PhysicsDirect.h
View File

@@ -19,6 +19,8 @@ protected:
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
public:
PhysicsDirect();

68
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -972,6 +972,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int width, height;
int numPixelsCopied = 0;
if (
(clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) &&
(clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0)
{
m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
clientCmd.m_requestPixelDataArguments.m_pixelHeight);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0)
{
m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0);
@@ -1003,8 +1011,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0)
{
printf("-------------------------------\nRendering\n");
// printf("-------------------------------\nRendering\n");
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0)
{
m_data->m_visualConverter.render(
@@ -1192,12 +1201,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->clearForcesAndTorques();
int torqueIndex = 0;
btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
btVector3 f(0,0,0);
btVector3 t(0,0,0);
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
}
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
@@ -1206,7 +1221,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{
double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
double torque = 0.f;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++;
}
@@ -1233,10 +1250,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
btScalar desiredVelocity = 0.f;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0)
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
@@ -1247,6 +1269,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
if (m_data->m_verboseOutput)
@@ -1271,11 +1294,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
btScalar desiredVelocity = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0)
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
btScalar desiredPosition = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0)
desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
btScalar kp = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0)
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
btScalar kd = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0)
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
int dof1 = 0;
btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
@@ -1288,9 +1319,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(1000);//maxImp);
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}

25
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -30,6 +30,7 @@
#define MAX_DEGREE_OF_FREEDOM 64
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
@@ -54,6 +55,16 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z)
return tmp;
}
enum EnumSdfArgsUpdateFlags
{
SDF_ARGS_FILE_NAME=1,
};
struct SdfArgs
{
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
};
enum EnumUrdfArgsUpdateFlags
{
URDF_ARGS_FILE_NAME=1,
@@ -118,12 +129,16 @@ struct RequestPixelDataArgs
float m_viewMatrix[16];
float m_projectionMatrix[16];
int m_startPixelIndex;
int m_pixelWidth;
int m_pixelHeight;
};
enum EnumRequestPixelDataUpdateFlags
{
REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL=2,
REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
};
@@ -179,6 +194,15 @@ struct SendDesiredStateArgs
};
enum EnumSimDesiredStateUpdateFlags
{
SIM_DESIRED_STATE_HAS_Q=1,
SIM_DESIRED_STATE_HAS_QDOT=2,
SIM_DESIRED_STATE_HAS_KD=4,
SIM_DESIRED_STATE_HAS_KP=8,
SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
};
enum EnumSimParamUpdateFlags
{
@@ -287,6 +311,7 @@ struct SharedMemoryCommand
union
{
struct UrdfArgs m_urdfArguments;
struct SdfArgs m_sdfArguments;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
enum EnumSharedMemoryClientCommand
{
CMD_LOAD_URDF,
CMD_LOAD_SDF,
CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,
@@ -35,7 +36,8 @@ enum EnumSharedMemoryServerStatus
CMD_CLIENT_COMMAND_COMPLETED,
//the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
CMD_UNKNOWN_COMMAND_FLUSHED,
CMD_SDF_LOADING_COMPLETED,
CMD_SDF_LOADING_FAILED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,

19
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -462,7 +462,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_swWidth,m_data->m_swHeight,m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor);
visuals->m_renderObjects.push_back(tinyObj);
}
@@ -541,7 +541,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
lightDirWorld.normalize();
printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
// printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
for (int i=0;i<m_data->m_swRenderInstances.size();i++)
{
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
@@ -590,6 +590,17 @@ void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height
height = m_data->m_swHeight;
}
void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
{
m_data->m_swWidth = width;
m_data->m_swHeight = height;
m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
}
void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
{
int w = m_data->m_rgbColorBuffer.get_width();
@@ -612,6 +623,10 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
{
for (int i=0;i<numRequestedPixels;i++)
{
if (depthBuffer)
{
depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
}
if (pixelsRGBA)
{
pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];

1
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -24,6 +24,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
void resetAll();
void getWidthAndHeight(int& width, int& height);
void setWidthAndHeight(int width, int height);
void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);

6
examples/SimpleOpenGL3/main.cpp
View File

@@ -14,8 +14,8 @@ static b3MouseButtonCallback sOldMouseButtonCB = 0;
static b3KeyboardCallback sOldKeyboardCB = 0;
//static b3RenderCallback sOldRenderCB = 0;
float gWidth = 0 ;
float gHeight = 0;
float gWidth = 1024;
float gHeight = 768;
void MyWheelCallback(float deltax, float deltay)
{
@@ -61,7 +61,7 @@ int main(int argc, char* argv[])
b3CommandLineArgs myArgs(argc,argv);
SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768,true);
SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",gWidth,gHeight,true);
app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);

5
examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
View File

@@ -110,8 +110,9 @@ public:
int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices);
if (shapeIndex>=0)
{
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
float rgbaColor[4] = {1,1,1,1};
swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
m_swRenderObjects.insert(shapeIndex,swObj);
}

5
examples/StandaloneMain/main_tinyrenderer_single_example.cpp
View File

@@ -253,8 +253,9 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
{
int shapeIndex = m_swRenderObjects.size();
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
float rgbaColor[4] = {1,1,1,1};
swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
m_swRenderObjects.insert(shapeIndex,swObj);
return shapeIndex;

38
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -76,21 +76,20 @@ struct Shader : public IShader {
float diff = ambient+b3Min(b3Max(0.f, bn*m_light_dir_local),(1-ambient));
//float diff = b3Max(0.f, n*m_light_dir_local);
color = m_model->diffuse(uv)*diff;
//colors are store in BGRA?
color = TGAColor(color[0]*m_colorRGBA[2],
color[1]*m_colorRGBA[1],
color[2]*m_colorRGBA[0],
color[3]*m_colorRGBA[3]);
//warning: bgra color is swapped to rgba to upload texture
color.bgra[0] *= m_colorRGBA[0];
color.bgra[1] *= m_colorRGBA[1];
color.bgra[2] *= m_colorRGBA[2];
color.bgra[3] *= m_colorRGBA[3];
return false;
}
};
TinyRenderObjectData::TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_width(width),
m_height(height),
m_rgbColorBuffer(rgbColorBuffer),
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_model(0),
m_userData(0),
@@ -102,11 +101,6 @@ m_userIndex(-1)
m_lightDirWorld.setValue(0,0,0);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
m_viewMatrix = lookat(eye, center, up);
//m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
//m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
m_viewportMatrix = viewport(0,0,width,height);
m_projectionMatrix = projection(-1.f/(eye-center).norm());
}
@@ -239,6 +233,9 @@ TinyRenderObjectData::~TinyRenderObjectData()
void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
{
int width = renderData.m_rgbColorBuffer.get_width();
int height = renderData.m_rgbColorBuffer.get_height();
Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
Model* model = renderData.m_model;
if (0==model)
@@ -246,13 +243,8 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
//renderData.m_viewMatrix = lookat(eye, center, up);
int width = renderData.m_width;
int height = renderData.m_height;
//renderData.m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
renderData.m_viewportMatrix = viewport(0,0,renderData.m_width,renderData.m_height);
//renderData.m_projectionMatrix = projection(-1.f/(eye-center).norm());
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
TGAImage& frame = renderData.m_rgbColorBuffer;
@@ -264,7 +256,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA());
printf("Render %d triangles.\n",model->nfaces());
//printf("Render %d triangles.\n",model->nfaces());
for (int i=0; i<model->nfaces(); i++)
{

5
examples/TinyRenderer/TinyRenderer.h
View File

@@ -25,12 +25,11 @@ struct TinyRenderObjectData
//class IShader* m_shader; todo(erwincoumans) expose the shader, for now we use a default shader
//Output
int m_width;
int m_height;
TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;
TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);

2
examples/TinyRenderer/main.cpp
View File

@@ -86,7 +86,7 @@ int main(int argc, char* argv[])
b3AlignedObjectArray<float> depthBuffer;
depthBuffer.resize(gWidth*gHeight);
TinyRenderObjectData renderData(textureWidth, textureHeight,rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
TinyRenderObjectData renderData(rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
//renderData.loadModel("african_head/african_head.obj");
renderData.loadModel("floor.obj");

6
examples/TinyRenderer/model.cpp
View File

@@ -57,9 +57,9 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid
for (int j=0;j<textureHeight;j++)
{
TGAColor color;
color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
color.bgra[3] = 255;
color.bytespp = 3;

10
examples/pybullet/CMakeLists.txt
View File

@@ -9,6 +9,15 @@ INCLUDE_DIRECTORIES(
SET(pybullet_SRCS
pybullet.c
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
../../examples/OpenGLWindow/SimpleCamera.cpp
../../examples/OpenGLWindow/SimpleCamera.h
../../examples/TinyRenderer/geometry.cpp
../../examples/TinyRenderer/model.cpp
../../examples/TinyRenderer/tgaimage.cpp
../../examples/TinyRenderer/our_gl.cpp
../../examples/TinyRenderer/TinyRenderer.cpp
../../examples/SharedMemory/InProcessMemory.cpp
../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/PhysicsClient.h
@@ -48,7 +57,6 @@ SET(pybullet_SRCS
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp
../../examples/MultiThreading/b3PosixThreadSupport.cpp
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
../../examples/MultiThreading/b3ThreadSupportInterface.cpp

180
examples/pybullet/pybullet.c
View File

@@ -134,7 +134,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
float startOrnY = 0;
float startOrnZ = 0;
float startOrnW = 1;
printf("size=%d\n", size);
//printf("size=%d\n", size);
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -164,7 +164,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
printf("urdf filename = %s\n", urdfFileName);
//printf("urdf filename = %s\n", urdfFileName);
//setting the initial position, orientation and other arguments are optional
b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -359,6 +359,176 @@ pybullet_getNumJoints(PyObject* self, PyObject* args)
}
}
static PyObject*
pybullet_setJointPositions(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
// const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes
// const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats
static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
///request an image from a simulated camera, using a software renderer.
struct b3CameraImageData imageData;
PyObject* objViewMat,* objProjMat;
int width, height;
if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat))
{
PyObject* seq;
int i, len;
PyObject* item;
float viewMatrix[16];
float projectionMatrix[16];
int valid = 1;
{
seq = PySequence_Fast(objViewMat, "expected a sequence");
len = PySequence_Size(objViewMat);
//printf("objViewMat size = %d\n", len);
if (len==16)
{
if (PyList_Check(seq))
{
for (i = 0; i < len; i++)
{
item = PyList_GET_ITEM(seq, i);
viewMatrix[i] = PyFloat_AsDouble(item);
float v = viewMatrix[i];
//printf("view %d to %f\n", i,v);
}
}
else
{
for (i = 0; i < len; i++)
{
item = PyTuple_GET_ITEM(seq,i);
viewMatrix[i] = PyFloat_AsDouble(item);
}
}
} else
{
valid = 0;
}
}
{
seq = PySequence_Fast(objProjMat, "expected a sequence");
len = PySequence_Size(objProjMat);
//printf("projMat len = %d\n", len);
if (len==16)
{
if (PyList_Check(seq))
{
for (i = 0; i < len; i++)
{
item = PyList_GET_ITEM(seq, i);
projectionMatrix[i] = PyFloat_AsDouble(item);
}
}
else
{
for (i = 0; i < len; i++)
{
item = PyTuple_GET_ITEM(seq,i);
projectionMatrix[i] = PyFloat_AsDouble(item);
}
}
} else
{
valid = 0;
}
}
Py_DECREF(seq);
if (valid)
{
b3SharedMemoryCommandHandle command;
command = b3InitRequestCameraImage(sm);
//printf("set b3RequestCameraImageSetCameraMatrices\n");
b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
b3RequestCameraImageSetPixelResolution(command,width,height);
if (b3CanSubmitCommand(sm))
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CAMERA_IMAGE_COMPLETED)
{
PyObject *item2;
PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
b3GetCameraImageData(sm, &imageData);
//todo: error handling if image size is 0
pyResultList = PyTuple_New(4);
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
PyObject *pylistPos;
PyObject* pylistDep;
//printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight);
{
PyObject *item;
int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values
int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
pylistPos = PyTuple_New(num);
pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
for (int i=0;i<imageData.m_pixelWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
{
int depIndex = i+j*imageData.m_pixelWidth;
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
PyTuple_SetItem(pylistDep, depIndex, item);
for (int p=0;p<bytesPerPixel;p++)
{
int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p;
item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
PyTuple_SetItem(pylistPos, pixelIndex, item);
}
}
}
}
PyTuple_SetItem(pyResultList, 2,pylistPos);
PyTuple_SetItem(pyResultList, 3,pylistDep);
return pyResultList;
}
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyMethodDef SpamMethods[] = {
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
@@ -378,6 +548,12 @@ static PyMethodDef SpamMethods[] = {
{"setGravity", pybullet_setGravity, METH_VARARGS,
"Set the gravity acceleration (x,y,z)."},
{"initializeJointPositions", pybullet_setJointPositions, METH_VARARGS,
"Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."},
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS,
"Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},

4
src/LinearMath/btMatrix3x3.h
View File

@@ -1329,7 +1329,9 @@ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
c0 = _mm_and_ps(c0, c1);
c0 = _mm_and_ps(c0, c2);
return (0x7 == _mm_movemask_ps((__m128)c0));
int m = _mm_movemask_ps((__m128)c0);
return (0x7 == (m & 0x7));
#else
return
( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&