diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf index 900626708..1b6f8320d 100644 --- a/data/kuka_iiwa/model.sdf +++ b/data/kuka_iiwa/model.sdf @@ -88,7 +88,7 @@ - 0 0 0.2025 1.57079632679 0 3.14159265359 + 0 0 0.36 1.5708 -0 -3.14159 0.0003 0.059 0.042 0 -0 0 4 @@ -141,7 +141,7 @@ - 0 0.2045 0 1.57079632679 0 3.14159265359 + 0 -0 0.5645 0 0 0 0 0.03 0.13 0 -0 0 3 @@ -194,7 +194,7 @@ - 0 0 0.2155 1.57079632679 0 0 + 0 -0 0.78 1.5708 0 0 0 0.067 0.034 0 -0 0 2.7 @@ -247,7 +247,7 @@ - 0 0.1845 0 -1.57079632679 3.14159265359 0 + 0 -0 0.9645 0 -0 -3.14159 0.0001 0.021 0.076 0 -0 0 1.7 @@ -300,7 +300,7 @@ - 0 0 0.2155 1.57079632679 0 0 + 0 0 1.18 1.5708 -0 -3.14159 0 0.0006 0.0004 0 -0 0 1.8 @@ -353,7 +353,7 @@ - 0 0.081 0 -1.57079632679 3.14159265359 0 + 0 0 1.261 0 0 0 0 0 0.02 0 -0 0 0.3 diff --git a/data/kuka_iiwa/model2.sdf b/data/kuka_iiwa/model2.sdf new file mode 100644 index 000000000..117357cf4 --- /dev/null +++ b/data/kuka_iiwa/model2.sdf @@ -0,0 +1,818 @@ + + + + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + + 2 2 0 0 -0 0 + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + \ No newline at end of file diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index fa54b0d29..848ee699c 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -131,7 +131,7 @@ ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, co if (gFileNameArray.size()==0) { - gFileNameArray.push_back("kuka_iiwa/model.sdf"); + gFileNameArray.push_back("two_cubes.sdf"); } @@ -214,10 +214,8 @@ void ImportSDFSetup::initPhysics() //u2b.printTree(); - btTransform identityTrans; - identityTrans.setIdentity(); - - + btTransform rootTrans; + rootTrans.setIdentity(); for (int m =0; mstepSimulation(deltaTime,10,1./240.); + m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index a6cc8019f..b409551f3 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -296,7 +296,7 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect } } -bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const +bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const { jointLowerLimit = 0.f; jointUpperLimit = 0.f; @@ -308,7 +308,7 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi if (linkPtr) { UrdfLink* link = *linkPtr; - + linkTransformInWorld = link->m_linkTransformInWorld; if (link->m_parentJoint) { @@ -333,7 +333,11 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi } - +bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const +{ + rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld; + return true; +} void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index edfafc135..5e1437158 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -40,7 +40,9 @@ public: virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp index 6608a2ad4..5e6ce6fb9 100644 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp @@ -223,7 +223,7 @@ void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3 } } -bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const +bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const { jointLowerLimit = 0.f; jointUpperLimit = 0.f; @@ -281,7 +281,9 @@ bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, } } - +bool ROSURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const +{ +} void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) { diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h index ad09339e1..289fcbb8b 100644 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h @@ -33,7 +33,9 @@ public: virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index a7db650ef..6ff360788 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -143,7 +143,7 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat } -void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) +void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) { //b3Printf("start converting/extracting data from URDF interface\n"); @@ -199,7 +199,15 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btScalar jointFriction; - bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); + bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); + if (useSDF) + { + parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace; + } + else + { + linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; + } int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); @@ -400,18 +408,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { int urdfChildLinkIndex = urdfChildIndices[i]; - ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix); + ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); } } -void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) +void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) { URDF2BulletCachedData cache; InitURDF2BulletCache(u2b,cache); int urdfLinkIndex = u2b.getRootLinkIndex(); - ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix); + ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); if (world1 && cache.m_bulletMultiBody) { diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 0d9306848..148535812 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -20,7 +20,8 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world, bool createMultiBody, - const char* pathPrefix); + const char* pathPrefix, + bool useSDF = false); #endif //_URDF2BULLET_H diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 91dc52f31..fd794ba3c 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -37,7 +37,9 @@ public: ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const =0; - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0; ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 074266fc6..60b40b512 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -532,11 +532,11 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi TiXmlElement* pose = config->FirstChildElement("pose"); if (0==pose) { - link.m_parentLinktoLinkTransform.setIdentity(); + link.m_linkTransformInWorld.setIdentity(); } else { - parseTransform(link.m_parentLinktoLinkTransform, pose,logger,m_parseSDF); + parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF); } } @@ -1017,11 +1017,6 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger) parentLink->m_childJoints.push_back(joint); parentLink->m_childLinks.push_back(childLink); parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str()); - - if (m_parseSDF) { - joint->m_parentLinkToJointTransform = childLink->m_parentLinktoLinkTransform; - } - } } @@ -1255,8 +1250,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) } localModel->m_name = name; - - + TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose"); + if (0==pose_xml) + { + localModel->m_rootTransformInWorld.setIdentity(); + } + else + { + parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF); + } + // Get all Material elements for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 62897257e..bb0877172 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -88,7 +88,7 @@ struct UrdfLink { std::string m_name; UrdfInertia m_inertia; - btTransform m_parentLinktoLinkTransform; + btTransform m_linkTransformInWorld; btArray m_visualArray; btArray m_collisionArray; UrdfLink* m_parentLink; @@ -128,6 +128,7 @@ struct UrdfJoint struct UrdfModel { std::string m_name; + btTransform m_rootTransformInWorld; btHashMap m_materials; btHashMap m_links; btHashMap m_joints;