Merge pull request #342 from erwincoumans/master
small URDF2Bullet refactor to make URDF import a little bit more re-useable
This commit is contained in:
erwincoumans
@@ -46,6 +46,7 @@ SET(App_AllBullet2Demos_SRCS
|
|||||||
# ../bullet2/RagdollDemo/RagdollDemo.h
|
# ../bullet2/RagdollDemo/RagdollDemo.h
|
||||||
../bullet2/LuaDemo/LuaPhysicsSetup.cpp
|
../bullet2/LuaDemo/LuaPhysicsSetup.cpp
|
||||||
../bullet2/LuaDemo/LuaPhysicsSetup.h
|
../bullet2/LuaDemo/LuaPhysicsSetup.h
|
||||||
|
../ImportURDFDemo/URDF2Bullet.cpp
|
||||||
../ImportURDFDemo/ImportURDFSetup.cpp
|
../ImportURDFDemo/ImportURDFSetup.cpp
|
||||||
../ImportURDFDemo/ImportURDFSetup.h
|
../ImportURDFDemo/ImportURDFSetup.h
|
||||||
../ImportObjDemo/ImportObjSetup.cpp
|
../ImportObjDemo/ImportObjSetup.cpp
|
||||||
|
|||||||
@@ -69,6 +69,7 @@
|
|||||||
-- "../bullet2/SoftDemo/SoftDemo.cpp",
|
-- "../bullet2/SoftDemo/SoftDemo.cpp",
|
||||||
"../ImportColladaDemo/LoadMeshFromCollada.cpp",
|
"../ImportColladaDemo/LoadMeshFromCollada.cpp",
|
||||||
"../ImportColladaDemo/ImportColladaSetup.cpp",
|
"../ImportColladaDemo/ImportColladaSetup.cpp",
|
||||||
|
"../ImportURDFDemo/URDF2Bullet.cpp",
|
||||||
"../ImportURDFDemo/ImportURDFSetup.cpp",
|
"../ImportURDFDemo/ImportURDFSetup.cpp",
|
||||||
"../ImportObjDemo/ImportObjSetup.cpp",
|
"../ImportObjDemo/ImportObjSetup.cpp",
|
||||||
"../ImportObjDemo/LoadMeshFromObj.cpp",
|
"../ImportObjDemo/LoadMeshFromObj.cpp",
|
||||||
|
|||||||
@@ -2,60 +2,296 @@
|
|||||||
#include "ImportURDFSetup.h"
|
#include "ImportURDFSetup.h"
|
||||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||||
#include "Bullet3Common/b3FileUtils.h"
|
#include "Bullet3Common/b3FileUtils.h"
|
||||||
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
|
|
||||||
#include "../ImportObjDemo/LoadMeshFromObj.h"
|
#include "../ImportObjDemo/LoadMeshFromObj.h"
|
||||||
|
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
|
||||||
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
|
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
|
|
||||||
#include "Bullet3Common/b3FileUtils.h"
|
#include "Bullet3Common/b3FileUtils.h"
|
||||||
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
|
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
|
||||||
|
|
||||||
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
|
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
|
||||||
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
|
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
|
||||||
static bool enableConstraints = true;//false;
|
static bool enableConstraints = true;//false;
|
||||||
|
#include "URDF2Bullet.h"
|
||||||
|
|
||||||
|
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
|
||||||
|
|
||||||
const char* fileNames[] =
|
#include "urdf_samples.h"
|
||||||
|
|
||||||
|
//#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
||||||
|
//#define USE_BARREL_VERTICES
|
||||||
|
//#include "OpenGLWindow/ShapeData.h"
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
using namespace urdf;
|
||||||
|
|
||||||
|
void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
|
||||||
|
btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix);
|
||||||
|
|
||||||
|
class MyURDF2Bullet : public URDF2Bullet
|
||||||
{
|
{
|
||||||
"r2d2.urdf",
|
my_shared_ptr<ModelInterface> m_robot;
|
||||||
"r2d2.urdf",
|
std::vector<my_shared_ptr<Link> > m_links;
|
||||||
|
GraphicsPhysicsBridge& m_gfxBridge;
|
||||||
|
mutable btMultiBody* m_bulletMultiBody;
|
||||||
|
|
||||||
};
|
public:
|
||||||
|
MyURDF2Bullet(my_shared_ptr<ModelInterface> robot,GraphicsPhysicsBridge& gfxBridge)
|
||||||
#define MAX_NUM_MOTORS 1024
|
:m_robot(robot),
|
||||||
|
m_gfxBridge(gfxBridge),
|
||||||
struct ImportUrdfInternalData
|
m_bulletMultiBody(0)
|
||||||
{
|
{
|
||||||
ImportUrdfInternalData()
|
m_robot->getLinks(m_links);
|
||||||
:m_numMotors(0)
|
|
||||||
|
//initialize the 'index' of each link
|
||||||
|
for (int i=0;i<m_links.size();i++)
|
||||||
{
|
{
|
||||||
|
m_links[i]->m_link_index = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual int getRootLinkIndex() const
|
||||||
|
{
|
||||||
|
if (m_links.size())
|
||||||
|
{
|
||||||
|
int rootLinkIndex = m_robot->getRoot()->m_link_index;
|
||||||
|
// btAssert(m_links[0]->m_link_index == rootLinkIndex);
|
||||||
|
return rootLinkIndex;
|
||||||
|
}
|
||||||
|
return -1;
|
||||||
|
};
|
||||||
|
|
||||||
|
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
|
||||||
|
{
|
||||||
|
childLinkIndices.resize(0);
|
||||||
|
int numChildren = m_links[linkIndex]->child_links.size();
|
||||||
|
|
||||||
|
for (int i=0;i<numChildren;i++)
|
||||||
|
{
|
||||||
|
int childIndex =m_links[linkIndex]->child_links[i]->m_link_index;
|
||||||
|
childLinkIndices.push_back(childIndex);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
virtual std::string getLinkName(int linkIndex) const
|
||||||
|
{
|
||||||
|
std::string n = m_links[linkIndex]->name;
|
||||||
|
return n;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
|
||||||
|
{
|
||||||
|
if ((*m_links[linkIndex]).inertial)
|
||||||
|
{
|
||||||
|
mass = (*m_links[linkIndex]).inertial->mass;
|
||||||
|
localInertiaDiagonal.setValue((*m_links[linkIndex]).inertial->ixx,(*m_links[linkIndex]).inertial->iyy,(*m_links[linkIndex]).inertial->izz);
|
||||||
|
inertialFrame.setOrigin(btVector3((*m_links[linkIndex]).inertial->origin.position.x,(*m_links[linkIndex]).inertial->origin.position.y,(*m_links[linkIndex]).inertial->origin.position.z));
|
||||||
|
inertialFrame.setRotation(btQuaternion((*m_links[linkIndex]).inertial->origin.rotation.x,(*m_links[linkIndex]).inertial->origin.rotation.y,(*m_links[linkIndex]).inertial->origin.rotation.z,(*m_links[linkIndex]).inertial->origin.rotation.w));
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
mass = 1.f;
|
||||||
|
localInertiaDiagonal.setValue(1,1,1);
|
||||||
|
inertialFrame.setIdentity();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
|
||||||
|
{
|
||||||
|
jointLowerLimit = 0.f;
|
||||||
|
jointUpperLimit = 0.f;
|
||||||
|
|
||||||
|
if ((*m_links[urdfLinkIndex]).parent_joint)
|
||||||
|
{
|
||||||
|
my_shared_ptr<Joint> pj =(*m_links[urdfLinkIndex]).parent_joint;
|
||||||
|
|
||||||
|
const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
|
||||||
|
const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
|
||||||
|
|
||||||
|
jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
|
||||||
|
parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
|
||||||
|
parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
|
||||||
|
|
||||||
|
switch (pj->type)
|
||||||
|
{
|
||||||
|
case Joint::REVOLUTE:
|
||||||
|
jointType = URDF2Bullet::RevoluteJoint;
|
||||||
|
break;
|
||||||
|
case Joint::FIXED:
|
||||||
|
jointType = URDF2Bullet::FixedJoint;
|
||||||
|
break;
|
||||||
|
case Joint::PRISMATIC:
|
||||||
|
jointType = URDF2Bullet::PrismaticJoint;
|
||||||
|
break;
|
||||||
|
case Joint::PLANAR:
|
||||||
|
jointType = URDF2Bullet::PlanarJoint;
|
||||||
|
break;
|
||||||
|
case Joint::CONTINUOUS:
|
||||||
|
jointType = URDF2Bullet::ContinuousJoint;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
{
|
||||||
|
printf("Error: unknown joint type %d\n", pj->type);
|
||||||
|
btAssert(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
if (pj->limits)
|
||||||
|
{
|
||||||
|
jointLowerLimit = pj->limits.get()->lower;
|
||||||
|
jointUpperLimit = pj->limits.get()->upper;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
parent2joint.setIdentity();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
|
||||||
|
{
|
||||||
|
btAlignedObjectArray<GLInstanceVertex> vertices;
|
||||||
|
btAlignedObjectArray<int> indices;
|
||||||
|
btTransform startTrans; startTrans.setIdentity();
|
||||||
|
int graphicsIndex = -1;
|
||||||
|
|
||||||
|
for (int v = 0; v < (int)m_links[linkIndex]->visual_array.size(); v++)
|
||||||
|
{
|
||||||
|
const Visual* vis = m_links[linkIndex]->visual_array[v].get();
|
||||||
|
btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
|
||||||
|
btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
|
||||||
|
btTransform childTrans;
|
||||||
|
childTrans.setOrigin(childPos);
|
||||||
|
childTrans.setRotation(childOrn);
|
||||||
|
|
||||||
|
convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
if (vertices.size() && indices.size())
|
||||||
|
{
|
||||||
|
graphicsIndex = m_gfxBridge.registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
return graphicsIndex;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
|
||||||
|
{
|
||||||
|
|
||||||
|
btCompoundShape* compoundShape = new btCompoundShape();
|
||||||
|
compoundShape->setMargin(0.001);
|
||||||
|
|
||||||
|
for (int v=0;v<(int)m_links[linkIndex]->collision_array.size();v++)
|
||||||
|
{
|
||||||
|
const Collision* col = m_links[linkIndex]->collision_array[v].get();
|
||||||
|
btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
|
||||||
|
|
||||||
|
if (childShape)
|
||||||
|
{
|
||||||
|
btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
|
||||||
|
btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
|
||||||
|
btTransform childTrans;
|
||||||
|
childTrans.setOrigin(childPos);
|
||||||
|
childTrans.setRotation(childOrn);
|
||||||
|
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return compoundShape;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual class btMultiBody* allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const
|
||||||
|
{
|
||||||
|
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
|
||||||
|
return m_bulletMultiBody;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const
|
||||||
|
{
|
||||||
|
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
||||||
|
rbci.m_startWorldTransform = initialWorldTrans;
|
||||||
|
btRigidBody* body = new btRigidBody(rbci);
|
||||||
|
return body;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) const
|
||||||
|
{
|
||||||
|
btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
|
||||||
|
return mbCol;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) const
|
||||||
|
{
|
||||||
|
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
virtual void createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const
|
||||||
|
{
|
||||||
|
|
||||||
|
m_gfxBridge.createRigidBodyGraphicsObject(body, colorRgba);
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba) const
|
||||||
|
{
|
||||||
|
m_gfxBridge.createCollisionObjectGraphicsObject(colObj,colorRgba);
|
||||||
|
}
|
||||||
|
|
||||||
|
btMultiBody* getBulletMultiBody()
|
||||||
|
{
|
||||||
|
return m_bulletMultiBody;
|
||||||
}
|
}
|
||||||
|
|
||||||
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
|
|
||||||
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
|
|
||||||
int m_numMotors;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
btAlignedObjectArray<std::string> gFileNameArray;
|
||||||
|
|
||||||
ImportUrdfSetup::ImportUrdfSetup()
|
ImportUrdfSetup::ImportUrdfSetup()
|
||||||
{
|
{
|
||||||
m_data = new ImportUrdfInternalData;
|
|
||||||
|
|
||||||
static int count = 0;
|
static int count = 0;
|
||||||
|
gFileNameArray.clear();
|
||||||
|
gFileNameArray.push_back("r2d2.urdf");
|
||||||
|
|
||||||
sprintf(m_fileName,fileNames[count++]);
|
//load additional urdf file names from file
|
||||||
int sz = sizeof(fileNames)/sizeof(char*);
|
|
||||||
if (count>=sz)
|
FILE* f = fopen("urdf_files.txt","r");
|
||||||
|
if (f)
|
||||||
|
{
|
||||||
|
int result;
|
||||||
|
//warning: we don't avoid string buffer overflow in this basic example in fscanf
|
||||||
|
char fileName[1024];
|
||||||
|
do
|
||||||
|
{
|
||||||
|
result = fscanf(f,"%s",fileName);
|
||||||
|
if (result==1)
|
||||||
|
{
|
||||||
|
gFileNameArray.push_back(fileName);
|
||||||
|
}
|
||||||
|
} while (result==1);
|
||||||
|
|
||||||
|
fclose(f);
|
||||||
|
}
|
||||||
|
|
||||||
|
int numFileNames = gFileNameArray.size();
|
||||||
|
|
||||||
|
if (count>=numFileNames)
|
||||||
{
|
{
|
||||||
count=0;
|
count=0;
|
||||||
}
|
}
|
||||||
|
sprintf(m_fileName,gFileNameArray[count++].c_str());
|
||||||
}
|
}
|
||||||
|
|
||||||
ImportUrdfSetup::~ImportUrdfSetup()
|
ImportUrdfSetup::~ImportUrdfSetup()
|
||||||
{
|
{
|
||||||
delete m_data;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static btVector4 colors[4] =
|
static btVector4 colors[4] =
|
||||||
@@ -82,18 +318,9 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
|
|||||||
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
|
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
|
||||||
}
|
}
|
||||||
|
|
||||||
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
|
|
||||||
|
|
||||||
#include "urdf_samples.h"
|
|
||||||
|
|
||||||
//#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
|
||||||
//#define USE_BARREL_VERTICES
|
|
||||||
//#include "OpenGLWindow/ShapeData.h"
|
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
#include <fstream>
|
|
||||||
|
|
||||||
using namespace urdf;
|
|
||||||
|
|
||||||
void printTree(my_shared_ptr<const Link> link,int level = 0)
|
void printTree(my_shared_ptr<const Link> link,int level = 0)
|
||||||
{
|
{
|
||||||
@@ -157,24 +384,13 @@ struct URDF_JointInformation
|
|||||||
struct URDF2BulletMappings
|
struct URDF2BulletMappings
|
||||||
{
|
{
|
||||||
btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
|
btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
|
||||||
//btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
|
|
||||||
|
|
||||||
//btAlignedObjectArray<btTransform> m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
|
|
||||||
btAlignedObjectArray<btScalar> m_linkMasses;
|
btAlignedObjectArray<btScalar> m_linkMasses;
|
||||||
//btAlignedObjectArray<btVector3> m_linkLocalDiagonalInertiaTensors;
|
|
||||||
|
|
||||||
//btAlignedObjectArray<int> m_parentIndices;//for root, it is identity
|
|
||||||
//btAlignedObjectArray<btVector3> m_jointAxisArray;
|
|
||||||
//btAlignedObjectArray<btTransform> m_jointOffsetInParent;
|
|
||||||
//btAlignedObjectArray<btTransform> m_jointOffsetInChild;
|
|
||||||
//btAlignedObjectArray<int> m_jointTypeArray;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
bool m_createMultiBody;
|
bool m_createMultiBody;
|
||||||
int m_totalNumJoints;
|
int m_totalNumJoints;
|
||||||
btMultiBody* m_bulletMultiBody;
|
btMultiBody* m_bulletMultiBody;
|
||||||
|
|
||||||
|
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
||||||
URDF2BulletMappings()
|
URDF2BulletMappings()
|
||||||
:m_createMultiBody(false),
|
:m_createMultiBody(false),
|
||||||
m_totalNumJoints(0),
|
m_totalNumJoints(0),
|
||||||
@@ -293,6 +509,7 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
|
|||||||
glmesh = LoadMeshFromObj(fullPath,pathPrefix);
|
glmesh = LoadMeshFromObj(fullPath,pathPrefix);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case FILE_STL:
|
case FILE_STL:
|
||||||
{
|
{
|
||||||
glmesh = LoadMeshFromSTL(fullPath);
|
glmesh = LoadMeshFromSTL(fullPath);
|
||||||
@@ -381,6 +598,8 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
|
|||||||
}
|
}
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
|
printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
|
||||||
|
btAssert(0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -390,7 +609,7 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
printf("issue extracting mesh from COLLADA/STL/obj file %s\n", fullPath);
|
printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
@@ -679,6 +898,8 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
|
|||||||
}
|
}
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
|
printf("Unsupported file type in Collision: %s\n",fullPath);
|
||||||
|
btAssert(0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -725,27 +946,30 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
|
|||||||
}
|
}
|
||||||
return shape;
|
return shape;
|
||||||
}
|
}
|
||||||
void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix, ImportUrdfInternalData* data)
|
void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
|
||||||
{
|
{
|
||||||
//btCollisionShape* shape = 0;
|
//btCollisionShape* shape = 0;
|
||||||
|
|
||||||
btTransform linkTransformInWorldSpace;
|
btTransform linkTransformInWorldSpace;
|
||||||
linkTransformInWorldSpace.setIdentity();
|
linkTransformInWorldSpace.setIdentity();
|
||||||
|
|
||||||
btScalar mass = 1;
|
btScalar mass = 0;
|
||||||
btTransform inertialFrame;
|
btTransform inertialFrame;
|
||||||
inertialFrame.setIdentity();
|
inertialFrame.setIdentity();
|
||||||
const Link* parentLink = (*link).getParent();
|
const Link* parentLink = (*link).getParent();
|
||||||
URDF_LinkInformation* pp = 0;
|
URDF_LinkInformation* pp = 0;
|
||||||
int linkIndex = mappings.m_linkMasses.size();
|
|
||||||
btVector3 localInertiaDiagonal(1,1,1);
|
int linkIndex = mappings.m_linkMasses.size();//assuming root == 0, child links use contiguous numbering > 0
|
||||||
|
|
||||||
|
btVector3 localInertiaDiagonal(0,0,0);
|
||||||
|
|
||||||
int parentIndex = -1;
|
int parentIndex = -1;
|
||||||
|
|
||||||
|
|
||||||
if (parentLink)
|
if (parentLink)
|
||||||
{
|
{
|
||||||
parentIndex = parentLink->m_link_index;
|
parentIndex = mappings.m_urdfLinkIndices2BulletLinkIndices[parentLink->m_link_index];
|
||||||
|
|
||||||
btAssert(parentIndex>=0);
|
btAssert(parentIndex>=0);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -759,7 +983,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
(*link).m_link_index = linkIndex;
|
mappings.m_urdfLinkIndices2BulletLinkIndices[(*link).m_link_index] = linkIndex;
|
||||||
|
|
||||||
if ((*link).inertial)
|
if ((*link).inertial)
|
||||||
{
|
{
|
||||||
@@ -868,13 +1092,11 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
|||||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame;
|
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame;
|
||||||
URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
|
URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
|
||||||
|
|
||||||
linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
|
|
||||||
|
|
||||||
if (!mappings.m_createMultiBody)
|
if (!mappings.m_createMultiBody)
|
||||||
{
|
{
|
||||||
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
|
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
|
||||||
rbci.m_startWorldTransform = inertialFrameInWorldSpace;
|
rbci.m_startWorldTransform = inertialFrameInWorldSpace;
|
||||||
|
linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
|
||||||
//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
|
//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
|
||||||
btRigidBody* body = new btRigidBody(rbci);
|
btRigidBody* body = new btRigidBody(rbci);
|
||||||
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
|
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
|
||||||
@@ -995,7 +1217,6 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
|||||||
{
|
{
|
||||||
if (mappings.m_createMultiBody)
|
if (mappings.m_createMultiBody)
|
||||||
{
|
{
|
||||||
printf("Revolute joint (btMultiBody)\n");
|
|
||||||
//todo: adjust the center of mass transform and pivot axis properly
|
//todo: adjust the center of mass transform and pivot axis properly
|
||||||
/*mappings.m_bulletMultiBody->setupRevolute(
|
/*mappings.m_bulletMultiBody->setupRevolute(
|
||||||
linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||||
@@ -1014,34 +1235,10 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
|||||||
disableParentCollision);
|
disableParentCollision);
|
||||||
//linkInfo->m_localVisualFrame.setIdentity();
|
//linkInfo->m_localVisualFrame.setIdentity();
|
||||||
|
|
||||||
{
|
|
||||||
if (data->m_numMotors<MAX_NUM_MOTORS)
|
|
||||||
{
|
|
||||||
const char* jointName = pj->name.c_str();
|
|
||||||
char motorName[1024];
|
|
||||||
sprintf(motorName,"%s q'", jointName);
|
|
||||||
|
|
||||||
btScalar* motorVel = &data->m_motorTargetVelocities[data->m_numMotors];
|
|
||||||
*motorVel = 0.f;
|
|
||||||
|
|
||||||
SliderParams slider(motorName,motorVel);
|
|
||||||
slider.m_minVal=-4;
|
|
||||||
slider.m_maxVal=4;
|
|
||||||
gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
|
|
||||||
btScalar maxMotorImpulse = 0.1f;
|
|
||||||
|
|
||||||
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mappings.m_bulletMultiBody,linkIndex-1,0,0,maxMotorImpulse);
|
|
||||||
data->m_jointMotors[data->m_numMotors]=motor;
|
|
||||||
world1->addMultiBodyConstraint(motor);
|
|
||||||
data->m_numMotors++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
//only handle principle axis at the moment,
|
//only handle principle axis at the moment,
|
||||||
//@todo(erwincoumans) orient the constraint for non-principal axis
|
//@todo(erwincoumans) orient the constraint for non-principal axis
|
||||||
printf("Hinge joint (btGeneric6DofSpring2Constraint)\n");
|
|
||||||
btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
|
btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
|
||||||
int principleAxis = axis.closestAxis();
|
int principleAxis = axis.closestAxis();
|
||||||
switch (principleAxis)
|
switch (principleAxis)
|
||||||
@@ -1094,7 +1291,11 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
|||||||
{
|
{
|
||||||
if (mappings.m_createMultiBody)
|
if (mappings.m_createMultiBody)
|
||||||
{
|
{
|
||||||
printf("Prismatic joint (btMultiBody)\n");
|
//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||||
|
// parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
|
||||||
|
|
||||||
|
//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||||
|
// parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
|
||||||
|
|
||||||
mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||||
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||||
@@ -1103,11 +1304,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
|
|
||||||
printf("Slider joint (btGeneric6DofSpring2Constraint)\n");
|
|
||||||
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
|
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
|
||||||
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
|
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
|
||||||
btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
|
btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
|
||||||
@@ -1211,7 +1410,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
|||||||
{
|
{
|
||||||
if (*child)
|
if (*child)
|
||||||
{
|
{
|
||||||
URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix, data);
|
URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix);
|
||||||
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@@ -1297,31 +1496,49 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
|||||||
|
|
||||||
// print entire tree
|
// print entire tree
|
||||||
printTree(root_link);
|
printTree(root_link);
|
||||||
|
printf("now using new interface\n");
|
||||||
|
std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
|
||||||
|
|
||||||
|
//now print the tree using the new interface
|
||||||
|
MyURDF2Bullet u2b(robot,gfxBridge);
|
||||||
|
printTree(u2b, 0,0);
|
||||||
|
|
||||||
btTransform identityTrans;
|
btTransform identityTrans;
|
||||||
identityTrans.setIdentity();
|
identityTrans.setIdentity();
|
||||||
|
|
||||||
int numJoints = (*robot).m_numJoints;
|
int numJoints = (*robot).m_numJoints;
|
||||||
|
|
||||||
static bool useFeatherstone = true;
|
static bool useFeatherstone = true;
|
||||||
|
bool useUrdfInterfaceClass = true;
|
||||||
|
|
||||||
{
|
{
|
||||||
URDF2BulletMappings mappings;
|
URDF2BulletMappings mappings;
|
||||||
|
|
||||||
|
btMultiBody* mb = 0;
|
||||||
|
|
||||||
|
if (!useUrdfInterfaceClass)
|
||||||
|
{
|
||||||
mappings.m_createMultiBody = useFeatherstone;
|
mappings.m_createMultiBody = useFeatherstone;
|
||||||
mappings.m_totalNumJoints = numJoints;
|
mappings.m_totalNumJoints = numJoints;
|
||||||
URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix,m_data);
|
mappings.m_urdfLinkIndices2BulletLinkIndices.resize(numJoints+1,-2);//root and child links (=1+numJoints)
|
||||||
|
URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
|
||||||
|
mb = mappings.m_bulletMultiBody;
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
|
||||||
|
|
||||||
|
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
|
||||||
|
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||||
|
printf("urdf root link index = %d\n",rootLinkIndex);
|
||||||
|
ConvertURDF2Bullet(u2b,identityTrans,m_dynamicsWorld,useFeatherstone,pathPrefix);
|
||||||
|
mb = u2b.getBulletMultiBody();
|
||||||
|
|
||||||
|
}
|
||||||
if (useFeatherstone)
|
if (useFeatherstone)
|
||||||
{
|
{
|
||||||
btMultiBody* mb = mappings.m_bulletMultiBody;
|
|
||||||
mb->setHasSelfCollision(false);
|
mb->setHasSelfCollision(false);
|
||||||
if (mb->isMultiDof())
|
|
||||||
{
|
|
||||||
mb->finalizeMultiDof();
|
mb->finalizeMultiDof();
|
||||||
}
|
|
||||||
m_dynamicsWorld->addMultiBody(mb);
|
m_dynamicsWorld->addMultiBody(mb);
|
||||||
|
|
||||||
for (int i=0;i<m_data->m_numMotors;i++)
|
|
||||||
{
|
|
||||||
m_data->m_jointMotors[i]->finalizeMultiDof();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1341,7 +1558,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
|||||||
gfxBridge.createCollisionShapeGraphicsObject(box);
|
gfxBridge.createCollisionShapeGraphicsObject(box);
|
||||||
btTransform start; start.setIdentity();
|
btTransform start; start.setIdentity();
|
||||||
btVector3 groundOrigin(0,0,0);
|
btVector3 groundOrigin(0,0,0);
|
||||||
groundOrigin[upAxis]=-2.;
|
groundOrigin[upAxis]=-2;//.5;
|
||||||
start.setOrigin(groundOrigin);
|
start.setOrigin(groundOrigin);
|
||||||
btRigidBody* body = createRigidBody(0,start,box);
|
btRigidBody* body = createRigidBody(0,start,box);
|
||||||
//m_dynamicsWorld->removeRigidBody(body);
|
//m_dynamicsWorld->removeRigidBody(body);
|
||||||
@@ -1358,14 +1575,7 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
|
|||||||
{
|
{
|
||||||
if (m_dynamicsWorld)
|
if (m_dynamicsWorld)
|
||||||
{
|
{
|
||||||
//set the new target velocities
|
|
||||||
for (int i=0;i<m_data->m_numMotors;i++)
|
|
||||||
{
|
|
||||||
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
|
|
||||||
}
|
|
||||||
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
|
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
|
||||||
int actualSteps = m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
|
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
|
||||||
//int actualSteps = m_dynamicsWorld->stepSimulation(deltaTime,1000,1./3000.f);//240.);
|
|
||||||
//printf("deltaTime %f took actualSteps = %d\n",deltaTime,actualSteps);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -7,7 +7,6 @@
|
|||||||
class ImportUrdfSetup : public CommonMultiBodySetup
|
class ImportUrdfSetup : public CommonMultiBodySetup
|
||||||
{
|
{
|
||||||
char m_fileName[1024];
|
char m_fileName[1024];
|
||||||
struct ImportUrdfInternalData* m_data;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ImportUrdfSetup();
|
ImportUrdfSetup();
|
||||||
|
|||||||
505
Demos3/ImportURDFDemo/URDF2Bullet.cpp
Normal file
505
Demos3/ImportURDFDemo/URDF2Bullet.cpp
Normal file
@@ -0,0 +1,505 @@
|
|||||||
|
#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);
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
} 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);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
} 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);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
68
Demos3/ImportURDFDemo/URDF2Bullet.h
Normal file
68
Demos3/ImportURDFDemo/URDF2Bullet.h
Normal file
@@ -0,0 +1,68 @@
|
|||||||
|
#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;
|
||||||
|
|
||||||
|
//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;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
@@ -58,7 +58,7 @@ class btRigidBody;
|
|||||||
|
|
||||||
enum RotateOrder
|
enum RotateOrder
|
||||||
{
|
{
|
||||||
RO_XYZ,
|
RO_XYZ=0,
|
||||||
RO_XZY,
|
RO_XZY,
|
||||||
RO_YXZ,
|
RO_YXZ,
|
||||||
RO_YZX,
|
RO_YZX,
|
||||||
|
|||||||
Reference in New Issue
Block a user