mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 05:10:08 +00:00
refactor of URDF importer (work-in-progress)
This commit is contained in:
erwin coumans
parent
f2aef6b73e
commit
b64f5feba4
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,29 @@
|
||||
#ifndef MULTIBODY_CREATION_INTERFACE_H
|
||||
#define MULTIBODY_CREATION_INTERFACE_H
|
||||
|
||||
#include "LinearMath/btTransform.h"
|
||||
|
||||
class MultiBodyCreationInterface
|
||||
{
|
||||
public:
|
||||
|
||||
virtual ~MultiBodyCreationInterface() {}
|
||||
|
||||
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 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) = 0;
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) =0;
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0;
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
|
||||
|
||||
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
|
||||
};
|
||||
|
||||
#endif //MULTIBODY_CREATION_INTERFACE_H
|
||||
71
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
Normal file
71
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
Normal file
@ -0,0 +1,71 @@
|
||||
#include "MyMultiBodyCreator.h"
|
||||
|
||||
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
|
||||
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
|
||||
|
||||
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
|
||||
:m_guiHelper(guiHelper),
|
||||
m_bulletMultiBody(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)
|
||||
{
|
||||
m_urdf2mbLink.resize(totalNumJoints+1,-2);
|
||||
m_mb2urdfLink.resize(totalNumJoints+1,-2);
|
||||
|
||||
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
|
||||
return m_bulletMultiBody;
|
||||
}
|
||||
|
||||
class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
|
||||
{
|
||||
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
||||
rbci.m_startWorldTransform = initialWorldTrans;
|
||||
btRigidBody* body = new btRigidBody(rbci);
|
||||
return body;
|
||||
}
|
||||
|
||||
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
|
||||
{
|
||||
btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
|
||||
return mbCol;
|
||||
}
|
||||
|
||||
|
||||
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
|
||||
return c;
|
||||
}
|
||||
|
||||
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
|
||||
{
|
||||
m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
|
||||
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
|
||||
}
|
||||
|
||||
void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
|
||||
{
|
||||
|
||||
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
|
||||
}
|
||||
|
||||
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
|
||||
{
|
||||
m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
|
||||
}
|
||||
|
||||
btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
|
||||
{
|
||||
return m_bulletMultiBody;
|
||||
}
|
||||
48
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
Normal file
48
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
Normal file
@ -0,0 +1,48 @@
|
||||
|
||||
#ifndef MY_MULTIBODY_CREATOR
|
||||
#define MY_MULTIBODY_CREATOR
|
||||
|
||||
#include "MultiBodyCreationInterface.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
class GUIHelperInterface;
|
||||
class btMultiBody;
|
||||
|
||||
class MyMultiBodyCreator : public MultiBodyCreationInterface
|
||||
{
|
||||
|
||||
btMultiBody* m_bulletMultiBody;
|
||||
|
||||
struct GUIHelperInterface* m_guiHelper;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btAlignedObjectArray<int> m_urdf2mbLink;
|
||||
btAlignedObjectArray<int> m_mb2urdfLink;
|
||||
|
||||
|
||||
MyMultiBodyCreator(GUIHelperInterface* guiHelper);
|
||||
|
||||
virtual ~MyMultiBodyCreator() {}
|
||||
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) ;
|
||||
|
||||
///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);
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof);
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0);
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
|
||||
|
||||
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
|
||||
|
||||
btMultiBody* getBulletMultiBody();
|
||||
};
|
||||
|
||||
#endif //MY_MULTIBODY_CREATOR
|
||||
762
examples/Importers/ImportURDFDemo/MyURDFImporter.cpp
Normal file
762
examples/Importers/ImportURDFDemo/MyURDFImporter.cpp
Normal file
@ -0,0 +1,762 @@
|
||||
#include "MyURDFImporter.h"
|
||||
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
#include "btBulletCollisionCommon.h"
|
||||
#include "../ImportObjDemo/LoadMeshFromObj.h"
|
||||
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
|
||||
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
|
||||
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
|
||||
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
|
||||
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);
|
||||
|
||||
|
||||
struct MyURDFInternalData
|
||||
{
|
||||
my_shared_ptr<ModelInterface> m_robot;
|
||||
std::vector<my_shared_ptr<Link> > m_links;
|
||||
struct GUIHelperInterface* m_guiHelper;
|
||||
|
||||
};
|
||||
|
||||
enum MyFileType
|
||||
{
|
||||
FILE_STL=1,
|
||||
FILE_COLLADA=2,
|
||||
FILE_OBJ=3,
|
||||
};
|
||||
|
||||
|
||||
|
||||
MyURDFImporter::MyURDFImporter(my_shared_ptr<ModelInterface> robot,struct GUIHelperInterface* helper)
|
||||
{
|
||||
m_data = new MyURDFInternalData;
|
||||
m_data->m_robot = robot;
|
||||
m_data->m_guiHelper = helper;
|
||||
m_data->m_robot->getLinks(m_data->m_links);
|
||||
|
||||
//initialize the 'index' of each link
|
||||
for (int i=0;i<m_data->m_links.size();i++)
|
||||
{
|
||||
m_data->m_links[i]->m_link_index = i;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
MyURDFImporter::~MyURDFImporter()
|
||||
{
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
|
||||
int MyURDFImporter::getRootLinkIndex() const
|
||||
{
|
||||
if (m_data->m_links.size())
|
||||
{
|
||||
int rootLinkIndex = m_data->m_robot->getRoot()->m_link_index;
|
||||
// btAssert(m_links[0]->m_link_index == rootLinkIndex);
|
||||
return rootLinkIndex;
|
||||
}
|
||||
return -1;
|
||||
};
|
||||
|
||||
void MyURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
|
||||
{
|
||||
childLinkIndices.resize(0);
|
||||
int numChildren = m_data->m_links[linkIndex]->child_links.size();
|
||||
|
||||
for (int i=0;i<numChildren;i++)
|
||||
{
|
||||
int childIndex =m_data->m_links[linkIndex]->child_links[i]->m_link_index;
|
||||
childLinkIndices.push_back(childIndex);
|
||||
}
|
||||
}
|
||||
|
||||
std::string MyURDFImporter::getLinkName(int linkIndex) const
|
||||
{
|
||||
std::string n = m_data->m_links[linkIndex]->name;
|
||||
return n;
|
||||
}
|
||||
|
||||
std::string MyURDFImporter::getJointName(int linkIndex) const
|
||||
{
|
||||
return m_data->m_links[linkIndex]->parent_joint->name;
|
||||
}
|
||||
|
||||
|
||||
void MyURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
|
||||
{
|
||||
if ((*m_data->m_links[linkIndex]).inertial)
|
||||
{
|
||||
mass = (*m_data->m_links[linkIndex]).inertial->mass;
|
||||
localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz);
|
||||
inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z));
|
||||
inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w));
|
||||
} else
|
||||
{
|
||||
mass = 1.f;
|
||||
localInertiaDiagonal.setValue(1,1,1);
|
||||
inertialFrame.setIdentity();
|
||||
}
|
||||
}
|
||||
|
||||
bool MyURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
|
||||
{
|
||||
jointLowerLimit = 0.f;
|
||||
jointUpperLimit = 0.f;
|
||||
|
||||
if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
|
||||
{
|
||||
my_shared_ptr<Joint> pj =(*m_data->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 = URDFRevoluteJoint;
|
||||
break;
|
||||
case Joint::FIXED:
|
||||
jointType = URDFFixedJoint;
|
||||
break;
|
||||
case Joint::PRISMATIC:
|
||||
jointType = URDFPrismaticJoint;
|
||||
break;
|
||||
case Joint::PLANAR:
|
||||
jointType = URDFPlanarJoint;
|
||||
break;
|
||||
case Joint::CONTINUOUS:
|
||||
jointType = URDFContinuousJoint;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
|
||||
{
|
||||
|
||||
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
btConvexShape* convexColShape = 0;
|
||||
|
||||
switch (visual->geometry->type)
|
||||
{
|
||||
case Geometry::CYLINDER:
|
||||
{
|
||||
printf("processing a cylinder\n");
|
||||
urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
|
||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||
int numSteps = 32;
|
||||
for (int i = 0; i<numSteps; i++)
|
||||
{
|
||||
|
||||
btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.);
|
||||
vertices.push_back(vert);
|
||||
vert[2] = -cyl->length / 2.;
|
||||
vertices.push_back(vert);
|
||||
}
|
||||
|
||||
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
|
||||
cylZShape->setMargin(0.001);
|
||||
convexColShape = cylZShape;
|
||||
break;
|
||||
}
|
||||
case Geometry::BOX:
|
||||
{
|
||||
printf("processing a box\n");
|
||||
urdf::Box* box = (urdf::Box*)visual->geometry.get();
|
||||
btVector3 extents(box->dim.x, box->dim.y, box->dim.z);
|
||||
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
|
||||
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
|
||||
convexColShape = boxShape;
|
||||
convexColShape->setMargin(0.001);
|
||||
break;
|
||||
}
|
||||
case Geometry::SPHERE:
|
||||
{
|
||||
printf("processing a sphere\n");
|
||||
urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
|
||||
btScalar radius = sphere->radius;
|
||||
btSphereShape* sphereShape = new btSphereShape(radius);
|
||||
convexColShape = sphereShape;
|
||||
convexColShape->setMargin(0.001);
|
||||
break;
|
||||
|
||||
break;
|
||||
}
|
||||
case Geometry::MESH:
|
||||
{
|
||||
if (visual->name.length())
|
||||
{
|
||||
printf("visual->name=%s\n", visual->name.c_str());
|
||||
}
|
||||
if (visual->geometry)
|
||||
{
|
||||
const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
|
||||
if (mesh->filename.length())
|
||||
{
|
||||
const char* filename = mesh->filename.c_str();
|
||||
printf("mesh->filename=%s\n", filename);
|
||||
char fullPath[1024];
|
||||
int fileType = 0;
|
||||
sprintf(fullPath, "%s%s", pathPrefix, filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
if (strstr(fullPath, ".dae"))
|
||||
{
|
||||
fileType = FILE_COLLADA;
|
||||
}
|
||||
if (strstr(fullPath, ".stl"))
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
|
||||
sprintf(fullPath, "%s%s", pathPrefix, filename);
|
||||
FILE* f = fopen(fullPath, "rb");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
|
||||
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath,pathPrefix);
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
break;
|
||||
}
|
||||
case FILE_COLLADA:
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans; upAxisTrans.setIdentity();
|
||||
float unitMeterScaling = 1;
|
||||
int upAxis = 2;
|
||||
|
||||
LoadMeshFromCollada(fullPath,
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
unitMeterScaling,
|
||||
upAxis);
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
int index = 0;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
for (int i = 0; i<visualShapeInstances.size(); i++)
|
||||
{
|
||||
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
||||
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
||||
|
||||
b3AlignedObjectArray<GLInstanceVertex> verts;
|
||||
verts.resize(gfxShape->m_vertices->size());
|
||||
|
||||
int baseIndex = glmesh->m_vertices->size();
|
||||
|
||||
for (int i = 0; i<gfxShape->m_vertices->size(); i++)
|
||||
{
|
||||
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
|
||||
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
|
||||
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||
|
||||
}
|
||||
|
||||
int curNumIndices = glmesh->m_indices->size();
|
||||
int additionalIndices = gfxShape->m_indices->size();
|
||||
glmesh->m_indices->resize(curNumIndices + additionalIndices);
|
||||
for (int k = 0; k<additionalIndices; k++)
|
||||
{
|
||||
glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
|
||||
}
|
||||
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setIdentity();
|
||||
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
|
||||
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
|
||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||
int curNumVertices = glmesh->m_vertices->size();
|
||||
int additionalVertices = verts.size();
|
||||
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
|
||||
|
||||
for (int v = 0; v<verts.size(); v++)
|
||||
{
|
||||
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
|
||||
pos = worldMat*pos;
|
||||
verts[v].xyzw[0] = float(pos[0]);
|
||||
verts[v].xyzw[1] = float(pos[1]);
|
||||
verts[v].xyzw[2] = float(pos[2]);
|
||||
glmesh->m_vertices->push_back(verts[v]);
|
||||
}
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(fullPath);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (glmesh && (glmesh->m_numvertices>0))
|
||||
{
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("mesh geometry not found %s\n", fullPath);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Error: unknown visual geometry type\n");
|
||||
}
|
||||
}
|
||||
|
||||
//if we have a convex, tesselate into localVertices/localIndices
|
||||
if (convexColShape)
|
||||
{
|
||||
btShapeHull* hull = new btShapeHull(convexColShape);
|
||||
hull->buildHull(0.0);
|
||||
{
|
||||
// int strideInBytes = 9*sizeof(float);
|
||||
int numVertices = hull->numVertices();
|
||||
int numIndices = hull->numIndices();
|
||||
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
int index = 0;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
|
||||
for (int i = 0; i < numVertices; i++)
|
||||
{
|
||||
GLInstanceVertex vtx;
|
||||
btVector3 pos = hull->getVertexPointer()[i];
|
||||
vtx.xyzw[0] = pos.x();
|
||||
vtx.xyzw[1] = pos.y();
|
||||
vtx.xyzw[2] = pos.z();
|
||||
vtx.xyzw[3] = 1.f;
|
||||
pos.normalize();
|
||||
vtx.normal[0] = pos.x();
|
||||
vtx.normal[1] = pos.y();
|
||||
vtx.normal[2] = pos.z();
|
||||
vtx.uv[0] = 0.5f;
|
||||
vtx.uv[1] = 0.5f;
|
||||
glmesh->m_vertices->push_back(vtx);
|
||||
}
|
||||
|
||||
btAlignedObjectArray<int> indices;
|
||||
for (int i = 0; i < numIndices; i++)
|
||||
{
|
||||
glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
|
||||
}
|
||||
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
}
|
||||
delete convexColShape;
|
||||
convexColShape = 0;
|
||||
|
||||
}
|
||||
|
||||
if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
|
||||
{
|
||||
|
||||
int baseIndex = verticesOut.size();
|
||||
|
||||
|
||||
|
||||
for (int i = 0; i < glmesh->m_indices->size(); i++)
|
||||
{
|
||||
indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
|
||||
}
|
||||
|
||||
for (int i = 0; i < glmesh->m_vertices->size(); i++)
|
||||
{
|
||||
GLInstanceVertex& v = glmesh->m_vertices->at(i);
|
||||
btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
|
||||
btVector3 vt = visualTransform*vert;
|
||||
v.xyzw[0] = vt[0];
|
||||
v.xyzw[1] = vt[1];
|
||||
v.xyzw[2] = vt[2];
|
||||
btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
|
||||
triNormal = visualTransform.getBasis()*triNormal;
|
||||
v.normal[0] = triNormal[0];
|
||||
v.normal[1] = triNormal[1];
|
||||
v.normal[2] = triNormal[2];
|
||||
verticesOut.push_back(v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix)
|
||||
{
|
||||
btCollisionShape* shape = 0;
|
||||
|
||||
switch (visual->geometry->type)
|
||||
{
|
||||
case Geometry::CYLINDER:
|
||||
{
|
||||
printf("processing a cylinder\n");
|
||||
urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
|
||||
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||
int numSteps = 32;
|
||||
for (int i=0;i<numSteps;i++)
|
||||
{
|
||||
|
||||
btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
|
||||
vertices.push_back(vert);
|
||||
vert[2] = -cyl->length/2.;
|
||||
vertices.push_back(vert);
|
||||
|
||||
}
|
||||
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
|
||||
cylZShape->setMargin(0.001);
|
||||
cylZShape->initializePolyhedralFeatures();
|
||||
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
|
||||
|
||||
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
|
||||
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
|
||||
|
||||
|
||||
shape = cylZShape;
|
||||
break;
|
||||
}
|
||||
case Geometry::BOX:
|
||||
{
|
||||
printf("processing a box\n");
|
||||
urdf::Box* box = (urdf::Box*)visual->geometry.get();
|
||||
btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
|
||||
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
|
||||
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
|
||||
shape = boxShape;
|
||||
shape ->setMargin(0.001);
|
||||
break;
|
||||
}
|
||||
case Geometry::SPHERE:
|
||||
{
|
||||
printf("processing a sphere\n");
|
||||
urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
|
||||
btScalar radius = sphere->radius;
|
||||
btSphereShape* sphereShape = new btSphereShape(radius);
|
||||
shape = sphereShape;
|
||||
shape ->setMargin(0.001);
|
||||
break;
|
||||
|
||||
break;
|
||||
}
|
||||
case Geometry::MESH:
|
||||
{
|
||||
if (visual->name.length())
|
||||
{
|
||||
printf("visual->name=%s\n",visual->name.c_str());
|
||||
}
|
||||
if (visual->geometry)
|
||||
{
|
||||
const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
|
||||
if (mesh->filename.length())
|
||||
{
|
||||
const char* filename = mesh->filename.c_str();
|
||||
printf("mesh->filename=%s\n",filename);
|
||||
char fullPath[1024];
|
||||
int fileType = 0;
|
||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
if (strstr(fullPath,".dae"))
|
||||
{
|
||||
fileType = FILE_COLLADA;
|
||||
}
|
||||
if (strstr(fullPath,".stl"))
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
||||
FILE* f = fopen(fullPath,"rb");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath,pathPrefix);
|
||||
break;
|
||||
}
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
break;
|
||||
}
|
||||
case FILE_COLLADA:
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans;upAxisTrans.setIdentity();
|
||||
float unitMeterScaling=1;
|
||||
int upAxis = 2;
|
||||
LoadMeshFromCollada(fullPath,
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
unitMeterScaling,
|
||||
upAxis );
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
int index = 0;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
for (int i=0;i<visualShapeInstances.size();i++)
|
||||
{
|
||||
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
||||
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
||||
|
||||
b3AlignedObjectArray<GLInstanceVertex> verts;
|
||||
verts.resize(gfxShape->m_vertices->size());
|
||||
|
||||
int baseIndex = glmesh->m_vertices->size();
|
||||
|
||||
for (int i=0;i<gfxShape->m_vertices->size();i++)
|
||||
{
|
||||
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
|
||||
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
|
||||
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||
|
||||
}
|
||||
|
||||
int curNumIndices = glmesh->m_indices->size();
|
||||
int additionalIndices = gfxShape->m_indices->size();
|
||||
glmesh->m_indices->resize(curNumIndices+additionalIndices);
|
||||
for (int k=0;k<additionalIndices;k++)
|
||||
{
|
||||
glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
|
||||
}
|
||||
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
|
||||
btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
|
||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||
int curNumVertices = glmesh->m_vertices->size();
|
||||
int additionalVertices = verts.size();
|
||||
glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
|
||||
|
||||
for(int v=0;v<verts.size();v++)
|
||||
{
|
||||
btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
|
||||
pos = worldMat*pos;
|
||||
verts[v].xyzw[0] = float(pos[0]);
|
||||
verts[v].xyzw[1] = float(pos[1]);
|
||||
verts[v].xyzw[2] = float(pos[2]);
|
||||
glmesh->m_vertices->push_back(verts[v]);
|
||||
}
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(fullPath);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Unsupported file type in Collision: %s\n",fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (glmesh && (glmesh->m_numvertices>0))
|
||||
{
|
||||
printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
|
||||
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
|
||||
//convex->setUserIndex(shapeId);
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0;i<glmesh->m_numvertices;i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2]));
|
||||
}
|
||||
//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
|
||||
btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
//cylZShape->initializePolyhedralFeatures();
|
||||
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
|
||||
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
|
||||
cylZShape->setMargin(0.001);
|
||||
shape = cylZShape;
|
||||
} else
|
||||
{
|
||||
printf("issue extracting mesh from STL file %s\n", fullPath);
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
printf("mesh geometry not found %s\n",fullPath);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Error: unknown visual geometry type\n");
|
||||
}
|
||||
}
|
||||
return shape;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int MyURDFImporter::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_data->m_links[linkIndex]->visual_array.size(); v++)
|
||||
{
|
||||
const Visual* vis = m_data->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_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
|
||||
}
|
||||
|
||||
return graphicsIndex;
|
||||
|
||||
}
|
||||
|
||||
class btCompoundShape* MyURDFImporter::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_data->m_links[linkIndex]->collision_array.size();v++)
|
||||
{
|
||||
const Collision* col = m_data->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;
|
||||
}
|
||||
41
examples/Importers/ImportURDFDemo/MyURDFImporter.h
Normal file
41
examples/Importers/ImportURDFDemo/MyURDFImporter.h
Normal file
@ -0,0 +1,41 @@
|
||||
#ifndef MY_URDF_IMPORTER_H
|
||||
#define MY_URDF_IMPORTER_H
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
#include <vector> //temp, replace by btAlignedObjectArray
|
||||
|
||||
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
|
||||
|
||||
class MyURDFImporter : public URDFImporterInterface
|
||||
{
|
||||
|
||||
struct MyURDFInternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
|
||||
MyURDFImporter(my_shared_ptr<urdf::ModelInterface> robot,struct GUIHelperInterface* helper);
|
||||
|
||||
virtual ~MyURDFImporter();
|
||||
|
||||
virtual int getRootLinkIndex() const;
|
||||
|
||||
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
|
||||
|
||||
virtual std::string getLinkName(int linkIndex) const;
|
||||
|
||||
virtual std::string getJointName(int linkIndex) const;
|
||||
|
||||
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
|
||||
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //MY_URDF_IMPORTER_H
|
||||
@ -1,4 +1,4 @@
|
||||
#include "URDF2Bullet.h"
|
||||
#include "URDFImporterInterface.h"
|
||||
#include <stdio.h>
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||
@ -6,7 +6,9 @@
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
#include "MultiBodyCreationInterface.h"
|
||||
#include <string>
|
||||
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
|
||||
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
|
||||
static bool enableConstraints = true;
|
||||
@ -30,7 +32,7 @@ static btVector3 selectColor2()
|
||||
return color;
|
||||
}
|
||||
|
||||
void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
|
||||
void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex,childIndices);
|
||||
@ -50,6 +52,7 @@ void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
struct URDF2BulletCachedData
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
@ -104,7 +107,7 @@ struct URDF2BulletCachedData
|
||||
|
||||
};
|
||||
|
||||
void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex)
|
||||
void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex,childIndices);
|
||||
@ -121,7 +124,7 @@ void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
}
|
||||
}
|
||||
|
||||
void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
|
||||
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
|
||||
{
|
||||
cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
|
||||
@ -134,7 +137,7 @@ void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache,
|
||||
}
|
||||
}
|
||||
|
||||
void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
|
||||
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
|
||||
{
|
||||
//compute the number of links, and compute parent indices array (and possibly other cached data?)
|
||||
cache.m_totalNumJoints1 = 0;
|
||||
@ -156,7 +159,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
|
||||
|
||||
}
|
||||
|
||||
void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
{
|
||||
printf("start converting/extracting data from URDF interface\n");
|
||||
|
||||
@ -241,14 +244,14 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
linkRigidBody = body;
|
||||
|
||||
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
|
||||
creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
|
||||
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
} else
|
||||
{
|
||||
@ -258,7 +261,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
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.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
|
||||
|
||||
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
}
|
||||
@ -275,7 +278,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
bool disableParentCollision = true;
|
||||
switch (jointType)
|
||||
{
|
||||
case URDF2Bullet::FixedJoint:
|
||||
case URDFFixedJoint:
|
||||
{
|
||||
if (createMultiBody)
|
||||
{
|
||||
@ -285,7 +288,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
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);
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
btMatrix3x3 rm(rot);
|
||||
btScalar y,p,r;
|
||||
@ -302,7 +305,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
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);
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
@ -315,8 +318,8 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDF2Bullet::ContinuousJoint:
|
||||
case URDF2Bullet::RevoluteJoint:
|
||||
case URDFContinuousJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
if (createMultiBody)
|
||||
{
|
||||
@ -326,7 +329,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
} else
|
||||
{
|
||||
@ -337,7 +340,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
@ -350,7 +353,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
@ -364,7 +367,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
@ -379,7 +382,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDF2Bullet::PrismaticJoint:
|
||||
case URDFPrismaticJoint:
|
||||
{
|
||||
if (createMultiBody)
|
||||
{
|
||||
@ -389,11 +392,11 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
} else
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.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)
|
||||
@ -440,7 +443,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
{
|
||||
if (compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
@ -462,7 +465,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
|
||||
btVector3 color = selectColor2();//(0.0,0.0,0.5);
|
||||
|
||||
u2b.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
|
||||
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
|
||||
|
||||
btScalar friction = 0.5f;
|
||||
|
||||
@ -489,18 +492,18 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
{
|
||||
URDF2BulletCachedData cache;
|
||||
|
||||
InitURDF2BulletCache(u2b,cache);
|
||||
int urdfLinkIndex = u2b.getRootLinkIndex();
|
||||
ConvertURDF2BulletInternal(u2b, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -8,64 +8,21 @@ 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;
|
||||
};
|
||||
class URDFImporterInterface;
|
||||
class MultiBodyCreationInterface;
|
||||
|
||||
|
||||
void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0);
|
||||
void printTree(const URDFImporterInterface& u2b, int linkIndex, int identationLevel=0);
|
||||
|
||||
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix);
|
||||
|
||||
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
|
||||
MultiBodyCreationInterface& creationCallback,
|
||||
const btTransform& rootTransformInWorldSpace,
|
||||
btMultiBodyDynamicsWorld* world,
|
||||
bool createMultiBody,
|
||||
const char* pathPrefix);
|
||||
|
||||
#endif //_URDF2BULLET_H
|
||||
|
||||
|
||||
41
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
Normal file
41
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
Normal file
@ -0,0 +1,41 @@
|
||||
#ifndef URDF_IMPORTER_INTERFACE_H
|
||||
#define URDF_IMPORTER_INTERFACE_H
|
||||
|
||||
#include <string>
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "URDFJointTypes.h"
|
||||
|
||||
class URDFImporterInterface
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
|
||||
virtual ~URDFImporterInterface() {}
|
||||
|
||||
|
||||
///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;
|
||||
|
||||
///quick hack: need to rethink the API/dependencies of this
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0;
|
||||
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
|
||||
};
|
||||
|
||||
#endif //URDF_IMPORTER_INTERFACE_H
|
||||
|
||||
16
examples/Importers/ImportURDFDemo/URDFJointTypes.h
Normal file
16
examples/Importers/ImportURDFDemo/URDFJointTypes.h
Normal file
@ -0,0 +1,16 @@
|
||||
#ifndef URDF_JOINT_TYPES_H
|
||||
#define URDF_JOINT_TYPES_H
|
||||
|
||||
|
||||
enum
|
||||
{
|
||||
URDFRevoluteJoint=1,
|
||||
URDFPrismaticJoint,
|
||||
URDFContinuousJoint,
|
||||
URDFFloatingJoint,
|
||||
URDFPlanarJoint,
|
||||
URDFFixedJoint,
|
||||
};
|
||||
|
||||
|
||||
#endif //URDF_JOINT_TYPES_H
|
||||
Loading…
Reference in New Issue
Block a user