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

parse root transformation and test loading two robots

Browse Source
This commit is contained in:
yunfeibai
2016-05-11 15:52:50 -07:00
parent 06a2669b32
commit 7929bee128
12 changed files with 879 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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; m<u2b.getNumModels();m++)
{
@@ -232,7 +230,8 @@ void ImportSDFSetup::initPhysics()
b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
u2b.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
mb = creation.getBulletMultiBody();
}
@@ -291,7 +290,7 @@ void ImportSDFSetup::stepSimulation(float deltaTime)
}
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
//m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
}
}

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

@@ -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<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{

4
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -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;

6
examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp
View File

@@ -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<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{

4
examples/Importers/ImportURDFDemo/ROSURDFImporter.h
View File

@@ -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;

18
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -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)
{

3
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -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

4
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -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<int>& 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;

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

@@ -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"))
{

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

@@ -88,7 +88,7 @@ struct UrdfLink
{
std::string m_name;
UrdfInertia m_inertia;
btTransform m_parentLinktoLinkTransform;
btTransform m_linkTransformInWorld;
btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink;
@@ -128,6 +128,7 @@ struct UrdfJoint
struct UrdfModel
{
std::string m_name;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;