diff --git a/.travis.yml b/.travis.yml index bb6dcde2e..25053a90d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,10 +5,14 @@ os: compiler: - gcc - clang +addons: + apt: + packages: + - python3 script: - echo "CXX="$CXX - echo "CC="$CC - - cmake . -G "Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror + - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - make -j8 - ctest -j8 --output-on-failure # Build again with double precision diff --git a/CMakeLists.txt b/CMakeLists.txt index b34c7f996..083835f6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,14 +191,15 @@ IF (APPLE) ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) + +FIND_PACKAGE(PythonLibs) OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF) + IF(BUILD_PYBULLET) IF(WIN32) - FIND_PACKAGE(PythonLibs 3.4 REQUIRED) SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE) ELSE(WIN32) - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE) ENDIF(WIN32) ENDIF(BUILD_PYBULLET) diff --git a/README.md b/README.md index 711ebcf98..942500cb9 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ The entire collision detection and rigid body dynamics is executed on the GPU. A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better. We succesfully tested the software under Windows, Linux and Mac OSX. The software currently doesn't work on OpenCL CPU devices. It might run -on a laptop GPU but performance is likely not very good. Note that +on a laptop GPU but performance will not likely be very good. Note that often an OpenCL drivers fails to compile a kernel. Some unit tests exist to track down the issue, but more work is required to cover all OpenCL kernels. @@ -88,7 +88,7 @@ You can just run it though a terminal/command prompt, or by clicking it. [--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666) ``` -You can use mouse picking to grab objects. When holding the ALT of CONTROL key, you have Maya style camera mouse controls. -Press F1 to create series of screenshot. Hit ESCAPE to exit the demo app. +You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls. +Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app. See docs folder for further information. diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh index 1405060f4..28354357b 100755 --- a/build_and_run_cmake.sh +++ b/build_and_run_cmake.sh @@ -2,6 +2,6 @@ rm CMakeCache.txt mkdir build_cmake cd build_cmake -cmake .. +cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release .. make -j12 examples/ExampleBrowser/App_ExampleBrowser diff --git a/data/cube.mtl b/data/cube.mtl index fca828974..26b4b44e4 100644 --- a/data/cube.mtl +++ b/data/cube.mtl @@ -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 diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf index 1b6f8320d..0df20cff7 100644 --- a/data/kuka_iiwa/model.sdf +++ b/data/kuka_iiwa/model.sdf @@ -32,6 +32,12 @@ meshes/link_0.stl + + 1 0 0 1 + 0 0 1 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -407,4 +413,4 @@ - \ No newline at end of file + diff --git a/data/sphere2.urdf b/data/sphere2.urdf index 891e018cb..fb0108b3d 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -9,7 +9,7 @@ - + diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf index 24c0854bd..e19b73b43 100644 --- a/data/two_cubes.sdf +++ b/data/two_cubes.sdf @@ -86,6 +86,7 @@ 0 + 1 0.512455 -1.58317 0.5 0 -0 0 @@ -121,11 +122,13 @@ - - 1 1 1 - - + + 1 1 1 + cube.obj + + + 1 1 1 1 + 1 1 1 1 0 diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp index f5bd9486c..b6365df36 100644 --- a/examples/BasicDemo/BasicExample.cpp +++ b/examples/BasicDemo/BasicExample.cpp @@ -42,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase float dist = 41; float pitch = 52; float yaw = 35; - float targetPos[3]={0,0.46,0}; + float targetPos[3]={0,0,0}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } }; diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index 1105955dc..2bc8a0ea4 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -167,6 +167,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans } } } + delete hull; } } else { diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 97bb1ad26..c749e04ba 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -245,7 +245,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 diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 179f18036..b245cdecb 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -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) diff --git a/examples/ExtendedTutorials/premake4.lua b/examples/ExtendedTutorials/premake4.lua index ea1fc507e..92ac12d45 100644 --- a/examples/ExtendedTutorials/premake4.lua +++ b/examples/ExtendedTutorials/premake4.lua @@ -38,7 +38,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -89,7 +90,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -153,7 +155,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -213,6 +216,7 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 3f6553b81..67b64fd26 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -1,15 +1,12 @@ #include "b3ImportMeshUtility.h" #include -#include "../../OpenGLWindow/GLInstancingRenderer.h" -#include"Wavefront/tiny_obj_loader.h" -#include "../../OpenGLWindow/GLInstanceGraphicsShape.h" -#include "btBulletDynamicsCommon.h" -#include "../../OpenGLWindow/SimpleOpenGL3App.h" +#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" +#include "LinearMath/btVector3.h" #include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h" #include "../../Utils/b3ResourcePath.h" #include "Bullet3Common/b3FileUtils.h" -#include "stb_image/stb_image.h" +#include "../../ThirdPartyLibs/stb_image/stb_image.h" bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData) @@ -88,28 +85,4 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& return false; } -int b3ImportMeshUtility::loadAndRegisterMeshFromFile(const std::string& fileName, CommonRenderInterface* renderer) -{ - int shapeId = -1; - b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData)) - { - int textureIndex = -1; - - if (meshData.m_textureImage) - { - textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight); - } - - shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], - meshData.m_gfxShape->m_numvertices, - &meshData.m_gfxShape->m_indices->at(0), - meshData.m_gfxShape->m_numIndices, - B3_GL_TRIANGLES, - textureIndex); - delete meshData.m_gfxShape; - delete meshData.m_textureImage; - } - return shapeId; -} diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h index 96c061eaf..7bf6af841 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h @@ -15,7 +15,6 @@ struct b3ImportMeshData class b3ImportMeshUtility { public: -static int loadAndRegisterMeshFromFile(const std::string& fileName, class CommonRenderInterface* renderer); static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData); diff --git a/examples/Importers/ImportObjDemo/ImportObjExample.cpp b/examples/Importers/ImportObjDemo/ImportObjExample.cpp index a8995c148..2fc11afa3 100644 --- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp +++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp @@ -56,7 +56,31 @@ ImportObjSetup::~ImportObjSetup() - +int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer) +{ + int shapeId = -1; + + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData)) + { + int textureIndex = -1; + + if (meshData.m_textureImage) + { + textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight); + } + + shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], + meshData.m_gfxShape->m_numvertices, + &meshData.m_gfxShape->m_indices->at(0), + meshData.m_gfxShape->m_numIndices, + B3_GL_TRIANGLES, + textureIndex); + delete meshData.m_gfxShape; + delete meshData.m_textureImage; + } + return shapeId; +} @@ -77,7 +101,7 @@ void ImportObjSetup::initPhysics() btVector3 scaling(1,1,1); btVector3 color(1,1,1); - int shapeId = b3ImportMeshUtility::loadAndRegisterMeshFromFile(m_fileName, m_guiHelper->getRenderInterface()); + int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface()); if (shapeId>=0) { //int id = diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 54883298b..f55cc9758 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -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;iregisterNameForPointer(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_numMotorsc_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++; + } + } + + } + } } diff --git a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp index b7f25c3dc..c05138ef1 100644 --- a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp +++ b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp @@ -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); } diff --git a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h index f670dfeec..65bdd330b 100644 --- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h +++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h @@ -66,17 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) for (int i=0;ivertex0[v]; - v1.xyzw[v] = tri->vertex1[v]; - v2.xyzw[v] = tri->vertex2[v]; - v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v]; + v0.xyzw[v] = tmp.vertex0[v]; + v1.xyzw[v] = tmp.vertex1[v]; + v2.xyzw[v] = tmp.vertex2[v]; + v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v]; } v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 7e9624d64..9b6eb8ae1 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: #include "BulletUrdfImporter.h" - +#include "../../CommonInterfaces/CommonRenderInterface.h" #include "URDFImporterInterface.h" #include "btBulletCollisionCommon.h" @@ -26,12 +26,20 @@ subject to the following restrictions: #include #include "../../Utils/b3ResourcePath.h" +#include "../ImportMeshUtility/b3ImportMeshUtility.h" #include #include #include "UrdfParser.h" +struct MyTexture +{ + int m_width; + int m_height; + unsigned char* textureData; +}; + struct BulletURDFInternalData { UrdfParser m_urdfParser; @@ -267,14 +275,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 { @@ -578,7 +597,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co } -static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) +static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) { @@ -682,7 +701,23 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha { case FILE_OBJ: { - glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); +// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) + { + + if (meshData.m_textureImage) + { + MyTexture texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; + } + break; } @@ -903,7 +938,8 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP btAlignedObjectArray vertices; btAlignedObjectArray indices; btTransform startTrans; startTrans.setIdentity(); - + btAlignedObjectArray textures; + const UrdfModel& model = m_data->m_urdfParser.getModel(); UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); if (linkPtr) @@ -923,14 +959,34 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); } - convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices); + convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); } } if (vertices.size() && indices.size()) { - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); +// graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); + //graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); + + CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface(); + + if (renderer) + { + int textureIndex = -1; + if (textures.size()) + { + textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height); + } + graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex); + + } + } + + //delete textures + for (int i=0;iAttribute("filename"); - - if (shape->Attribute("scale")) + geom.m_meshScale.setValue(1,1,1); + + if (shape->Attribute("scale")) { - parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger); + if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger)) + { + logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n"); + std::string scalar_str = shape->Attribute("scale"); + double scaleFactor = urdfLexicalCast(scalar_str.c_str()); + if (scaleFactor) + { + geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); + } + } } else { - geom.m_meshScale.setValue(1,1,1); } } } @@ -490,29 +499,47 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* // Material TiXmlElement *mat = config->FirstChildElement("material"); //todo(erwincoumans) skip materials in SDF for now (due to complexity) - if (mat && !m_parseSDF) + if (mat) { - // get material name - if (!mat->Attribute("name")) - { - logger->reportError("Visual material must contain a name attribute"); - return false; - } - visual.m_materialName = mat->Attribute("name"); - - // try to parse material element in place - - TiXmlElement *t = mat->FirstChildElement("texture"); - TiXmlElement *c = mat->FirstChildElement("color"); - if (t||c) - { - if (parseMaterial(visual.m_localMaterial, mat,logger)) - { - UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); - model.m_materials.insert(matPtr->m_name.c_str(),matPtr); - visual.m_hasLocalMaterial = true; - } - } + if (m_parseSDF) + { + UrdfMaterial* matPtr = new UrdfMaterial; + matPtr->m_name = "mat"; + TiXmlElement *diffuse = mat->FirstChildElement("diffuse"); + if (diffuse) { + std::string diffuseText = diffuse->GetText(); + btVector4 rgba(1,0,0,1); + parseVector4(rgba,diffuseText); + matPtr->m_rgbaColor = rgba; + model.m_materials.insert(matPtr->m_name.c_str(),matPtr); + visual.m_materialName = "mat"; + visual.m_hasLocalMaterial = true; + } + } + else + { + // get material name + if (!mat->Attribute("name")) + { + logger->reportError("Visual material must contain a name attribute"); + return false; + } + visual.m_materialName = mat->Attribute("name"); + + // try to parse material element in place + + TiXmlElement *t = mat->FirstChildElement("texture"); + TiXmlElement *c = mat->FirstChildElement("color"); + if (t||c) + { + if (parseMaterial(visual.m_localMaterial, mat,logger)) + { + UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); + model.m_materials.insert(matPtr->m_name.c_str(),matPtr); + visual.m_hasLocalMaterial = true; + } + } + } } return true; @@ -529,6 +556,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 +601,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 +1269,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"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1a855f281..b83c0b3c8 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -134,6 +134,13 @@ struct UrdfModel btHashMap m_joints; btArray m_rootLinks; + bool m_overrideFixedBase; + + UrdfModel() + :m_overrideFixedBase(false) + { + m_rootTransformInWorld.setIdentity(); + } }; diff --git a/examples/InverseDynamics/premake4.lua b/examples/InverseDynamics/premake4.lua index 3871526ce..53c4fa2ce 100644 --- a/examples/InverseDynamics/premake4.lua +++ b/examples/InverseDynamics/premake4.lua @@ -38,7 +38,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -89,7 +90,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -153,7 +155,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -211,6 +214,7 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm index 301608cca..f4f853889 100644 --- a/examples/OpenGLWindow/MacOpenGLWindow.mm +++ b/examples/OpenGLWindow/MacOpenGLWindow.mm @@ -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()); } } diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index d7e9f05aa..fd3f80e41 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -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"); diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 59a1a4e90..07b10aa13 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -26,7 +26,7 @@ public: virtual int getNumJoints(int bodyIndex) const = 0; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0; virtual void setSharedMemoryKey(int key) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 31b8df6b9..e67a2c144 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1,10 +1,34 @@ #include "PhysicsClientC_API.h" #include "PhysicsClientSharedMemory.h" #include "Bullet3Common/b3Scalar.h" +#include "Bullet3Common/b3Vector3.h" #include #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 (lenm_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) { @@ -127,7 +151,12 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand } -b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode) +b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode) +{ + return b3JointControlCommandInit2(physClient,0,controlMode); +} + +b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -136,8 +165,12 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy b3Assert(command); command->m_type = CMD_SEND_DESIRED_STATE; command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; - command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0; + command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId; command->m_updateFlags = 0; + for (int i=0;im_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0; + } return (b3SharedMemoryCommandHandle) command; } @@ -146,6 +179,9 @@ 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; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q; + return 0; } @@ -154,6 +190,9 @@ 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; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP; + return 0; } @@ -162,6 +201,9 @@ 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; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD; + return 0; } @@ -170,6 +212,9 @@ 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; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT; + return 0; } @@ -179,6 +224,9 @@ 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; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + return 0; } @@ -187,6 +235,9 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + return 0; } @@ -335,6 +386,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) { + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -343,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl command->m_type = CMD_INIT_POSE; command->m_updateFlags =0; command->m_initPoseArgs.m_bodyUniqueId = bodyIndex; + //a bit slow, initialing the full range to zero... + for (int i=0;im_initPoseArgs.m_hasInitialStateQ[i] = 0; + } return (b3SharedMemoryCommandHandle) command; } @@ -355,6 +412,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle command->m_initPoseArgs.m_initialStateQ[0] = startPosX; command->m_initPoseArgs.m_initialStateQ[1] = startPosY; command->m_initPoseArgs.m_initialStateQ[2] = startPosZ; + + command->m_initPoseArgs.m_hasInitialStateQ[0] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[1] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[2] = 1; + return 0; } @@ -368,6 +430,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan command->m_initPoseArgs.m_initialStateQ[4] = startOrnY; command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ; command->m_initPoseArgs.m_initialStateQ[6] = startOrnW; + + command->m_initPoseArgs.m_hasInitialStateQ[3] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[4] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[5] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[6] = 1; + return 0; } @@ -380,6 +448,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand for (int i=0;im_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i]; + command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1; } return 0; } @@ -396,6 +465,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0) { command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition; + command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1; } return 0; } @@ -403,7 +473,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar -b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -414,7 +484,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys command->m_type = CMD_CREATE_SENSOR; command->m_updateFlags = 0; command->m_createSensorArguments.m_numJointSensorChanges = 0; - command->m_createSensorArguments.m_bodyUniqueId = 0; + command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId; return (b3SharedMemoryCommandHandle) command; } @@ -489,6 +559,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) return CMD_INVALID_STATUS; } +int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity) +{ + int numBodies = 0; + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + + if (status) + { + switch (status->m_type) + { + case CMD_SDF_LOADING_COMPLETED: + { + int i,maxBodies; + numBodies = status->m_sdfLoadedArgs.m_numBodies; + maxBodies = btMin(bodyIndicesCapacity, numBodies); + for (i=0;im_sdfLoadedArgs.m_bodyUniqueIds[i]; + } + break; + } + } + } + + return numBodies; +} + int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; @@ -528,6 +625,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* jointReactionForces[]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SendActualStateArgs &args = status->m_sendActualStateArgs; + btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED); + if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + return false; + if (bodyUniqueId) { *bodyUniqueId = args.m_bodyUniqueId; } @@ -592,10 +693,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) } -void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info) +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; - cl->getJointInfo(bodyIndex, linkIndex,*info); + return cl->getJointInfo(bodyIndex, linkIndex,*info); } b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, @@ -705,6 +806,94 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } +void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]) +{ + b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]); + b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]); + b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]); + b3Vector3 f = (center - eye).normalized(); + b3Vector3 u = up.normalized(); + b3Vector3 s = (f.cross(u)).normalized(); + u = s.cross(f); + + float viewMatrix[16]; + + viewMatrix[0*4+0] = s.x; + viewMatrix[1*4+0] = s.y; + viewMatrix[2*4+0] = s.z; + + viewMatrix[0*4+1] = u.x; + viewMatrix[1*4+1] = u.y; + viewMatrix[2*4+1] = u.z; + + viewMatrix[0*4+2] =-f.x; + viewMatrix[1*4+2] =-f.y; + viewMatrix[2*4+2] =-f.z; + + viewMatrix[0*4+3] = 0.f; + viewMatrix[1*4+3] = 0.f; + viewMatrix[2*4+3] = 0.f; + + viewMatrix[3*4+0] = -s.dot(eye); + viewMatrix[3*4+1] = -u.dot(eye); + viewMatrix[3*4+2] = f.dot(eye); + viewMatrix[3*4+3] = 1.f; + + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + for (int i=0;i<16;i++) + { + command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i]; + } + command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; + +} + +void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal) +{ + float frustum[16]; + + frustum[0*4+0] = (float(2) * nearVal) / (right - left); + frustum[0*4+1] = float(0); + frustum[0*4+2] = float(0); + frustum[0*4+3] = float(0); + + frustum[1*4+0] = float(0); + frustum[1*4+1] = (float(2) * nearVal) / (top - bottom); + frustum[1*4+2] = float(0); + frustum[1*4+3] = float(0); + + frustum[2*4+0] = (right + left) / (right - left); + frustum[2*4+1] = (top + bottom) / (top - bottom); + frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal); + frustum[2*4+3] = float(-1); + + frustum[3*4+0] = float(0); + frustum[3*4+1] = float(0); + frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal); + frustum[3*4+3] = float(0); + + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + for (int i=0;i<16;i++) + { + command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i]; + } + 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; @@ -714,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage } } +b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_APPLY_EXTERNAL_FORCE; + command->m_updateFlags = 0; + command->m_externalForceArguments.m_numForcesAndTorques = 0; + return (b3SharedMemoryCommandHandle) command; +} + +void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE); + int index = command->m_externalForceArguments.m_numForcesAndTorques; + command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId; + command->m_externalForceArguments.m_linkIds[index] = linkId; + command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag; + for (int i = 0; i < 3; ++i) { + command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i]; + command->m_externalForceArguments.m_positions[index+i] = position[i]; + } + + command->m_externalForceArguments.m_numForcesAndTorques++; +} + +void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE); + int index = command->m_externalForceArguments.m_numForcesAndTorques; + command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId; + command->m_externalForceArguments.m_linkIds[index] = linkId; + command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag; + + for (int i = 0; i < 3; ++i) { + command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i]; + } + command->m_externalForceArguments.m_numForcesAndTorques++; +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b202daa35..8f57f4db9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien /// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes. int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); +int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity); + int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, @@ -55,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); ///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h -void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); ///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h @@ -68,6 +70,9 @@ 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 b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]); +void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal); +void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); @@ -88,14 +93,22 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); -///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 b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); + +///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); + + +///Set joint motor 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 b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); + ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); -//Only use when controlMode is CONTROL_MODE_VELOCITY + +///Only use when controlMode is CONTROL_MODE_VELOCITY int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, @@ -127,7 +140,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar ///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors. ///This is rather inconsistent, to mix programmatical creation with loading from file. -b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient); +b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable); ///b3CreateSensorEnableIMUForLink is not implemented yet. ///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU. @@ -146,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d double rayToWorldZ); b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient); +/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates. +b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); +void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); +void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); + #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index e249977bd..a4199071a 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -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 @@ -37,6 +37,9 @@ protected: bool m_wantsTermination; btAlignedObjectArray m_userCommandRequests; + btAlignedObjectArray m_bodyUniqueIds; + + int m_sharedMemoryKey; int m_selectedBody; int m_prevSelectedBody; @@ -66,7 +69,8 @@ protected: { if (m_guiHelper && m_guiHelper->getParameterInterface()) { - int bodyIndex = comboIndex; + int itemIndex = int(atoi(name)); + int bodyIndex = m_bodyUniqueIds[itemIndex]; if (m_selectedBody != bodyIndex) { m_selectedBody = bodyIndex; @@ -77,10 +81,10 @@ protected: virtual void resetCamera() { - float dist = 5; - float pitch = 50; - float yaw = 35; - float targetPos[3]={0,0,0};//-3,2.8,-2.5}; + float dist = 4; + float pitch = 193; + float yaw = 25; + float targetPos[3]={0,0,0.5};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } @@ -154,18 +158,20 @@ protected: void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle) { + for (int i=0;im_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: @@ -363,11 +374,18 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } case CMD_SEND_DESIRED_STATE: { - // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY); - b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD); + if (m_selectedBody>=0) + { + // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD); + // b3Printf("prepare control command for body %d", m_selectedBody); + prepareControlCommand(command); + + b3SubmitClientCommand(m_physicsClientHandle, command); - break; + } + break; } case CMD_RESET_SIMULATION: { @@ -430,6 +448,11 @@ PhysicsClientExample::~PhysicsClientExample() bool deInitializeSharedMemory = true; m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); } + + if (m_canvas && (m_canvasIndex>=0)) + { + m_canvas->destroyCanvas(m_canvasIndex); + } b3Printf("~PhysicsClientExample\n"); } @@ -450,6 +473,7 @@ void PhysicsClientExample::createButtons() m_guiHelper->getParameterInterface()->removeAllParameters(); createButton("Load URDF",CMD_LOAD_URDF, isTrigger); + createButton("Load SDF",CMD_LOAD_SDF, isTrigger); createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger); createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); @@ -464,9 +488,36 @@ void PhysicsClientExample::createButtons() createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); + if (m_bodyUniqueIds.size()) + { + if (m_selectedBody<0) + m_selectedBody = 0; + + ComboBoxParams comboParams; + comboParams.m_comboboxId = 0; + comboParams.m_numItems = m_bodyUniqueIds.size(); + comboParams.m_startItem = m_selectedBody; + comboParams.m_callback = MyComboBoxCallback; + comboParams.m_userPointer = this; + //todo: get the real object name + + const char** blarray = new const char*[m_bodyUniqueIds.size()]; + + for (int i=0;igetParameterInterface()->registerComboBox(comboParams); + } + if (m_physicsClientHandle && m_selectedBody>=0) { + m_numMotors = 0; + int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody); for (int i=0;iget2dCanvasInterface(); if (m_canvas) @@ -557,13 +607,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,48 +662,78 @@ 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;isetPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex, + int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel; + m_canvas->setPixel(m_canvasIndex,i,j, imageData.m_rgbColorData[pixelIndex], imageData.m_rgbColorData[pixelIndex+1], imageData.m_rgbColorData[pixelIndex+2], - 255); -// imageData.m_rgbColorData[pixelIndex+3]); + 255); //alpha set to 255 } } 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"); } - - if (statusType == CMD_URDF_LOADING_COMPLETED) + if (statusType == CMD_SDF_LOADING_COMPLETED) { - int bodyIndex = b3GetStatusBodyIndex(status); - if (bodyIndex>=0) + int bodyIndicesOut[1024]; + int bodyCapacity = 1024; + int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity); + if (numBodies > bodyCapacity) + { + b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity); + } else + { + for (int i=0;i0) + { + m_selectedBody = bodyUniqueId; + } +/* int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId); + b3Printf("numJoints = %d", numJoints); + for (int i=0;i=0) { int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); @@ -653,7 +742,6 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3JointInfo info; b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); - } ComboBoxParams comboParams; comboParams.m_comboboxId = bodyIndex; @@ -667,12 +755,29 @@ void PhysicsClientExample::stepSimulation(float deltaTime) comboParams.m_items=blarray;//{&bla}; m_guiHelper->getParameterInterface()->registerComboBox(comboParams); - - } - + */ + } + + + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + int bodyIndex = b3GetStatusBodyIndex(status); + if (bodyIndex>=0) + { + m_bodyUniqueIds.push_back(bodyIndex); + m_selectedBody = bodyIndex; + int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); + + for (int i=0;i m_cachedCameraPixelsRGBA; btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_bodyIdsRequestInfo; + SharedMemoryStatus m_tempBackupServerStatus; + SharedMemoryStatus m_lastServerStatus; int m_counter; - bool m_serverLoadUrdfOK; + bool m_isConnected; bool m_waitingForServer; bool m_hasLastServerStatus; @@ -54,7 +57,6 @@ struct PhysicsClientSharedMemoryInternalData { m_counter(0), m_cachedCameraPixelsWidth(0), m_cachedCameraPixelsHeight(0), - m_serverLoadUrdfOK(false), m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), @@ -83,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const } -void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const +bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; info = bodyJoints->m_jointInfo[jointIndex]; + return true; } - + return false; } PhysicsClientSharedMemory::PhysicsClientSharedMemory() @@ -168,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() { return true; } + +///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo +void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + bParse::btBulletFile bf( + &this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0], + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf.setFileDNAisMemoryDNA(); + bf.parse(false); + + + BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } +} + const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { SharedMemoryStatus* stat = 0; @@ -204,8 +249,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } break; } + case CMD_SDF_LOADING_COMPLETED: { + + if (m_data->m_verboseOutput) { + b3Printf("Server loading the SDF OK\n"); + } + + break; + } case CMD_URDF_LOADING_COMPLETED: { - m_data->m_serverLoadUrdfOK = true; + if (m_data->m_verboseOutput) { b3Printf("Server loading the URDF OK\n"); } @@ -265,7 +318,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_verboseOutput) { b3Printf("Server failed loading the URDF...\n"); } - m_data->m_serverLoadUrdfOK = false; + + break; + } + + case CMD_BODY_INFO_COMPLETED: + { + if (m_data->m_verboseOutput) { + b3Printf("Received body info\n"); + } + int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyUniqueId, serverCmd); + + break; + } + case CMD_SDF_LOADING_FAILED: { + if (m_data->m_verboseOutput) { + b3Printf("Server failed loading the SDF...\n"); + } + break; } @@ -448,8 +519,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;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] @@ -461,7 +539,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { case CMD_CAMERA_IMAGE_FAILED: { - b3Printf("Camera image FAILED\n"); + b3Warning("Camera image FAILED\n"); break; } @@ -483,6 +561,50 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_waitingForServer = true; } + if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) + { + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + if (numBodies>0) + { + m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; + + for (int i=0;im_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]); + } + + int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1]; + m_data->m_bodyIdsRequestInfo.pop_back(); + + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + //now transfer the information of the individual objects etc. + command.m_type = CMD_REQUEST_BODY_INFO; + command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; + submitClientCommand(command); + return 0; + } + } + + if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED) + { + //are there any bodies left to be processed? + if (m_data->m_bodyIdsRequestInfo.size()) + { + int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1]; + m_data->m_bodyIdsRequestInfo.pop_back(); + + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + //now transfer the information of the individual objects etc. + command.m_type = CMD_REQUEST_BODY_INFO; + command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; + submitClientCommand(command); + return 0; + } else + { + m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; + } + } + if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 1081771e5..a47fc551a 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -11,7 +11,9 @@ class PhysicsClientSharedMemory : public PhysicsClient { protected: virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + public: PhysicsClientSharedMemory(); virtual ~PhysicsClientSharedMemory(); @@ -34,7 +36,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 72b5eb65e..511290778 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -33,6 +33,12 @@ struct PhysicsDirectInternalData char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + + PhysicsServerCommandProcessor* m_commandProcessor; PhysicsDirectInternalData() @@ -167,6 +173,122 @@ 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;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_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; + + +} + + +void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf.setFileDNAisMemoryDNA(); + bf.parse(false); + + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } +} + bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { if (command.m_type==CMD_REQUEST_DEBUG_LINES) @@ -174,6 +296,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) @@ -210,46 +337,35 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman break; } + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i=0;im_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, infoStatus); + } + } + break; + } case CMD_URDF_LOADING_COMPLETED: { + if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) { - bParse::btBulletFile bf( - &m_data->m_bulletStreamDataServerToClient[0], - serverCmd.m_dataStreamArguments.m_streamChunkLength); - bf.setFileDNAisMemoryDNA(); - bf.parse(false); - int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; - - BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; - m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints); - - for (int i = 0; i < bf.m_multiBodies.size(); i++) - { - int flag = bf.getFlags(); - if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) - { - Bullet::btMultiBodyDoubleData* mb = - (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; - - addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); - } else - { - Bullet::btMultiBodyFloatData* mb = - (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; - - addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); - } - } - if (bf.ok()) { - if (m_data->m_verboseOutput) - { - b3Printf("Received robot description ok!\n"); - } - } else - { - b3Warning("Robot description not received"); - } + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex,serverCmd); } break; } @@ -277,14 +393,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const return 0; } -void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - info = bodyJoints->m_jointInfo[jointIndex]; + if (jointIndex < bodyJoints->m_jointInfo.size()) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } } + return false; } ///todo: move this out of the @@ -333,9 +454,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; } } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index fb1aeefa6..aaa2d8639 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -19,6 +19,10 @@ protected: bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + bool processCamera(const struct SharedMemoryCommand& orgCommand); + + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + public: PhysicsDirect(); @@ -44,7 +48,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 985352113..ada37ce88 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -79,9 +79,9 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const return m_data->m_physicsClient->getNumJoints(bodyIndex); } -void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const { - m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); + return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); } ///todo: move this out of the diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index edc71ceb9..8c0e187b6 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -40,7 +40,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1fc4efdcc..6ead18363 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -55,12 +55,12 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw virtual void draw3dText(const btVector3& location,const char* textString) { } - + virtual void setDebugMode(int debugMode) { m_debugMode = debugMode; } - + virtual int getDebugMode() const { return m_debugMode; @@ -100,11 +100,11 @@ struct InternalBodyHandle : public InteralBodyData BT_DECLARE_ALIGNED_ALLOCATOR(); int m_nextFreeHandle; - void SetNextFree(int next) + void SetNextFree(int next) { m_nextFreeHandle = next; } - int GetNextFree() const + int GetNextFree() const { return m_nextFreeHandle; } @@ -153,7 +153,7 @@ public: struct CommandLogger { FILE* m_file; - + void writeHeader(unsigned char* buffer) const { @@ -185,15 +185,15 @@ struct CommandLogger buffer[9] = 0; buffer[10] = 0; buffer[11] = 0; - + int ver = btGetVersion(); if (ver>=0 && ver<999) { sprintf((char*)&buffer[9],"%d",ver); } - + } - + void logCommand(const SharedMemoryCommand& command) { btCommandChunk chunk; @@ -240,10 +240,10 @@ struct CommandLogPlayback } unsigned char c = m_header[7]; m_fileIs64bit = (c=='-'); - + const bool VOID_IS_8 = ((sizeof(void*)==8)); m_bitsVary = (VOID_IS_8 != m_fileIs64bit); - + } @@ -260,7 +260,7 @@ struct CommandLogPlayback if (m_file) { size_t s = 0; - + if (m_fileIs64bit) { @@ -271,7 +271,7 @@ struct CommandLogPlayback bCommandChunkPtr4 chunk4; s = fread((void*)&chunk4,sizeof(bCommandChunkPtr4),1,m_file); } - + if (s==1) { s = fread(cmd,sizeof(SharedMemoryCommand),1,m_file); @@ -324,7 +324,7 @@ struct PhysicsServerCommandProcessorInternalData { m_numUsedHandles = 0; m_firstFreeHandle = -1; - + increaseHandleCapacity(1); } @@ -349,10 +349,10 @@ struct PhysicsServerCommandProcessorInternalData int additionalCapacity= m_bodyHandles.size(); increaseHandleCapacity(additionalCapacity); - + getHandle(handle)->SetNextFree(m_firstFreeHandle); } - + return handle; } @@ -369,20 +369,19 @@ struct PhysicsServerCommandProcessorInternalData } ///end handle management - - + + CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; - + btScalar m_physicsDeltaTime; btAlignedObjectArray m_multiBodyJointFeedbacks; - + btAlignedObjectArray m_worldImporters; btAlignedObjectArray m_urdfLinkNameMapper; - btHashMap m_multiBodyJointMotorMap; btAlignedObjectArray m_strings; btAlignedObjectArray m_collisionShapes; @@ -392,16 +391,17 @@ struct PhysicsServerCommandProcessorInternalData btDefaultCollisionConfiguration* m_collisionConfiguration; btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; - - - + btAlignedObjectArray m_sdfRecentLoadedBodies; + + + struct GUIHelperInterface* m_guiHelper; int m_sharedMemoryKey; bool m_verboseOutput; - - + + //data for picking objects class btRigidBody* m_pickedBody; class btTypedConstraint* m_pickedConstraint; @@ -426,7 +426,7 @@ struct PhysicsServerCommandProcessorInternalData m_pickedConstraint(0), m_pickingMultiBodyPoint2Point(0) { - + initHandles(); #if 0 btAlignedObjectArray bla; @@ -478,16 +478,16 @@ struct PhysicsServerCommandProcessorInternalData void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper) { - + if (guiHelper) { - + guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld); } else { if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer()) { - + m_data->m_dynamicsWorld->setDebugDrawer(0); } } @@ -521,46 +521,45 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() ///collision configuration contains default setup for memory, collision setup m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); //m_collisionConfiguration->setConvexConvexMultipointIterations(); - + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); - + m_data->m_broadphase = new btDbvtBroadphase(); - + m_data->m_solver = new btMultiBodyConstraintSolver; - + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); - - + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); } void PhysicsServerCommandProcessor::deleteDynamicsWorld() { - + for (int i=0;im_multiBodyJointFeedbacks.size();i++) { delete m_data->m_multiBodyJointFeedbacks[i]; } m_data->m_multiBodyJointFeedbacks.clear(); - - + + for (int i=0;im_worldImporters.size();i++) { delete m_data->m_worldImporters[i]; } m_data->m_worldImporters.clear(); - + for (int i=0;im_urdfLinkNameMapper.size();i++) { delete m_data->m_urdfLinkNameMapper[i]; } m_data->m_urdfLinkNameMapper.clear(); - - m_data->m_multiBodyJointMotorMap.clear(); - + + for (int i=0;im_strings.size();i++) { delete m_data->m_strings[i]; @@ -569,11 +568,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() btAlignedObjectArray constraints; btAlignedObjectArray mbconstraints; - - + + if (m_data->m_dynamicsWorld) { - + int i; for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--) { @@ -587,7 +586,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() mbconstraints.push_back(mbconstraint); m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint); } - + for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) { btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; @@ -663,7 +662,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) for (int i=0;isetMaxAppliedImpulse(0); - m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); + mb->getLink(mbLinkIndex).m_userPtr = motor; m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); motor->finalizeMultiDof(); - + } } } +bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes) +{ + btAssert(m_data->m_dynamicsWorld); + if (!m_data->m_dynamicsWorld) + { + b3Error("loadSdf: No valid m_dynamicsWorld"); + return false; + } + + m_data->m_sdfRecentLoadedBodies.clear(); + + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); + + bool useFixedBase = false; + bool loadOk = u2b.loadSDF(fileName, useFixedBase); + if (loadOk) + { + for (int i=0;im_collisionShapes.push_back(shape); + if (shape->isCompound()) + { + btCompoundShape* compound = (btCompoundShape*) shape; + for (int childIndex=0;childIndexgetNumChildShapes();childIndex++) + { + m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex)); + } + } + + } + + btTransform rootTrans; + rootTrans.setIdentity(); + if (m_data->m_verboseOutput) + { + b3Printf("loaded %s OK!", fileName); + } + + for (int m =0; mallocHandle(); + + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + { + btScalar mass = 0; + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + btVector3 localInertiaDiagonal(0,0,0); + int urdfLinkIndex = u2b.getRootLinkIndex(); + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); + } + + + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_data->m_guiHelper); + + u2b.getRootTransformInWorld(rootTrans); + bool useMultiBody = true; + ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); + + + + mb = creation.getBulletMultiBody(); + if (mb) + { + bodyHandle->m_multiBody = mb; + + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); + + createJointMotors(mb); + + + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + + bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + + int urdfLinkIndex = creation.m_mb2urdfLink[i]; + btScalar mass; + btVector3 localInertiaDiagonal(0,0,0); + btTransform localInertialFrame; + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); + bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame); + + std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(linkName); + + mb->getLink(i).m_linkName = linkName->c_str(); + + std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(jointName); + + mb->getLink(i).m_jointName = jointName->c_str(); + } + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + m_data->m_strings.push_back(baseName); + mb->setBaseName(baseName->c_str()); + } else + { + b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); + } + + } + } + return loadOk; +} bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes) @@ -693,13 +809,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } - + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); - + bool loadOk = u2b.loadURDF(fileName, useFixedBase); - - + + if (loadOk) { //get a body index @@ -720,7 +836,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto { b3Printf("loaded %s OK!", fileName); } - + btTransform tr; tr.setIdentity(); tr.setOrigin(pos); @@ -728,11 +844,9 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto //int rootLinkIndex = u2b.getRootLinkIndex(); // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_data->m_guiHelper); - + ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); - - - ///todo(erwincoumans) refactor this memory allocation issue + for (int i=0;im_collisionShapes.push_back(compound->getChildShape(childIndex)); } } - + } - + btMultiBody* mb = creation.getBulletMultiBody(); if (useMultiBody) { - - + + if (mb) { + bodyHandle->m_multiBody = mb; + createJointMotors(mb); @@ -789,37 +905,38 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str()); mb->getLink(i).m_jointName = jointName->c_str(); } - + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_data->m_strings.push_back(baseName); - + util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str()); mb->setBaseName(baseName->c_str()); - - + + util->m_memSerializer->insertHeader(); - + int len = mb->calculateSerializeBufferSize(); btChunk* chunk = util->m_memSerializer->allocate(len,1); const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); - return true; + return true; } else { b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); return false; } + } else { btAssert(0); - + return true; } - + } - + return false; } @@ -827,10 +944,10 @@ void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, { if (m_data->m_logPlayback) { - + SharedMemoryCommand clientCmd; SharedMemoryStatus serverStatus; - + bool hasCommand = m_data->m_logPlayback->processNextCommand(&clientCmd); if (hasCommand) { @@ -839,7 +956,44 @@ void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, } } +int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes) +{ + int streamSizeInBytes = 0; + //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + btMultiBody* mb = bodyHandle->m_multiBody; + if (mb) + { + UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; + m_data->m_urdfLinkNameMapper.push_back(util); + util->m_mb = mb; + util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); + + bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); + util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName); + util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName); + } + + util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); + + util->m_memSerializer->insertHeader(); + + int len = mb->calculateSerializeBufferSize(); + btChunk* chunk = util->m_memSerializer->allocate(len,1); + const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); + util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); + streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); + + } + return streamSizeInBytes; +} bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) { @@ -848,12 +1002,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { ///we ignore overflow of integer for now - + { - + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands - - + + //const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; #if 1 if (m_data->m_commandLogger) @@ -863,7 +1017,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm #endif //m_data->m_testBlock1->m_numProcessedClientCommands++; - + //no timestamp yet int timeStamp = 0; @@ -877,11 +1031,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); } - + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - + if (completedOk) { SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); @@ -892,7 +1046,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); } - + break; } #endif @@ -900,7 +1054,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case CMD_REQUEST_DEBUG_LINES: { int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); - + int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; if (startingLineIndex<0) @@ -908,7 +1062,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("startingLineIndex should be non-negative"); startingLineIndex = 0; } - + if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) { m_data->m_remoteDebugDrawer->m_lines2.resize(0); @@ -929,9 +1083,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("m_startingLineIndex exceeds total number of debug lines"); startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); } - + int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); - + if (numLines) { @@ -954,57 +1108,66 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); } } - + serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); hasStatus = true; - + break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { - + int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; 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); - } + } else { m_data->m_visualConverter.getWidthAndHeight(width,height); } - - - + + + int numTotalPixels = width*height; int numRemainingPixels = numTotalPixels - startPixelIndex; - - + + if (numRemainingPixels>0) { int maxNumPixels = bufferSizeInBytes/8-1; unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); - + float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); - + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0) { m_data->m_guiHelper->copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied); } else { - + 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( @@ -1014,19 +1177,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_visualConverter.render(); } - + } - + m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied); } - + //each pixel takes 4 RGBA values and 1 float = 8 bytes - + } else { - + } - + serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; @@ -1034,13 +1197,47 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height; hasStatus = true; - + break; } + case CMD_REQUEST_BODY_INFO: + { + const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; + //stream info into memory + int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; + serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes; + hasStatus = true; + break; + } + case CMD_LOAD_SDF: + { + const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); + } + + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); + + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + hasStatus = true; + break; + } case CMD_LOAD_URDF: { - + const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; if (m_data->m_verboseOutput) { @@ -1070,32 +1267,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, initialPos,initialOrn, useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - - + + if (completedOk) { - + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - + serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0; - + if (m_data->m_urdfLinkNameMapper.size()) { serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; hasStatus = true; - + } else { serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; hasStatus = true; } - - - + + + break; } case CMD_CREATE_SENSOR: @@ -1125,7 +1322,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->getLink(jointIndex).m_jointFeedback = fb; m_data->m_multiBodyJointFeedbacks.push_back(fb); }; - + } else { if (mb->getLink(jointIndex).m_jointFeedback) @@ -1140,7 +1337,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - + } else { b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); @@ -1155,15 +1352,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btJointFeedback* fb = new btJointFeedback(); m_data->m_jointFeedbacks.push_back(fb); c->setJointFeedback(fb); - - + + } */ #endif - + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; - + break; } case CMD_SEND_DESIRED_STATE: @@ -1172,7 +1369,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_SEND_DESIRED_STATE"); } - + int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); @@ -1180,7 +1377,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btMultiBody* mb = body->m_multiBody; btAssert(mb); - + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { case CONTROL_MODE_TORQUE: @@ -1189,26 +1386,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Using CONTROL_MODE_TORQUE"); } - mb->clearForcesAndTorques(); - + // 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]); - torqueIndex+=6; - mb->addBaseForce(f); - mb->addBaseTorque(t); - for (int link=0;linkgetNumLinks();link++) + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) { - - for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; - mb->addJointTorqueMultiDof(link,dof,torque); - torqueIndex++; + for (int link=0;linkgetNumLinks();link++) + { + + for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + double torque = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + mb->addJointTorqueMultiDof(link,dof,torque); + } + torqueIndex++; + } } } break; @@ -1219,7 +1413,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Using CONTROL_MODE_VELOCITY"); } - + int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque { @@ -1228,16 +1422,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (supportsJointMotor(mb,link)) { - - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; - if (motorPtr) - { - btMultiBodyJointMotor* motor = *motorPtr; - btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; - motor->setVelocityTarget(desiredVelocity); - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(maxImp); + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + + if (motor) + { + btScalar desiredVelocity = 0.f; + bool hasDesiredVelocity = false; + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + btScalar kd = 0.1f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; + } + + motor->setVelocityTarget(desiredVelocity,kd); + + btScalar kp = 0.f; + motor->setPositionTarget(0,kp); + hasDesiredVelocity = true; + } + if (hasDesiredVelocity) + { + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + } + motor->setMaxAppliedImpulse(maxImp); + } numMotors++; } @@ -1247,6 +1464,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } break; } + case CONTROL_MODE_POSITION_VELOCITY_PD: { if (m_data->m_verboseOutput) @@ -1254,7 +1472,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); } //compute the force base on PD control - mb->clearForcesAndTorques(); int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque @@ -1265,32 +1482,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (supportsJointMotor(mb,link)) { - - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; - if (motorPtr) - { - 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]; - int dof1 = 0; - btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; - btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; - btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; - btScalar velocityError = (desiredVelocity - currentVelocity); - - desiredVelocity = kp * positionStabiliationTerm + - kd * velocityError; - - motor->setVelocityTarget(desiredVelocity); - - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(1000);//maxImp); + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + if (motor) + { + + bool hasDesiredPosOrVel = false; + btScalar kp = 0.f; + btScalar kd = 0.f; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + kd = 0.1; + } + btScalar desiredPosition = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + kp = 0.1; + } + + if (hasDesiredPosOrVel) + { + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + } + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + } + + motor->setVelocityTarget(desiredVelocity,kd); + motor->setPositionTarget(desiredPosition,kp); + + 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(maxImp); + } numMotors++; } @@ -1307,10 +1546,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("m_controlMode not implemented yet"); break; } - + } } - + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; hasStatus = true; break; @@ -1323,7 +1562,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); - + if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; @@ -1332,9 +1571,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - + int totalDegreeOfFreedomU = 0; + + //always add the base, even for static (non-moving objects) //so that we can easily move the 'fixed' base when needed //do we don't use this conditional "if (!mb->hasFixedBase())" @@ -1342,7 +1581,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btTransform tr; tr.setOrigin(mb->getBasePos()); tr.setRotation(mb->getWorldToBaseRot().inverse()); - + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = @@ -1360,14 +1599,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm body->m_rootLocalInertialFrame.getRotation()[3]; - + //base position in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; @@ -1377,7 +1616,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; - + //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; @@ -1394,7 +1633,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } - + if (0 == mb->getLink(l).m_jointFeedback) { for (int d=0;d<6;d++) @@ -1405,25 +1644,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; } - + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; - + if (supportsJointMotor(mb,l)) { - - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[l]; - if (motorPtr && m_data->m_physicsDeltaTime>btScalar(0)) + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; + + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) { - btMultiBodyJointMotor* motor = *motorPtr; btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = force; @@ -1435,10 +1674,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); - + btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); - + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ(); @@ -1446,24 +1685,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w(); - + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ(); - + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w(); - + } - + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - + hasStatus = true; - + } else { if (body && body->m_rigidBody) @@ -1483,7 +1722,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; @@ -1493,7 +1732,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - + //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; @@ -1517,7 +1756,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_STEP_FORWARD_SIMULATION: { - + if (m_data->m_verboseOutput) { b3Printf("Step simulation request"); @@ -1528,13 +1767,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm applyJointDamping(i); } m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; hasStatus = true; break; } - + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) @@ -1553,12 +1792,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; - + }; case CMD_INIT_POSE: { @@ -1575,6 +1814,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) { btVector3 zero(0,0,0); + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); + mb->setBaseVel(zero); mb->setBasePos(btVector3( clientCmd.m_initPoseArgs.m_initialStateQ[0], @@ -1583,6 +1826,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) { + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); + mb->setBaseOmega(btVector3(0,0,0)); mb->setWorldToBaseRot(btQuaternion( clientCmd.m_initPoseArgs.m_initialStateQ[3], @@ -1595,9 +1843,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int dofIndex = 7; for (int i=0;igetNumLinks();i++) { - if (mb->getLink(i).m_dofCount==1) + if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1)) { - mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]); mb->setJointVel(i,0); } @@ -1605,14 +1852,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; } - + case CMD_RESET_SIMULATION: { @@ -1629,7 +1876,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm createEmptyDynamicsWorld(); m_data->exitHandles(); m_data->initHandles(); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; hasStatus = true; @@ -1659,7 +1906,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) { - + startTrans.setRotation(btQuaternion( clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], @@ -1684,7 +1931,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm m_data->m_worldImporters.push_back(worldImporter); btCollisionShape* shape = 0; - + switch (shapeType) { case COLLISION_SHAPE_TYPE_CYLINDER_X: @@ -1741,8 +1988,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm shape = worldImporter->createBoxShape(halfExtents); } } - - + + bool isDynamic = (mass>0); btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); rb->setRollingFriction(0.2); @@ -1757,11 +2004,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); - - + + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; - + int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); @@ -1780,7 +2027,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_pickBodyArguments.m_rayToWorld[1], clientCmd.m_pickBodyArguments.m_rayToWorld[2])); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; @@ -1809,10 +2056,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_APPLY_EXTERNAL_FORCE: + { + for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) + { + InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0); + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i*3+0], + clientCmd.m_externalForceArguments.m_positions[i*3+1], + clientCmd.m_externalForceArguments.m_positions[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal; + mb->addBaseForce(forceWorld); + mb->addBaseTorque(relPosWorld.cross(forceWorld)); + //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal; + btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal; + mb->addLinkForce(link, forceWorld); + mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); + //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); + } + } + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; + mb->addBaseTorque(torqueWorld); + //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal; + mb->addLinkTorque(link, torqueWorld); + //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + } default: { b3Error("Unknown command encountered"); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; hasStatus = true; @@ -1820,7 +2131,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } }; - + } } return hasStatus; @@ -1832,10 +2143,10 @@ void PhysicsServerCommandProcessor::renderScene() if (m_data->m_guiHelper) { m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); - + m_data->m_guiHelper->render(m_data->m_dynamicsWorld); } - + } void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) @@ -1886,7 +2197,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); if (multiCol && multiCol->m_multiBody) { - + m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); multiCol->m_multiBody->setCanSleep(false); @@ -1899,10 +2210,10 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) btScalar scaling=1; p2p->setMaxAppliedImpulse(2*scaling); - + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(p2p); - m_data->m_pickingMultiBodyPoint2Point =p2p; + m_data->m_pickingMultiBodyPoint2Point =p2p; } } @@ -1927,7 +2238,7 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld if (pickCon) { //keep it at the same picking distance - + btVector3 dir = rayToWorld-rayFromWorld; dir.normalize(); dir *= m_data->m_oldPickingDist; @@ -1936,21 +2247,21 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld pickCon->setPivotB(newPivotB); } } - + if (m_data->m_pickingMultiBodyPoint2Point) { //keep it at the same picking distance - + btVector3 dir = rayToWorld-rayFromWorld; dir.normalize(); dir *= m_data->m_oldPickingDist; btVector3 newPivotB = rayFromWorld + dir; - + m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); } - + return false; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index dca5f8930..bc74a0a27 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -19,10 +19,16 @@ class PhysicsServerCommandProcessor protected: + + + bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes); + bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); bool supportsJointMotor(class btMultiBody* body, int linkIndex); + + int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes); public: PhysicsServerCommandProcessor(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 42436d2b0..3ddb11e91 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -30,8 +30,10 @@ #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 +#define MAX_SDF_BODIES 1024 struct TmpFloat3 { @@ -54,6 +56,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, @@ -103,6 +115,7 @@ enum EnumInitPoseFlags struct InitPoseArgs { int m_bodyUniqueId; + int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; }; @@ -118,12 +131,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, + }; @@ -163,11 +180,14 @@ struct SendDesiredStateArgs double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link + int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM]; + //desired state is only written by the client, read-only access by server is expected //m_desiredStateQ is indexed by position variables, //starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; + //m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; @@ -179,6 +199,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 { @@ -274,6 +303,49 @@ struct CreateBoxShapeArgs double m_colorRGBA[4]; }; +struct SdfLoadedArgs +{ + int m_numBodies; + int m_bodyUniqueIds[MAX_SDF_BODIES]; + + ///@todo(erwincoumans) load cameras, lights etc + //int m_numCameras; + //int m_numLights; +}; + + +struct SdfRequestInfoArgs +{ + int m_bodyUniqueId; +}; + +///flags for b3ApplyExternalTorque and b3ApplyExternalForce +enum EnumExternalForcePrivateFlags +{ +// EF_LINK_FRAME=1, +// EF_WORLD_FRAME=2, + EF_TORQUE=4, + EF_FORCE=8, +}; + + + +struct ExternalForceArgs +{ + int m_numForcesAndTorques; + int m_bodyUniqueIds[MAX_SDF_BODIES]; + int m_linkIds[MAX_SDF_BODIES]; + double m_forcesAndTorques[3*MAX_SDF_BODIES]; + double m_positions[3*MAX_SDF_BODIES]; + int m_forceFlags[MAX_SDF_BODIES]; +}; + +enum EnumSdfRequestInfoFlags +{ + SDF_REQUEST_INFO_BODY=1, + //SDF_REQUEST_INFO_CAMERA=2, +}; + struct SharedMemoryCommand { int m_type; @@ -287,6 +359,8 @@ struct SharedMemoryCommand union { struct UrdfArgs m_urdfArguments; + struct SdfArgs m_sdfArguments; + struct SdfRequestInfoArgs m_sdfRequestInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; @@ -297,6 +371,7 @@ struct SharedMemoryCommand struct RequestDebugLinesArgs m_requestDebugLinesArguments; struct RequestPixelDataArgs m_requestPixelDataArguments; struct PickBodyArgs m_pickBodyArguments; + struct ExternalForceArgs m_externalForceArguments; }; }; @@ -315,6 +390,7 @@ struct SharedMemoryStatus union { struct BulletDataStreamArgs m_dataStreamArguments; + struct SdfLoadedArgs m_sdfLoadedArgs; struct SendActualStateArgs m_sendActualStateArgs; struct SendDebugLinesArgs m_sendDebugLinesArgs; struct SendPixelDataArgs m_sendPixelDataArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f7d04f724..3aeeb8f11 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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, @@ -18,12 +19,14 @@ enum EnumSharedMemoryClientCommand CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_DEBUG_LINES, + CMD_REQUEST_BODY_INFO, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, CMD_PICK_BODY, CMD_MOVE_PICKED_BODY, CMD_REMOVE_PICKING_CONSTRAINT_BODY, CMD_REQUEST_CAMERA_IMAGE_DATA, + CMD_APPLY_EXTERNAL_FORCE, CMD_MAX_CLIENT_COMMANDS }; @@ -35,7 +38,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, @@ -52,6 +56,8 @@ enum EnumSharedMemoryServerStatus CMD_RESET_SIMULATION_COMPLETED, CMD_CAMERA_IMAGE_COMPLETED, CMD_CAMERA_IMAGE_FAILED, + CMD_BODY_INFO_COMPLETED, + CMD_BODY_INFO_FAILED, CMD_INVALID_STATUS, CMD_MAX_SERVER_COMMANDS }; @@ -138,5 +144,11 @@ enum { CONTROL_MODE_POSITION_VELOCITY_PD, }; +///flags for b3ApplyExternalTorque and b3ApplyExternalForce +enum EnumExternalForceFlags +{ + EF_LINK_FRAME=1, + EF_WORLD_FRAME=2, +}; #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index bf0a601d6..a4ceb2d45 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -28,7 +28,7 @@ subject to the following restrictions: #include "../Utils/b3ResourcePath.h" #include "../TinyRenderer/TinyRenderer.h" #include "../OpenGLWindow/SimpleCamera.h" - +#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h" #include #include #include "../Importers/ImportURDFDemo/UrdfParser.h" @@ -41,6 +41,13 @@ enum MyFileType MY_FILE_OBJ=3, }; +struct MyTexture2 +{ + unsigned char* textureData; + int m_width; + int m_height; +}; + struct TinyRendererObjectArray { btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects; @@ -95,7 +102,7 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter() -void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) +void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) { @@ -201,7 +208,23 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref { case MY_FILE_OBJ: { - glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) + { + + if (meshData.m_textureImage) + { + MyTexture2 texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; + } + + break; } @@ -420,11 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj) { - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - int graphicsIndex = -1; - + UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); if (linkPtr) { @@ -433,6 +452,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const for (int v = 0; v < link->m_visualArray.size();v++) { + btAlignedObjectArray textures; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + int graphicsIndex = -1; + const UrdfVisual& vis = link->m_visualArray[v]; btTransform childTrans = vis.m_linkLocalFrame; btHashString matName(vis.m_materialName.c_str()); @@ -458,14 +483,29 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const btAssert(visualsPtr); TinyRendererObjectArray* visuals = *visualsPtr; - convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices); + convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); 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); - tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor); + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer); + unsigned char* textureImage=0; + int textureWidth=0; + int textureHeight=0; + if (textures.size()) + { + textureImage = textures[0].textureData; + textureWidth = textures[0].m_width; + textureHeight = textures[0].m_height; + } + + tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor, + textureImage,textureWidth,textureHeight); visuals->m_renderObjects.push_back(tinyObj); } + for (int i=0;im_swRenderInstances.size()); + // printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); for (int i=0;im_swRenderInstances.size();i++) { TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i); @@ -580,7 +620,9 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo } } //printf("write tga \n"); - m_data->m_rgbColorBuffer.write_tga_file("camera.tga"); + //m_data->m_rgbColorBuffer.write_tga_file("camera.tga"); +// printf("flipped!\n"); + m_data->m_rgbColorBuffer.flip_vertically(); } @@ -590,6 +632,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 +665,10 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels { for (int i=0;im_depthBuffer[i+startPixelIndex]; + } if (pixelsRGBA) { pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0]; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 3bce1add7..6baff2491 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -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); diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 0616a8865..34aea2de5 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -26,6 +26,8 @@ files { "PhysicsServer.h", "main.cpp", "PhysicsClientC_API.cpp", + "SharedMemoryCommands.h", + "SharedMemoryPublic.h", "PhysicsServer.cpp", "PosixSharedMemory.cpp", "Win32SharedMemory.cpp", @@ -78,6 +80,7 @@ files { "../ThirdPartyLibs/tinyxml/tinyxml.cpp", "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", - +"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } diff --git a/examples/SimpleOpenGL3/main.cpp b/examples/SimpleOpenGL3/main.cpp index 015db129e..726abf693 100644 --- a/examples/SimpleOpenGL3/main.cpp +++ b/examples/SimpleOpenGL3/main.cpp @@ -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); diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp index 69c3b7db5..1fa1b0ccd 100644 --- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp @@ -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); } diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp index 77ab903ab..565b771e7 100644 --- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp @@ -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; diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 77c387176..aebf0440e 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -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&depthBuffer) -:m_width(width), -m_height(height), -m_rgbColorBuffer(rgbColorBuffer), +TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&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& 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; infaces(); i++) { diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 0cb2ffca9..c6f4f9821 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -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& m_depthBuffer; - TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer); + TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer); virtual ~TinyRenderObjectData(); void loadModel(const char* fileName); diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp index 9995b8f66..32f5e7edd 100644 --- a/examples/TinyRenderer/main.cpp +++ b/examples/TinyRenderer/main.cpp @@ -86,7 +86,7 @@ int main(int argc, char* argv[]) b3AlignedObjectArray 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"); diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 626cc2331..f7a0bea75 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -57,9 +57,9 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid for (int j=0;j #endif + +#if PY_MAJOR_VERSION >= 3 +#define PyInt_FromLong PyLong_FromLong +#define PyString_FromString PyBytes_FromString +#endif + enum eCONNECT_METHOD { eCONNECT_GUI=1, @@ -16,10 +23,12 @@ enum eCONNECT_METHOD eCONNECT_SHARED_MEMORY=3, }; + + static PyObject *SpamError; static b3PhysicsClientHandle sm=0; - +// Step through one timestep of the simulation static PyObject * pybullet_stepSimulation(PyObject *self, PyObject *args) { @@ -118,7 +127,11 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) return Py_None; } - +// Load a URDF file indicating the links and joints of an object +// function can be called without arguments and will default +// to position (0,0,1) with orientation(0,0,0,1) +// els(x,y,z) or +// loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) static PyObject * pybullet_loadURDF(PyObject* self, PyObject* args) { @@ -127,14 +140,15 @@ pybullet_loadURDF(PyObject* self, PyObject* args) int bodyIndex = -1; const char* urdfFileName=""; + float startPosX =0; - float startPosY =0; - float startPosZ = 1; + float startPosY =0; + float startPosZ = 0; float startOrnX = 0; float startOrnY = 0; float startOrnZ = 0; float startOrnW = 1; - printf("size=%d\n", size); + if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -157,16 +171,17 @@ pybullet_loadURDF(PyObject* self, PyObject* args) &startPosX,&startPosY,&startPosZ, &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) return NULL; - } { + // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); - printf("urdf filename = %s\n", urdfFileName); + //setting the initial position, orientation and other arguments are optional b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); + b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,startOrnZ, startOrnW ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType!=CMD_URDF_LOADING_COMPLETED) @@ -178,6 +193,82 @@ pybullet_loadURDF(PyObject* self, PyObject* args) } return PyLong_FromLong(bodyIndex); } + +static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index) +{ + float v = 0.f; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + else + { + item = PyTuple_GET_ITEM(seq,index); + v = PyFloat_AsDouble(item); + } + return v; +} + + +#define MAX_SDF_BODIES 512 + +static PyObject* +pybullet_loadSDF(PyObject* self, PyObject* args) +{ + const char* sdfFileName=""; + int size= PySequence_Size(args); + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist=0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle; + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (size==1) + { + if (!PyArg_ParseTuple(args, "s", &sdfFileName)) + return NULL; + } + + commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType!=CMD_SDF_LOADING_COMPLETED) + { + PyErr_SetString(SpamError, "Cannot load SDF file."); + return NULL; + } + + numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } + + pylist = PyTuple_New(numBodies); + + if (numBodies >0 && numBodies <= MAX_SDF_BODIES) + { + for (i=0;i= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } + + commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); + + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); + double kd = gains; + b3JointControlSetKd(commandHandle,info.m_uIndex,kd); + b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + break; + } + + case CONTROL_MODE_TORQUE: + { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); + break; + } + + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue); + double kp = gains; + b3JointControlSetKp(commandHandle,info.m_uIndex,kp); + b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + break; + } + default: + { + } + }; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + Py_INCREF(Py_None); + return Py_None; + } + PyErr_SetString(SpamError, "error in setJointControl."); + return NULL; +} + + +// Set the gravity of the world with (x, y, z) arguments static PyObject * pybullet_setGravity(PyObject* self, PyObject* args) { @@ -228,7 +438,40 @@ pybullet_setGravity(PyObject* self, PyObject* args) return Py_None; } +static PyObject * +pybullet_setTimeStep(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double timeStep=0.001; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "d", &timeStep)) + { + PyErr_SetString(SpamError, "setTimeStep expected a single value (double)."); + return NULL; + } + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + + +// 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]) { basePosition[0] = 0.; @@ -242,7 +485,6 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double { - { b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); @@ -252,12 +494,18 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double const int status_type = b3GetStatusType(status_handle); const double* actualStateQ; + // const double* jointReactionForces[]; + int i; b3GetStatusActualState(status_handle, 0/* body_unique_id */, 0/* num_degree_of_freedom_q */, 0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/, &actualStateQ , 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } //now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] //and orientation x,y,z,w = actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] basePosition[0] = actualStateQ[0]; @@ -268,31 +516,38 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double baseOrientation[1] = actualStateQ[4]; baseOrientation[2] = actualStateQ[5]; baseOrientation[3] = actualStateQ[6]; + } } } +// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion +// values for the base link of your object +// Object is retrieved based on body index, which is the order +// the object was loaded into the simulation (0-based) static PyObject * pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) { + int bodyIndex = -1; + double basePosition[3]; + double baseOrientation[4]; + PyObject *pylistPos; + PyObject *pylistOrientation; + if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - int bodyIndex = -1; if (!PyArg_ParseTuple(args, "i", &bodyIndex )) { PyErr_SetString(SpamError, "Expected a body index (integer)."); return NULL; } - - double basePosition[3]; - double baseOrientation[4]; - + pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation); - PyObject *pylistPos; + { PyObject *item; @@ -306,7 +561,7 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) } } - PyObject *pylistOrientation; + { PyObject *item; @@ -331,7 +586,9 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) } - +// Return the number of joints in an object based on +// body index; body index is based on order of sequence +// the object is loaded into simulation static PyObject * pybullet_getNumJoints(PyObject* self, PyObject* args) { @@ -359,35 +616,787 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) } } +// Initalize all joint positions given a list of values +static PyObject* +pybullet_resetJointState(PyObject* self, PyObject* args) +{ + int size; + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + size= PySequence_Size(args); + + if (size==3) + { + int bodyIndex; + int jointIndex; + double targetValue; + + if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; + + numJoints = b3GetNumJoints(sm,bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} + +// Reset the position and orientation of the base/root link, position [x,y,z] and orientation quaternion [x,y,z,w] +static PyObject* +pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) +{ + int size; + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + size= PySequence_Size(args); + + if (size==3) + { + int bodyIndex; + PyObject* posObj; + PyObject* ornObj; + double pos[3]; + double orn[4];//as a quaternion + + if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len,i; + seq = PySequence_Fast(posObj, "expected a sequence"); + len = PySequence_Size(posObj); + if (len==3) + { + for (i = 0; i < 3; i++) + { + pos[i] = pybullet_internalGetFloatFromSequence(seq,i); + } + } else + { + PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + { + PyObject* seq; + int len,i; + seq = PySequence_Fast(ornObj, "expected a sequence"); + len = PySequence_Size(ornObj); + if (len==4) + { + for (i = 0; i < 4; i++) + { + orn[i] = pybullet_internalGetFloatFromSequence(seq,i); + } + } else + { + PyErr_SetString(SpamError, "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]); + b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} + + +// Get the a single joint info for a specific bodyIndex +// +// Args: +// bodyIndex - integer indicating body in simulation +// jointIndex - integer indicating joint for a specific body +// +// Joint information includes: +// index, name, type, q-index, u-index, +// flags, joint damping, joint friction +// +// The format of the returned list is +// [int, str, int, int, int, int, float, float] +// +// TODO(hellojas): get joint positions for a body +static PyObject* +pybullet_getJointInfo(PyObject* self, PyObject* args) +{ + PyObject *pyListJointInfo; + + struct b3JointInfo info; + + int bodyIndex = -1; + int jointIndex = -1; + int jointInfoSize = 8; //size of struct b3JointInfo + + int size= PySequence_Size(args); + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + + if (size==2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) + { + + // 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); + + // 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; + } + } + + Py_INCREF(Py_None); + return Py_None; +} + + +// Returns the state of a specific joint in a given bodyIndex +// +// Args: +// bodyIndex - integer indicating body in simulation +// jointIndex - integer indicating joint for a specific body +// +// The state of a joint includes the following: +// position, velocity, force torque (6 values), and motor torque +// The returned pylist is an array of [float, float, float[6], float] + +// TODO(hellojas): check accuracy of position and velocity +// TODO(hellojas): check force torque values + +static PyObject* +pybullet_getJointState(PyObject* self, PyObject* args) +{ + PyObject *pyListJointForceTorque; + PyObject *pyListJointState; + PyObject *item; + + struct b3JointInfo info; + struct b3JointSensorState sensorState; + + int bodyIndex = -1; + int jointIndex = -1; + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int i, j; + + + int size= PySequence_Size(args); + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (size==2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) + { + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); + + + b3GetJointState(sm, status_handle, jointIndex, &sensorState); + + PyTuple_SetItem(pyListJointState, 0, + PyFloat_FromDouble(sensorState.m_jointPosition)); + PyTuple_SetItem(pyListJointState, 1, + PyFloat_FromDouble(sensorState.m_jointVelocity)); + + for (j = 0; j < forceTorqueSize; j++) { + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + } + + PyTuple_SetItem(pyListJointState, 2, + pyListJointForceTorque); + + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + + return pyListJointState; + } + } else + { + PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index)."); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} + + + +// internal function to set a float matrix[16] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// matrix - float[16] which will be set by values from objMat +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) +{ + int i, len; + PyObject* seq; + + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len==16) + { + for (i = 0; i < len; i++) + { + matrix[i] = pybullet_internalGetFloatFromSequence(seq,i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + return 0; +} + +// Render an image from the current timestep of the simulation +// +// Examples: +// renderImage() - default image resolution and camera position +// renderImage(w, h) - image resolution of (w,h), default camera +// renderImage(w, h, view[16], projection[16]) - set both resolution +// and initialize camera to the view and projection values +// +// Note if the (w,h) is too small, the objects may not appear based on +// where the camera has been set +// +// TODO(hellojas): fix image is cut off at head +// TODO(hellojas): should we add check to give minimum image resolution +// to see object based on camera position? +static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) +{ + ///request an image from a simulated camera, using a software renderer. + struct b3CameraImageData imageData; + PyObject* objViewMat,* objProjMat; + int width, height; + int size= PySequence_Size(args); + float viewMatrix[16]; + float projectionMatrix[16]; + + // inialize cmd + b3SharedMemoryCommandHandle command; + + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + command = b3InitRequestCameraImage(sm); + + if (size==2) // only set camera resolution + { + if (PyArg_ParseTuple(args, "ii", &width, &height)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + } + } + + if (size==4) // set camera resoluation and view and projection matrix + { + if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + + // set camera matrices only if set matrix function succeeds + if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && + (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) + { + b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); + } + } + } + + 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 + PyObject *pylistRGB; + PyObject* pylistDep; + int i, j, p; + + b3GetCameraImageData(sm, &imageData); + //TODO(hellojas): 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 *item; + int bytesPerPixel = 4;//Red, Green, Blue, and Alpha each 8 bit values + int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; + pylistRGB = PyTuple_New(num); + pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); + + for (i=0;i euler yaw/pitch/roll convention from URDF/SDF, see Gazebo +///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc +static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) +{ + + double squ; + double sqx; + double sqy; + double sqz; + + double quat[4]; + + PyObject* quatObj; + + if (PyArg_ParseTuple(args, "O", &quatObj)) + { + PyObject* seq; + int len,i; + seq = PySequence_Fast(quatObj, "expected a sequence"); + len = PySequence_Size(quatObj); + if (len==4) + { + for (i = 0; i < 4; i++) + { + quat[i] = pybullet_internalGetFloatFromSequence(seq,i); + } + } else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + return NULL; + } + + { + double rpy[3]; + sqx = quat[0] * quat[0]; + sqy = quat[1] * quat[1]; + sqz = quat[2] * quat[2]; + squ = quat[3] * quat[3]; + rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz); + double sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]); + rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg)); + rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz); + { + PyObject *pylist; + int i; + pylist = PyTuple_New(3); + for (i=0;i<3;i++) + PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i])); + return pylist; + } + } + Py_INCREF(Py_None); + return Py_None; +} + static PyMethodDef SpamMethods[] = { - {"loadURDF", pybullet_loadURDF, METH_VARARGS, - "Create a multibody by loading a URDF file."}, - - {"connect", pybullet_connectPhysicsServer, METH_VARARGS, - "Connect to an existing physics server (using shared memory by default)."}, - - {"disconnect", pybullet_disconnectPhysicsServer, METH_VARARGS, - "Disconnect from the physics server."}, + + {"connect", pybullet_connectPhysicsServer, METH_VARARGS, + "Connect to an existing physics server (using shared memory by default)."}, + + {"disconnect", pybullet_disconnectPhysicsServer, METH_VARARGS, + "Disconnect from the physics server."}, + + {"resetSimulation", pybullet_resetSimulation, METH_VARARGS, + "Reset the simulation: remove all objects and start from an empty world."}, - {"resetSimulation", pybullet_resetSimulation, METH_VARARGS, - "Reset the simulation: remove all objects and start from an empty world."}, - {"stepSimulation", pybullet_stepSimulation, METH_VARARGS, "Step the simulation using forward dynamics."}, {"setGravity", pybullet_setGravity, METH_VARARGS, "Set the gravity acceleration (x,y,z)."}, - + + {"setTimeStep", pybullet_setTimeStep, METH_VARARGS, + "Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"}, + + {"loadURDF", pybullet_loadURDF, METH_VARARGS, + "Create a multibody by loading a URDF file."}, + + {"loadSDF", pybullet_loadSDF, METH_VARARGS, + "Load multibodies from an SDF file."}, + {"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."}, - - {"getNumsetGravity", pybullet_setGravity, METH_VARARGS, - "Set the gravity acceleration (x,y,z)."}, - { - "getNumJoints", pybullet_getNumJoints, METH_VARARGS, + + {"resetBasePositionAndOrientation", pybullet_resetBasePositionAndOrientation, METH_VARARGS, + "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, + + {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."}, - + + {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, + "Get the name and type info for a joint on a body."}, + + {"getJointState", pybullet_getJointState, METH_VARARGS, + "Get the state (position, velocity etc) for a joint on a body."}, + + {"resetJointState", pybullet_resetJointState, METH_VARARGS, + "Reset the state (position, velocity etc) for a joint on a body instantaneously, not through physics simulation."}, + + {"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS, + "Set a single joint motor control mode and desired target value. There is no immediate state change, stepSimulation will process the motors."}, + + {"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS, + "for objectUniqueId, linkIndex (-1 for base/root link), apply a force [x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or FORCE_IN_WORLD_FRAME coordinates"}, + + {"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS, + "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque [x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or TORQUE_IN_WORLD_FRAME coordinates"}, + + {"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"}, + + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, + "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"}, + + {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS, + "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"}, + + //todo(erwincoumans) + //saveSnapshot + //loadSnapshot + + ////todo(erwincoumans) + //collision info + //raycast info + {NULL, NULL, 0, NULL} /* Sentinel */ }; @@ -395,7 +1404,7 @@ static PyMethodDef SpamMethods[] = { static struct PyModuleDef moduledef = { PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ - "Python bindings for Bullet", /* m_doc */ + "Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)", /* m_doc */ -1, /* m_size */ SpamMethods, /* m_methods */ NULL, /* m_reload */ @@ -433,7 +1442,14 @@ initpybullet(void) PyModule_AddIntConstant (m, "SHARED_MEMORY", eCONNECT_SHARED_MEMORY); // user read PyModule_AddIntConstant (m, "DIRECT", eCONNECT_DIRECT); // user read PyModule_AddIntConstant (m, "GUI", eCONNECT_GUI); // user read - + + PyModule_AddIntConstant (m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); + PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read + PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read + + PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME); + PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); PyModule_AddObject(m, "error", SpamError); diff --git a/src/BulletCollision/CollisionShapes/btConeShape.h b/src/BulletCollision/CollisionShapes/btConeShape.h index 4a0df0d55..46d78d148 100644 --- a/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/src/BulletCollision/CollisionShapes/btConeShape.h @@ -43,6 +43,15 @@ public: btScalar getRadius() const { return m_radius;} btScalar getHeight() const { return m_height;} + void setRadius(const btScalar radius) + { + m_radius = radius; + } + void setHeight(const btScalar height) + { + m_height = height; + } + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const { diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index b55db0c1b..8a2a2d1ae 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -145,11 +145,6 @@ public: // lo and hi limits for variables (set to -/+ infinity on entry). btScalar *m_lowerLimit,*m_upperLimit; - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; // number of solver iterations int m_numIterations; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 50268611b..3d0f45b77 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -23,7 +23,10 @@ subject to the following restrictions: btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity) + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(0.1), + m_kp(0) { m_maxAppliedImpulse = maxMotorImpulse; @@ -51,7 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof() btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity) + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(0.1), + m_kp(0) { btAssert(linkDoF < body->getLink(link).m_dofCount); @@ -113,9 +119,22 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + btScalar velocityError = (m_desiredVelocity - currentVelocity); + btScalar rhs = m_kp * positionStabiliationTerm + m_kd * velocityError; + printf("m_kd = %f\n", m_kd); + printf("m_kp = %f\n", m_kp); + printf("m_desiredVelocity = %f\n", m_desiredVelocity); + printf("m_desiredPosition = %f\n", m_desiredPosition); + printf("m_maxAppliedImpulse = %f\n", m_maxAppliedImpulse); + printf("rhs = %f\n", rhs); + + + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index d322f87e7..e433515d4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -25,8 +25,11 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint { protected: - btScalar m_desiredVelocity; + btScalar m_desiredPosition; + btScalar m_kd; + btScalar m_kp; + public: @@ -42,11 +45,19 @@ public: btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal); - virtual void setVelocityTarget(btScalar velTarget) + virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 0.1f) { m_desiredVelocity = velTarget; + m_kd = kd; } + virtual void setPositionTarget(btScalar posTarget, btScalar kp = 0.1f) + { + m_desiredPosition = posTarget; + m_kp = kp; + } + + virtual void debugDraw(class btIDebugDraw* drawer) { //todo(erwincoumans) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index b6a4a4a03..8b9036303 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -132,6 +132,8 @@ btVector3 m_appliedConstraintForce; // In WORLD frame const char* m_linkName;//m_linkName memory needs to be managed by the developer/user! const char* m_jointName;//m_jointName memory needs to be managed by the developer/user! + const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user! + btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping. btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor. @@ -149,6 +151,7 @@ btVector3 m_appliedConstraintForce; // In WORLD frame m_jointFeedback(0), m_linkName(0), m_jointName(0), + m_userPtr(0), m_jointDamping(0), m_jointFriction(0) { diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index c77ba3cba..d81647b68 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -48,9 +48,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } class vecx : public btVectorX { public: - vecx(int size) : btVectorX(size) {} + vecx(int size) : btVectorX(size) {} const vecx& operator=(const btVectorX& rhs) { - *static_cast(this) = rhs; + *static_cast*>(this) = rhs; return *this; } diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 41dea6948..963c5db97 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -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] && diff --git a/test/InverseDynamics/premake4.lua b/test/InverseDynamics/premake4.lua index 498cf94d8..9d6770347 100644 --- a/test/InverseDynamics/premake4.lua +++ b/test/InverseDynamics/premake4.lua @@ -78,6 +78,8 @@ "../../examples/Importers/ImportURDFDemo/UrdfParser.h", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", "../../examples/Utils/b3Clock.cpp", "../../Extras/Serialize/BulletWorldImporter/*", "../../Extras/Serialize/BulletFileLoader/*", diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index 868841237..d39f36df4 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -71,7 +71,8 @@ ENDIF() ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp ) diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index ef70b0139..952b4c98a 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -91,7 +91,9 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", - } + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + } project ("Test_PhysicsServerDirect") @@ -157,6 +159,8 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -255,8 +259,10 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", "../../examples/MultiThreading/b3PosixThreadSupport.cpp", "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", - "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", - } + "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + } if os.is("Linux") then initX11() end diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index c6417b802..6bc517c8e 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -35,6 +35,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) int sensorJointIndexLeft=-1; int sensorJointIndexRight=-1; const char* urdfFileName = "r2d2.urdf"; + const char* sdfFileName = "kuka_iiwa/model.sdf"; double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; @@ -52,7 +53,53 @@ void testSharedMemory(b3PhysicsClientHandle sm) ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } - + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int bodyIndicesOut[10];//MAX_SDF_BODIES = 10 + int numJoints, numBodies; + b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED); + + numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); + ASSERT_EQ(numBodies,1); + int bodyUniqueId = bodyIndicesOut[0]; + + numJoints = b3GetNumJoints(sm,bodyUniqueId); + ASSERT_EQ(numJoints,7); + +#if 0 + b3Printf("numJoints: %d\n", numJoints); + for (i=0;i=0) || (sensorJointIndexRight>=0)) { - b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm); + b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle statusHandle; if (imuLinkIndex>=0) {