diff --git a/AUTHORS.txt b/AUTHORS.txt new file mode 100644 index 000000000..8e5b831e7 --- /dev/null +++ b/AUTHORS.txt @@ -0,0 +1,39 @@ +Bullet Physics is created by Erwin Coumans with contributions from the following authors / copyright holders: + +AMD +Apple +Steve Baker +Gino van den Bergen +Nicola Candussi +Erin Catto +Lawrence Chai +Erwin Coumans +Christer Ericson +Disney Animation +Google +Dirk Gregorius +Marcus Hennix +MBSim Development Team +Takahiro Harada +Simon Hobbs +John Hsu +Ole Kniemeyer +Jay Lee +Francisco Leon +Vsevolod Klementjev +Phil Knight +John McCutchan +Steven Peters +Roman Ponomarev +Nathanael Presson +Gabor PUHR +Arthur Shek +Russel Smith +Sony +Jakub Stephien +Marten Svanfeldt +Pierre Terdiman +Steven Thompson +Tamas Umenhoffer + +If your name is missing, please send an email to erwin.coumans@gmail.com or file an issue at http://github.com/bulletphysics/bullet3 diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 000000000..0ea051f5c --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,15 @@ + +The files in this repository are licensed under the zlib license, except for the files under examples/ThirdPartyLibs. + +Bullet Continuous Collision Detection and Physics Library +http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. diff --git a/build3/premake4.lua b/build3/premake4.lua index 01e7082cc..9e0f13f8c 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -121,6 +121,7 @@ include "../examples/HelloWorld" include "../examples/BasicDemo" + include "../examples/MultiBody" diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 58c662cc2..d5a5e85be 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -53,7 +53,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"MultiBody",0), ExampleEntry(1,"TestJointTorque",TestJointTorqueCreateFunc), ExampleEntry(1,"MultiDofCreateFunc",MultiDofCreateFunc), - ExampleEntry(1,"Custom URDF",MultiBodyCustomURDFDofCreateFunc), + ExampleEntry(1,"Custom URDF",MultiBodyCustomURDFDemoCreateFunc), #ifndef _DEBUG diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 0555be24f..f9d37b437 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -27,6 +27,10 @@ "../Importers/**", "../Planar2D/Planar2D.*", "../RenderingExamples/*", + "../Constraints/*", + "../MultiBody/MultiBodyCustomURDFDemo.cpp", + "../MultiBody/MultiDofDemo.cpp", + "../MultiBody/TestJointTorqueSetup.cpp", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", "../ThirdPartyLibs/tinyxml/*", "../Utils/b3Clock.*", diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index c91835233..e3283fa25 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -11,22 +11,14 @@ #include "../CommonInterfaces/CommonParameterInterface.h" #include "MyURDFImporter.h" -static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter; -static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); -static bool enableConstraints = true;//false; + #include "URDF2Bullet.h" -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" -#include "urdf_samples.h" +//#include "urdf_samples.h" + -//#include "BulletCollision/CollisionShapes/btCylinderShape.h" -//#define USE_BARREL_VERTICES -//#include "OpenGLWindow/ShapeData.h" -#include -#include -using namespace urdf; #include "../CommonInterfaces/CommonMultiBodyBase.h" @@ -148,32 +140,6 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName) - -void printTree1(my_shared_ptr link,int level = 0) -{ - level+=2; - int count = 0; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - for(int j=0;jname << std::endl; - // first grandchild - printTree1(*child,level); - } - else - { - for(int j=0;jname << " has a null child!" << *child << std::endl; - } - } - -} - - - - void ImportUrdfSetup::initPhysics() { @@ -194,153 +160,100 @@ void ImportUrdfSetup::initPhysics() gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); - //int argc=0; - char relativeFileName[1024]; - - b3FileUtils fu; - printf("m_fileName=%s\n", m_fileName); - bool fileFound = fu.findFile(m_fileName, relativeFileName, 1024); - - - std::string xml_string; - char pathPrefix[1024]; - pathPrefix[0] = 0; - - if (!fileFound){ - std::cerr << "URDF file not found, using a dummy test URDF" << std::endl; - xml_string = std::string(urdf_char); - - } else - { - - int maxPathLen = 1024; - fu.extractPath(relativeFileName,pathPrefix,maxPathLen); - - - std::fstream xml_file(relativeFileName, std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - } - - my_shared_ptr robot = parseURDF(xml_string); - if (!robot){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return ; - } - std::cout << "robot name is: " << robot->getName() << std::endl; - - // get info from parser - std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; - // get root link - my_shared_ptr root_link=robot->getRoot(); - if (!root_link) return ; - - std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; - - // print entire tree - printTree1(root_link); - printf("now using new interface\n"); - std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; + + //now print the tree using the new interface - MyURDFImporter u2b(robot,m_guiHelper); - printTree(u2b, 0); - - btTransform identityTrans; - identityTrans.setIdentity(); - - int numJoints = (*robot).m_numJoints; + MyURDFImporter u2b(m_guiHelper); + bool loadOk = u2b.loadURDF(m_fileName); - - bool useUrdfInterfaceClass = true; + if (loadOk) + { + u2b.printTree(); - { + btTransform identityTrans; + identityTrans.setIdentity(); + + + { - btMultiBody* mb = 0; - - + btMultiBody* mb = 0; - //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user - int rootLinkIndex = u2b.getRootLinkIndex(); - printf("urdf root link index = %d\n",rootLinkIndex); - MyMultiBodyCreator creation(m_guiHelper); + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user + int rootLinkIndex = u2b.getRootLinkIndex(); + printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); - ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,pathPrefix); - mb = creation.getBulletMultiBody(); + ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix()); + mb = creation.getBulletMultiBody(); - if (m_useMultiBody) - { - mb->setHasSelfCollision(false); - mb->finalizeMultiDof(); - m_dynamicsWorld->addMultiBody(mb); + if (m_useMultiBody) + { + - //create motors for each joint + //create motors for each joint - for (int i=0;igetNumLinks();i++) - { - int mbLinkIndex = i; - if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute) - { - if (m_data->m_numMotorsgetNumLinks();i++) + { + int mbLinkIndex = i; + if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute) + { + if (m_data->m_numMotorsm_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 = 0.1f; - btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); - m_data->m_jointMotors[m_data->m_numMotors]=motor; - m_dynamicsWorld->addMultiBodyConstraint(motor); - m_data->m_numMotors++; - } - } + std::string jointName = u2b.getJointName(urdfLinkIndex); + char motorName[1024]; + sprintf(motorName,"%s q'", jointName.c_str()); + btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors]; + *motorVel = 0.f; + SliderParams slider(motorName,motorVel); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + float maxMotorImpulse = 0.1f; + btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); + m_data->m_jointMotors[m_data->m_numMotors]=motor; + m_dynamicsWorld->addMultiBodyConstraint(motor); + m_data->m_numMotors++; + } + } - } - } - } + } + } + } - //the btMultiBody support is work-in-progress :-) + //the btMultiBody support is work-in-progress :-) - printf("numJoints/DOFS = %d\n", numJoints); + - bool createGround=true; - if (createGround) - { - btVector3 groundHalfExtents(20,20,20); - groundHalfExtents[upAxis]=1.f; - btBoxShape* box = new btBoxShape(groundHalfExtents); - box->initializePolyhedralFeatures(); + bool createGround=true; + if (createGround) + { + btVector3 groundHalfExtents(20,20,20); + groundHalfExtents[upAxis]=1.f; + btBoxShape* box = new btBoxShape(groundHalfExtents); + box->initializePolyhedralFeatures(); - m_guiHelper->createCollisionShapeGraphicsObject(box); - btTransform start; start.setIdentity(); - btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-2;//.5; - start.setOrigin(groundOrigin); - btRigidBody* body = createRigidBody(0,start,box); - //m_dynamicsWorld->removeRigidBody(body); - // m_dynamicsWorld->addRigidBody(body,2,1); - btVector3 color(0.5,0.5,0.5); - m_guiHelper->createRigidBodyGraphicsObject(body,color); - } + m_guiHelper->createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); + btVector3 groundOrigin(0,0,0); + groundOrigin[upAxis]=-2;//.5; + start.setOrigin(groundOrigin); + btRigidBody* body = createRigidBody(0,start,box); + //m_dynamicsWorld->removeRigidBody(body); + // m_dynamicsWorld->addRigidBody(body,2,1); + btVector3 color(0.5,0.5,0.5); + m_guiHelper->createRigidBodyGraphicsObject(body,color); + } - ///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates. - m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.); + ///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates. + m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.); + } } void ImportUrdfSetup::stepSimulation(float deltaTime) diff --git a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h index 293290c22..04cd5bd93 100644 --- a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h +++ b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h @@ -24,6 +24,7 @@ public: virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0; virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0; + }; #endif //MULTIBODY_CREATION_INTERFACE_H \ No newline at end of file diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index aa1c56503..c0f60066d 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -69,3 +69,4 @@ btMultiBody* MyMultiBodyCreator::getBulletMultiBody() { return m_bulletMultiBody; } + diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h index 0fc1df2dc..f28360313 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h @@ -43,6 +43,7 @@ public: virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex); btMultiBody* getBulletMultiBody(); + }; #endif //MY_MULTIBODY_CREATOR diff --git a/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp b/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp index d3494ef0c..737477ba7 100644 --- a/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp +++ b/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp @@ -1,3 +1,17 @@ +/* Copyright (C) 2015 Google + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + #include "MyURDFImporter.h" @@ -9,21 +23,58 @@ #include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "Bullet3Common/b3FileUtils.h" +#include +#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" +#include +#include using namespace urdf; void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut); btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix); + + +static void printTreeInternal(my_shared_ptr link,int level = 0) +{ + level+=2; + int count = 0; + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + { + if (*child) + { + for(int j=0;jname << std::endl; + // first grandchild + printTreeInternal(*child,level); + } + else + { + for(int j=0;jname << " has a null child!" << *child << std::endl; + } + } + +} + + + + struct MyURDFInternalData { my_shared_ptr m_robot; std::vector > m_links; struct GUIHelperInterface* m_guiHelper; + const char* m_pathPrefix; }; - + +void MyURDFImporter::printTree() +{ + printTreeInternal(m_data->m_robot->getRoot(),0); +} + enum MyFileType { FILE_STL=1, @@ -33,11 +84,72 @@ enum MyFileType -MyURDFImporter::MyURDFImporter(my_shared_ptr robot,struct GUIHelperInterface* helper) +MyURDFImporter::MyURDFImporter(struct GUIHelperInterface* helper) { m_data = new MyURDFInternalData; - m_data->m_robot = robot; + m_data->m_robot = 0; m_data->m_guiHelper = helper; + m_data->m_pathPrefix=0; + + + +} + +bool MyURDFImporter::loadURDF(const char* fileName) +{ + + +//int argc=0; + char relativeFileName[1024]; + + b3FileUtils fu; + + bool fileFound = fu.findFile(fileName, relativeFileName, 1024); + + std::string xml_string; + char pathPrefix[1024]; + pathPrefix[0] = 0; + + if (!fileFound){ + std::cerr << "URDF file not found" << std::endl; + return false; + } else + { + + int maxPathLen = 1024; + fu.extractPath(relativeFileName,pathPrefix,maxPathLen); + + + std::fstream xml_file(relativeFileName, std::fstream::in); + while ( xml_file.good() ) + { + std::string line; + std::getline( xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + } + + my_shared_ptr robot = parseURDF(xml_string); + if (!robot){ + std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; + return false; + } + std::cout << "robot name is: " << robot->getName() << std::endl; + + // get info from parser + std::cout << "Successfully Parsed URDF" << std::endl; + // get root link + my_shared_ptr root_link=robot->getRoot(); + if (!root_link) + { + std::cout << "Failed to find root link in URDF" << std::endl; + return false; + } + + + m_data->m_robot = robot; + m_data->m_robot->getLinks(m_data->m_links); //initialize the 'index' of each link @@ -45,9 +157,16 @@ MyURDFImporter::MyURDFImporter(my_shared_ptr robot,struct GUIHel { m_data->m_links[i]->m_link_index = i; } - + + return true; } +const char* MyURDFImporter::getPathPrefix() +{ + return m_data->m_pathPrefix; +} + + MyURDFImporter::~MyURDFImporter() { delete m_data; diff --git a/examples/Importers/ImportURDFDemo/MyURDFImporter.h b/examples/Importers/ImportURDFDemo/MyURDFImporter.h index 84e86bf38..d36d42d11 100644 --- a/examples/Importers/ImportURDFDemo/MyURDFImporter.h +++ b/examples/Importers/ImportURDFDemo/MyURDFImporter.h @@ -2,9 +2,7 @@ #define MY_URDF_IMPORTER_H #include "URDFImporterInterface.h" -#include //temp, replace by btAlignedObjectArray -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" class MyURDFImporter : public URDFImporterInterface { @@ -13,13 +11,18 @@ class MyURDFImporter : public URDFImporterInterface public: - - MyURDFImporter(my_shared_ptr robot,struct GUIHelperInterface* helper); - + MyURDFImporter(struct GUIHelperInterface* guiHelper); + virtual ~MyURDFImporter(); + + virtual bool loadURDF(const char* fileName); + + const char* getPathPrefix(); + + void printTree(); //for debugging - virtual int getRootLinkIndex() const; + virtual int getRootLinkIndex() const; virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; @@ -31,7 +34,7 @@ public: virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const; - virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index c13c3a45a..c389d6b3c 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -290,11 +290,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); - btMatrix3x3 rm(rot); - btScalar y,p,r; - rm.getEulerZYX(y,p,r); - printf("y=%f,p=%f,r=%f\n", y,p,r); - + } else { printf("Fixed joint\n"); @@ -505,6 +501,16 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter int urdfLinkIndex = u2b.getRootLinkIndex(); ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix); + if (world1 && cache.m_bulletMultiBody) + { + btMultiBody* mb = cache.m_bulletMultiBody; + mb->setHasSelfCollision(false); + mb->finalizeMultiDof(); + + mb->setBaseWorldTransform(rootTransformInWorldSpace); + + world1->addMultiBody(mb); + } } diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index aeca98dd5..73745e217 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -16,13 +16,13 @@ class MultiBodyCreationInterface; void printTree(const URDFImporterInterface& u2b, int linkIndex, int identationLevel=0); - void ConvertURDF2Bullet(const URDFImporterInterface& u2b, - MultiBodyCreationInterface& creationCallback, - const btTransform& rootTransformInWorldSpace, - btMultiBodyDynamicsWorld* world, - bool createMultiBody, - const char* pathPrefix); + MultiBodyCreationInterface& creationCallback, + const btTransform& rootTransformInWorldSpace, + btMultiBodyDynamicsWorld* world, + bool createMultiBody, + const char* pathPrefix); + #endif //_URDF2BULLET_H diff --git a/examples/MultiBody/CustomMultiBodyCreationCallback.h b/examples/MultiBody/CustomMultiBodyCreationCallback.h index a57da650d..7e013d38f 100644 --- a/examples/MultiBody/CustomMultiBodyCreationCallback.h +++ b/examples/MultiBody/CustomMultiBodyCreationCallback.h @@ -1,3 +1,17 @@ +/* Copyright (C) 2015 Google + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + #ifndef CUSTOM_MULTIBODY_CALLBACK_H #define CUSTOM_MULTIBODY_CALLBACK_H diff --git a/examples/MultiBody/MultiBodyCustomURDFDemo.cpp b/examples/MultiBody/MultiBodyCustomURDFDemo.cpp index 1ee37a7db..34131be2b 100644 --- a/examples/MultiBody/MultiBodyCustomURDFDemo.cpp +++ b/examples/MultiBody/MultiBodyCustomURDFDemo.cpp @@ -1,3 +1,15 @@ +/* Copyright (C) 2015 Google + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ #include "MultiBodyCustomURDFDemo.h" @@ -6,6 +18,9 @@ #include "../CommonInterfaces/CommonMultiBodyBase.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Importers/ImportURDFDemo/MyURDFImporter.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" + #include "CustomMultiBodyCreationCallback.h" @@ -34,31 +49,100 @@ MultiBodyCustomURDFDemo::~MultiBodyCustomURDFDemo() } + +static void myPrintTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel) +{ + btAlignedObjectArray childIndices; + u2b.getLinkChildIndices(linkIndex,childIndices); + + int numChildren = childIndices.size(); + + indentationLevel+=2; + int count = 0; + for (int i=0;isetUpAxis(upAxis); createEmptyDynamicsWorld(); + btVector3 grav(0,0,0); + //grav[upAxis] = -10.f; + + m_dynamicsWorld->setGravity(grav); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_dynamicsWorld->getDebugDrawer()->setDebugMode( - //btIDebugDraw::DBG_DrawConstraints - +btIDebugDraw::DBG_DrawWireframe - +btIDebugDraw::DBG_DrawContactPoints - +btIDebugDraw::DBG_DrawAabb - );//+btIDebugDraw::DBG_DrawConstraintLimits); - + + MyURDFImporter u2b(m_guiHelper); + bool loadOk = u2b.loadURDF("r2d2.urdf"); + + if (loadOk) + { + u2b.printTree(); + int urdfRootLinkIndex = u2b.getRootLinkIndex(); + myPrintTree(u2b,urdfRootLinkIndex,0); + + btTransform identityTrans; + identityTrans.setIdentity(); + + btMultiBody* mb = 0; + + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user + int rootLinkIndex = u2b.getRootLinkIndex(); + printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); + + bool useMultiBody = true; + + ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); + mb = creation.getBulletMultiBody(); + + } } void MultiBodyCustomURDFDemo::stepSimulation(float deltaTime) { - m_dynamicsWorld->stepSimulation(deltaTime); } -class ExampleInterface* MultiBodyCustomURDFDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option) +class ExampleInterface* MultiBodyCustomURDFDemoCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option) { return new MultiBodyCustomURDFDemo(helper); } \ No newline at end of file diff --git a/examples/MultiBody/MultiBodyCustomURDFDemo.h b/examples/MultiBody/MultiBodyCustomURDFDemo.h index e13a0102f..f84b6d72c 100644 --- a/examples/MultiBody/MultiBodyCustomURDFDemo.h +++ b/examples/MultiBody/MultiBodyCustomURDFDemo.h @@ -1,6 +1,20 @@ +/* Copyright (C) 2015 Google + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + #ifndef MULTI_DOF_CUSTOM_URDF_DEMO_H #define MULTI_DOF_CUSTOM_URDF_DEMO_H -class ExampleInterface* MultiBodyCustomURDFDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option); +class ExampleInterface* MultiBodyCustomURDFDemoCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option); #endif //MULTI_DOF_CUSTOM_URDF_DEMO_H diff --git a/examples/MultiBody/main.cpp b/examples/MultiBody/main.cpp new file mode 100644 index 000000000..5891fb4c6 --- /dev/null +++ b/examples/MultiBody/main.cpp @@ -0,0 +1,43 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "MultiBodyCustomURDFDemo.h" + +#include "../CommonInterfaces/ExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" + + + + +int main(int argc, char* argv[]) +{ + struct PhysicsInterface* pint = 0; + + DummyGUIHelper noGfx; + + int option = 0; + + ExampleInterface* example = MultiBodyCustomURDFDemoCreateFunc(pint, &noGfx, option); + + example->initPhysics(); + example->stepSimulation(1.f/60.f); + example->exitPhysics(); + + delete example; + + return 0; +} + diff --git a/examples/MultiBody/premake4.lua b/examples/MultiBody/premake4.lua new file mode 100644 index 000000000..9959f7c65 --- /dev/null +++ b/examples/MultiBody/premake4.lua @@ -0,0 +1,40 @@ + +project "App_CustomMultiBodyCreation" + +if _OPTIONS["ios"] then + kind "WindowedApp" +else + kind "ConsoleApp" +end + +includedirs {".", "../../src", "../ThirdPartyLibs"} + +links { + "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath" +} + +language "C++" + +files { + "MultiBodyCustomURDFDemo.cpp", + "main.cpp", + "../Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../Importers/ImportURDFDemo/MyURDFImporter.cpp", + "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../ThirdPartyLibs/tinyxml/tinystr.cpp", + "../ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp", + "../ThirdPartyLibs/urdf/boost_replacement/string_split.cpp", + "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp", + "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp", + "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp", + "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp", + +} + diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 2e2f645ff..c24b6db2d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -178,6 +178,13 @@ public: { m_basePos = pos; } + + void setBaseWorldTransform(const btTransform& tr) + { + setBasePos(tr.getOrigin()); + setWorldToBaseRot(tr.getRotation().inverse()); + + } void setBaseVel(const btVector3 &vel) {