From e9c6abff47718e68bb1c314b36814ca3a3ef6c7b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 9 May 2016 17:25:07 -0700 Subject: [PATCH] add initial SDF importer, work-in-progress (still too incomplete to be useful) --- data/two_cubes.sdf | 239 +++++++++++ examples/ExampleBrowser/CMakeLists.txt | 1 + examples/ExampleBrowser/ExampleEntries.cpp | 4 +- .../ImportSDFDemo/ImportSDFSetup.cpp | 302 ++++++++++++++ .../Importers/ImportSDFDemo/ImportSDFSetup.h | 8 + .../ImportURDFDemo/BulletUrdfImporter.cpp | 58 +++ .../ImportURDFDemo/BulletUrdfImporter.h | 6 + .../ImportURDFDemo/ImportURDFSetup.cpp | 2 +- .../ImportURDFDemo/URDFImporterInterface.h | 2 + .../Importers/ImportURDFDemo/UrdfParser.cpp | 370 ++++++++++++++---- .../Importers/ImportURDFDemo/UrdfParser.h | 78 +++- 11 files changed, 991 insertions(+), 79 deletions(-) create mode 100644 data/two_cubes.sdf create mode 100644 examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp create mode 100644 examples/Importers/ImportSDFDemo/ImportSDFSetup.h diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf new file mode 100644 index 000000000..24c0854bd --- /dev/null +++ b/data/two_cubes.sdf @@ -0,0 +1,239 @@ + + + + 99.2 + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 1 2 3 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 4 5 6 + 100 100 + + + + + + + 0 + 0 + 1 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 0.512455 -1.58317 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + 0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + 0 0 + 0 0 + 1462824251 956472000 + 0 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159 + 1 1 1 + + 0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159 + 0.004896 3e-06 -0.004891 -6e-06 0.009793 -0 + 0.010615 0.006191 -9.78231 -0.012424 0.021225 -1.8e-05 + 0.010615 0.006191 -9.78231 0 -0 0 + + + + 0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159 + 1 1 1 + + 0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 8.0562 -8.87312 3.07529 0 0.205021 2.5208 + orbit + perspective + + + + diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index fef09597d..0a1c172f0 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -132,6 +132,7 @@ SET(BulletExampleBrowser_SRCS ../Importers/ImportObjDemo/LoadMeshFromObj.cpp ../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp ../Importers/ImportSTLDemo/ImportSTLSetup.cpp + ../Importers/ImportSDFDemo/ImportSDFSetup.cpp ../Importers/ImportURDFDemo/ImportURDFSetup.cpp ../Importers/ImportURDFDemo/URDF2Bullet.cpp ../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 6c66e2716..6d8b86bea 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -14,6 +14,7 @@ #include "../Importers/ImportColladaDemo/ImportColladaSetup.h" #include "../Importers/ImportSTLDemo/ImportSTLSetup.h" #include "../Importers/ImportURDFDemo/ImportURDFSetup.h" +#include "../Importers/ImportSDFDemo/ImportSDFSetup.h" #include "../Collision/CollisionTutorialBullet2.h" #include "../GyroscopicDemo/GyroscopicSetup.h" #include "../Constraints/Dof6Spring2Setup.h" @@ -126,7 +127,6 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF), ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY), - ExampleEntry(0,"Tutorial"), ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY), ExampleEntry(1,"Gravity Acceleration","Motion of a free falling rigid body under constant gravitational acceleration", TutorialCreateFunc,TUT_ACCELERATION), @@ -210,7 +210,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"URDF (RigidBody)", "Import a URDF file, and create rigid bodies (btRigidBody) connected by constraints.", ImportURDFCreateFunc, 0), ExampleEntry(1,"URDF (MultiBody)", "Import a URDF file and create a single multibody (btMultiBody) with tree hierarchy of links (mobilizers).", ImportURDFCreateFunc, 1), - + ExampleEntry(1,"SDF (MultiBody)", "Import an SDF file, create multiple multibodies etc", ImportSDFCreateFunc), ExampleEntry(0,"Vehicles"), ExampleEntry(1,"Hinge2 Vehicle", "A rigid body chassis with 4 rigid body wheels attached by a btHinge2Constraint",Hinge2VehicleCreateFunc), diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp new file mode 100644 index 000000000..b95f528a9 --- /dev/null +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -0,0 +1,302 @@ + +#include "ImportSDFSetup.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "Bullet3Common/b3FileUtils.h" + +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "../../Utils/b3ResourcePath.h" + +#include "../ImportURDFDemo/BulletUrdfImporter.h" + + +#include "../ImportURDFDemo/URDF2Bullet.h" + + +//#include "urdf_samples.h" + + + + + +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +#include "../ImportURDFDemo/MyMultiBodyCreator.h" + + +class ImportSDFSetup : public CommonMultiBodyBase +{ + char m_fileName[1024]; + + struct ImportSDFInternalData* m_data; + bool m_useMultiBody; + + //todo(erwincoumans) we need a name memory for each model + btAlignedObjectArray m_nameMemory; + +public: + ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName); + virtual ~ImportSDFSetup(); + + virtual void initPhysics(); + virtual void stepSimulation(float deltaTime); + + void setFileName(const char* urdfFileName); + + virtual void resetCamera() + { + float dist = 3.5; + float pitch = -136; + float yaw = 28; + float targetPos[3]={0.47,0,-0.64}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } +}; + + +static btAlignedObjectArray gFileNameArray; + + +#define MAX_NUM_MOTORS 1024 + +struct ImportSDFInternalData +{ + ImportSDFInternalData() + :m_numMotors(0) + { + for (int i=0;i=numFileNames) + { + count=0; + } + sprintf(m_fileName,"%s",gFileNameArray[count++].c_str()); + } +} + +ImportSDFSetup::~ImportSDFSetup() +{ + for (int i=0;isetUpAxis(upAxis); + + this->createEmptyDynamicsWorld(); + //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawContactPoints + +btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); + + + btVector3 gravity(0,0,0); + gravity[upAxis]=-9.8; + + m_dynamicsWorld->setGravity(gravity); + + BulletURDFImporter u2b(m_guiHelper); + + bool loadOk = u2b.loadSDF(m_fileName); + + + if (loadOk) + { + //printTree(u2b,u2b.getRootLinkIndex()); + + //u2b.printTree(); + + btTransform identityTrans; + identityTrans.setIdentity(); + + + + for (int m =0; m