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

more URDF2Bullet refactor to make URDF import a little bit more re-usable

Browse Source
This commit is contained in:
Erwin Coumans
2015-03-20 13:07:25 -07:00
parent 3d19aec7fa
commit 20a270bc94
4 changed files with 222 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
Demos3/ImportURDFDemo/URDF2Bullet.h
View File

@@ -5,115 +5,14 @@
#include <string>
class btVector3;
class btTransform;
struct URDF2BulletConfig
{
URDF2BulletConfig()
:m_createMultiBody(true)
{
}
//true to create a btMultiBody, false to use btRigidBody
bool m_createMultiBody;
};
struct URDF2BulletCachedData
{
URDF2BulletCachedData()
:m_totalNumJoints1(0),
m_currentMultiBodyLinkIndex(-1),
m_bulletMultiBody(0)
{
}
//these arrays will be initialized in the 'InitURDF2BulletCache'
btAlignedObjectArray<int> m_urdfLinkParentIndices;
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
int m_currentMultiBodyLinkIndex;
class btMultiBody* m_bulletMultiBody;
//this will be initialized in the constructor
int m_totalNumJoints1;
int getParentUrdfIndex(int linkIndex) const
{
return m_urdfLinkParentIndices[linkIndex];
}
int getMbIndexFromUrdfIndex(int urdfIndex) const
{
if (urdfIndex==-2)
return -2;
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
}
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
{
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
{
return m_urdfLink2rigidBodies[urdfLinkIndex];
}
void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
{
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
/*
struct URDF_LinkInformation
{
const Link* m_thisLink;
int m_linkIndex;
//int m_parentIndex;
btTransform m_localInertialFrame;
//btTransform m_localVisualFrame;
btTransform m_bodyWorldTransform;
btVector3 m_localInertiaDiagonal;
btScalar m_mass;
btCollisionShape* m_collisionShape;
btRigidBody* m_bulletRigidBody;
URDF_LinkInformation()
:m_thisLink(0),
m_linkIndex(-2),
//m_parentIndex(-2),
m_collisionShape(0),
m_bulletRigidBody(0)
{
}
virtual ~URDF_LinkInformation()
{
printf("~\n");
}
};
*/
}
};
class btMultiBodyDynamicsWorld;
class btTransform;
class URDF2Bullet
{
public:
enum {
RevoluteJoint=1,
PrismaticJoint,
@@ -122,8 +21,7 @@ public:
PlanarJoint,
FixedJoint,
};
void printTree(int linkIndex, int indentationLevel=0);
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const = 0;
@@ -136,23 +34,35 @@ 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 getParent2JointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
virtual int convertLinkVisuals(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
virtual class btCompoundShape* convertLinkCollisions(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
///create Bullet collision shapes from URDF 'Collision' objects, specified in inertial frame of the link.
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
virtual void createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const = 0;
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const = 0;
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) const = 0;
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const =0;
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const = 0;
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) const = 0;
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) const = 0;
};
void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache);
void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0);
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix);
void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,const URDF2BulletConfig& mappings, const char* pathPrefix);
#endif //_URDF2BULLET_H