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

add initial examples, replacing the 'Demos/Demos3'. Will make it work cross-platform, OpenGL3/OpenGL2 and add more examples to it.

Browse Source
This commit is contained in:
erwincoumans
2015-04-16 09:55:32 -07:00
parent d9feaf2d2a
commit a1bf9c5556
425 changed files with 255913 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h Normal file
View File

@@ -0,0 +1,14 @@
#ifndef CONVERT_RIGIDBODIES_2_MULTIBODY_H
#define CONVERT_RIGIDBODIES_2_MULTIBODY_H
struct ConvertRigidBodies2MultiBody
{
btAlignedObjectArray<btRigidBody*> m_bodies;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
virtual void addRigidBody(btRigidBody* body);
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
virtual btMultiBody* convertToMultiBody();
};
#endif //CONVERT_RIGIDBODIES_2_MULTIBODY_H

1702
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

7
examples/Importers/ImportURDFDemo/ImportURDFSetup.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef IMPORT_URDF_SETUP_H
#define IMPORT_URDF_SETUP_H
class ExampleInterface* ImportURDFCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
#endif //IMPORT_URDF_SETUP_H

507
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp Normal file
View File

@@ -0,0 +1,507 @@
#include "URDF2Bullet.h"
#include <stdio.h>
#include "LinearMath/btTransform.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
static bool enableConstraints = true;
static btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
static btVector3 selectColor2()
{
static int curColor = 0;
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
return color;
}
void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
{
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices);
int numChildren = childIndices.size();
indentationLevel+=2;
int count = 0;
for (int i=0;i<numChildren;i++)
{
int childLinkIndex = childIndices[i];
std::string name = u2b.getLinkName(childLinkIndex);
for(int j=0;j<indentationLevel;j++) printf(" "); //indent
printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
// first grandchild
printTree(u2b,childLinkIndex,indentationLevel);
}
}
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;
}
};
void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex)
{
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices);
printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
for (int i=0;i<childIndices.size();i++)
{
printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
}
cache.m_totalNumJoints1 += childIndices.size();
for (int i=0;i<childIndices.size();i++)
{
int childIndex =childIndices[i];
ComputeTotalNumberOfJoints(u2b,cache,childIndex);
}
}
void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
{
cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(urdfLinkIndex,childIndices);
for (int i=0;i<childIndices.size();i++)
{
ComputeParentIndices(u2b,cache,childIndices[i],urdfLinkIndex);
}
}
void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
{
//compute the number of links, and compute parent indices array (and possibly other cached data?)
cache.m_totalNumJoints1 = 0;
int rootLinkIndex = u2b.getRootLinkIndex();
if (rootLinkIndex>=0)
{
ComputeTotalNumberOfJoints(u2b,cache,rootLinkIndex);
int numTotalLinksIncludingBase = 1+cache.m_totalNumJoints1;
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
}
}
void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{
printf("start converting/extracting data from URDF interface\n");
btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity();
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0;
std::string name = u2b.getLinkName(urdfLinkIndex);
printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity();
btScalar parentMass(1);
btVector3 parentLocalInertiaDiagonal(1,1,1);
if (urdfParentIndex==-2)
{
printf("root link has no parent\n");
} else
{
printf("urdf parent index = %d\n",urdfParentIndex);
printf("mb parent index = %d\n",mbParentIndex);
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
}
btScalar mass = 0;
btTransform localInertialFrame;
localInertialFrame.setIdentity();
btVector3 localInertiaDiagonal(0,0,0);
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
btTransform parent2joint;
parent2joint.setIdentity();
int jointType;
btVector3 jointAxisInJointSpace;
btScalar jointLowerLimit;
btScalar jointUpperLimit;
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
if (compoundShape)
{
btVector3 color = selectColor2();
/*
if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
//btVector3 localInertiaDiagonal(0, 0, 0);
//if (mass)
//{
// shape->calculateLocalInertia(mass, localInertiaDiagonal);
//}
btRigidBody* linkRigidBody = 0;
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
if (!createMultiBody)
{
btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
linkRigidBody = body;
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
compoundShape->setUserIndex(graphicsIndex);
u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
} else
{
if (cache.m_bulletMultiBody==0)
{
bool multiDof = true;
bool canSleep = false;
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
int totalNumJoints = cache.m_totalNumJoints1;
cache.m_bulletMultiBody = u2b.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
}
}
//create a joint if necessary
if (hasParentJoint) {
btTransform offsetInA,offsetInB;
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
offsetInB = localInertialFrame.inverse();
bool disableParentCollision = true;
switch (jointType)
{
case URDF2Bullet::FixedJoint:
{
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
printf("Fixed joint (btMultiBody)\n");
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
u2b.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");
btMatrix3x3 rm(offsetInA.getBasis());
btScalar y,p,r;
rm.getEulerZYX(y,p,r);
printf("y=%f,p=%f,r=%f\n", y,p,r);
//we could also use btFixedConstraint but it has some issues
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
}
break;
}
case URDF2Bullet::ContinuousJoint:
case URDF2Bullet::RevoluteJoint:
{
if (createMultiBody)
{
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else
{
//only handle principle axis at the moment,
//@todo(erwincoumans) orient the constraint for non-principal axis
int principleAxis = jointAxisInJointSpace.closestAxis();
switch (principleAxis)
{
case 0:
{
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(-1,0,0));
dof6->setAngularLowerLimit(btVector3(1,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
break;
}
case 1:
{
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,-1,0));
dof6->setAngularLowerLimit(btVector3(0,1,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
break;
}
case 2:
default:
{
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,-1));
dof6->setAngularLowerLimit(btVector3(0,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
}
};
printf("Revolute/Continuous joint\n");
}
break;
}
case URDF2Bullet::PrismaticJoint:
{
if (createMultiBody)
{
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else
{
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
int principleAxis = jointAxisInJointSpace.closestAxis();
switch (principleAxis)
{
case 0:
{
dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
break;
}
case 1:
{
dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
break;
}
case 2:
default:
{
dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
}
};
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
printf("Prismatic\n");
}
break;
}
default:
{
printf("Error: unsupported joint type in URDF (%d)\n", jointType);
btAssert(0);
}
}
}
if (createMultiBody)
{
if (compoundShape->getNumChildShapes()>0)
{
btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
compoundShape->setUserIndex(graphicsIndex);
col->setCollisionShape(compoundShape);
btTransform tr;
tr.setIdentity();
tr = linkTransformInWorldSpace;
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
col->setWorldTransform(tr);
bool isDynamic = true;
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
btVector3 color = selectColor2();//(0.0,0.0,0.5);
u2b.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
btScalar friction = 0.5f;
col->setFriction(friction);
if (mbLinkIndex>=0) //???? double-check +/- 1
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
} else
{
cache.m_bulletMultiBody->setBaseCollider(col);
}
}
}
}
btAlignedObjectArray<int> urdfChildIndices;
u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
int numChildren = urdfChildIndices.size();
for (int i=0;i<numChildren;i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
}
}
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
ConvertURDF2BulletInternal(u2b, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
}

71
examples/Importers/ImportURDFDemo/URDF2Bullet.h Normal file
View File

@@ -0,0 +1,71 @@
#ifndef _URDF2BULLET_H
#define _URDF2BULLET_H
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include <string>
class btVector3;
class btTransform;
class btMultiBodyDynamicsWorld;
class btTransform;
class URDF2Bullet
{
public:
enum {
RevoluteJoint=1,
PrismaticJoint,
ContinuousJoint,
FloatingJoint,
PlanarJoint,
FixedJoint,
};
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const = 0;
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const =0;
virtual std::string getJointName(int linkIndex) const = 0;
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0;
///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) const =0;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) 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, 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;
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const = 0;
};
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);
#endif //_URDF2BULLET_H

825
examples/Importers/ImportURDFDemo/urdf_samples.h Normal file
View File

@@ -0,0 +1,825 @@
#ifndef URDF_SAMPLES_H
#define URDF_SAMPLES_H
#define MSTRINGIFY(A) #A
const char* urdf_char2 = MSTRINGIFY(
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<link name="link3" />
<link name="link4" />
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
</joint>
</robot>);
const char* urdf_char1 = MSTRINGIFY(
<?xml version="1.0"?>
<robot name="myfirst">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</visual>
</link>
</robot>
);
const char* urdf_char3 = MSTRINGIFY(<?xml version="1.0"?>
<robot name="multipleshapes">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
</joint>
</robot>);
const char* urdf_char4 = MSTRINGIFY(
<?xml version="1.0"?>
<robot name="materials">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0.22 0 .25"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="-0.22 0 .25"/>
</joint>
<link name="head">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="head_swivel" type="fixed">
<parent link="base_link"/>
<child link="head"/>
<origin xyz="0 0 0.3"/>
</joint>
<link name="box">
<visual>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<joint name="tobox" type="fixed">
<parent link="head"/>
<child link="box"/>
<origin xyz="0 0.1414 0.1414"/>
</joint>
</robot>
);
const char* urdf_char_r2d2 = MSTRINGIFY(
<?xml version="1.0"?>
<robot name="visual">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0.22 0 .25"/>
</joint>
<link name="right_base">
<visual>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="right_base_joint" type="fixed">
<parent link="right_leg"/>
<child link="right_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="right_front_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="right_front_wheel_joint" type="fixed">
<parent link="right_base"/>
<child link="right_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
</joint>
<link name="right_back_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="right_back_wheel_joint" type="fixed">
<parent link="right_base"/>
<child link="right_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="-0.22 0 .25"/>
</joint>
<link name="left_base">
<visual>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="left_base_joint" type="fixed">
<parent link="left_leg"/>
<child link="left_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="left_front_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="left_front_wheel_joint" type="fixed">
<parent link="left_base"/>
<child link="left_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
</joint>
<link name="left_back_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="left_back_wheel_joint" type="fixed">
<parent link="left_base"/>
<child link="left_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
</joint>
<joint name="gripper_extension" type="fixed">
<parent link="base_link"/>
<child link="gripper_pole"/>
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
</joint>
<link name="gripper_pole">
<visual>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
<material name="Gray">
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
</link>
<joint name="left_gripper_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
</geometry>
</visual>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
</geometry>
</visual>
</link>
<joint name="right_gripper_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
<parent link="gripper_pole"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
</geometry>
</visual>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
</geometry>
</visual>
</link>
<link name="head">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="head_swivel" type="fixed">
<parent link="base_link"/>
<child link="head"/>
<origin xyz="0 0 0.3"/>
</joint>
<link name="box">
<visual>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<joint name="tobox" type="fixed">
<parent link="head"/>
<child link="box"/>
<origin xyz="0 0.1414 0.1414"/>
</joint>
</robot>
);
const char* urdf_char = MSTRINGIFY(
<?xml version="1.0"?>
<robot name="physics">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</collision>
<inertial>
<mass value="10"/>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0.22 0 .25"/>
</joint>
<link name="right_base">
<visual>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_base_joint" type="fixed">
<parent link="right_leg"/>
<child link="right_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="right_front_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_base"/>
<child link="right_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_back_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_base"/>
<child link="right_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</inertial>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="-0.22 0 .25"/>
</joint>
<link name="left_base">
<visual>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_base_joint" type="fixed">
<parent link="left_leg"/>
<child link="left_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="left_front_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_base"/>
<child link="left_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_back_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_base"/>
<child link="left_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="gripper_extension" type="prismatic">
<parent link="base_link"/>
<child link="gripper_pole"/>
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
</joint>
<link name="gripper_pole">
<visual>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
<material name="Gray">
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_gripper_joint" type="revolute">
<axis xyz="0 0 -1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
<parent link="gripper_pole"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="head">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="head_swivel" type="continuous">
<parent link="base_link"/>
<child link="head"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.3"/>
</joint>
<link name="box">
<visual>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="tobox" type="fixed">
<parent link="head"/>
<child link="box"/>
<origin xyz="0 0.1414 0.1414"/>
</joint>
</robot>
);
#endif //URDF_SAMPLES_H