fix some btMultiBody URDF conversion issues in ImportURDFSetup
remove various vertex format structures and use GLInstanceVertex from #include "OpenGLWindow/GLInstanceGraphicsShape.h" btMultiBody::setupPrismatic takes an additional argument to allow a shift of inertia tensor, relative to the joint frame (link frame at q=0)
This commit is contained in:
=
@@ -6,6 +6,7 @@
|
||||
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
|
||||
|
||||
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
|
||||
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
|
||||
@@ -154,8 +155,295 @@ enum MyFileType
|
||||
FILE_COLLADA=2
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix)
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
sprintf(fullPath, "%s%s", pathPrefix, filename);
|
||||
FILE* f = fopen(fullPath, "rb");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
|
||||
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
break;
|
||||
}
|
||||
case FILE_COLLADA:
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans; upAxisTrans.setIdentity();
|
||||
float unitMeterScaling = 1;
|
||||
|
||||
LoadMeshFromCollada(fullPath,
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
unitMeterScaling);
|
||||
|
||||
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*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:
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
|
||||
@@ -179,12 +467,13 @@ btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathP
|
||||
|
||||
}
|
||||
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
|
||||
cylZShape->initializePolyhedralFeatures();
|
||||
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);
|
||||
cylZShape->setMargin(0.001);
|
||||
|
||||
|
||||
shape = cylZShape;
|
||||
break;
|
||||
@@ -447,37 +736,45 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
|
||||
|
||||
{
|
||||
printf("converting visuals of link %s",link->name.c_str());
|
||||
|
||||
printf("converting visuals of link %s", link->name.c_str());
|
||||
|
||||
|
||||
|
||||
{
|
||||
|
||||
|
||||
{
|
||||
|
||||
|
||||
btCompoundShape* tmpGfxShape = new btCompoundShape();
|
||||
|
||||
for (int v=0;v<(int)link->visual_array.size();v++)
|
||||
btAlignedObjectArray<GLInstanceVertex> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btTransform startTrans; startTrans.setIdentity();
|
||||
int graphicsIndex = -1;
|
||||
|
||||
for (int v = 0; v < (int)link->visual_array.size(); v++)
|
||||
{
|
||||
const Visual* vis = link->visual_array[v].get();
|
||||
btCollisionShape* childShape = convertURDFToCollisionShape(vis,pathPrefix);
|
||||
if (childShape)
|
||||
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);
|
||||
if (1)//!mappings.m_createMultiBody)
|
||||
{
|
||||
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);
|
||||
if (!mappings.m_createMultiBody)
|
||||
{
|
||||
tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
|
||||
} else
|
||||
{
|
||||
tmpGfxShape->addChildShape(childTrans,childShape);
|
||||
}
|
||||
convertURDFToVisualShape(vis, pathPrefix, childTrans*inertialFrame.inverse(), vertices, indices);
|
||||
}
|
||||
else
|
||||
{
|
||||
convertURDFToVisualShape(vis, pathPrefix, childTrans, vertices, indices);
|
||||
}
|
||||
}
|
||||
|
||||
if (vertices.size() && indices.size())
|
||||
{
|
||||
graphicsIndex = gfxBridge.registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
|
||||
}
|
||||
|
||||
btCompoundShape* compoundShape = new btCompoundShape();
|
||||
compoundShape->setMargin(0.001);
|
||||
for (int v=0;v<(int)link->collision_array.size();v++)
|
||||
{
|
||||
const Collision* col = link->collision_array[v].get();
|
||||
@@ -489,7 +786,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
btTransform childTrans;
|
||||
childTrans.setOrigin(childPos);
|
||||
childTrans.setRotation(childOrn);
|
||||
if (!mappings.m_createMultiBody)
|
||||
if (1)//!mappings.m_createMultiBody)
|
||||
{
|
||||
compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
|
||||
} else
|
||||
@@ -533,9 +830,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
|
||||
btRigidBody* body = new btRigidBody(rbci);
|
||||
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
|
||||
gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
|
||||
//hack-> transfer user inder from visual to collision shape
|
||||
compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
gfxBridge.createRigidBodyGraphicsObject(body, color);
|
||||
linkInfo->m_bulletRigidBody = body;
|
||||
@@ -596,10 +892,20 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
|
||||
printf("Fixed joint (btMultiBody)\n");
|
||||
//btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());
|
||||
btQuaternion rot = parent2joint.inverse().getRotation();
|
||||
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
|
||||
//toggle=!toggle;
|
||||
//mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
// rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
|
||||
mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
|
||||
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
|
||||
|
||||
/*
|
||||
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
*/
|
||||
|
||||
btMatrix3x3 rm(rot);
|
||||
btScalar y,p,r;
|
||||
rm.getEulerZYX(y,p,r);
|
||||
@@ -642,19 +948,23 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
if (mappings.m_createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
/*mappings.m_bulletMultiBody->setupRevolute(
|
||||
linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
|
||||
parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(),
|
||||
btVector3(0,0,0),//offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
/*
|
||||
*/
|
||||
|
||||
|
||||
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
//parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
linkInfo->m_localVisualFrame.setIdentity();
|
||||
*/
|
||||
//linkInfo->m_localVisualFrame.setIdentity();
|
||||
|
||||
} else
|
||||
{
|
||||
//only handle principle axis at the moment,
|
||||
@@ -711,24 +1021,50 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
{
|
||||
if (mappings.m_createMultiBody)
|
||||
{
|
||||
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->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
// parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(),
|
||||
// offsetInB.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,
|
||||
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
|
||||
dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
|
||||
|
||||
//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);
|
||||
int principleAxis = axis.closestAxis();
|
||||
switch (principleAxis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
dof6->setLinearLowerLimit(btVector3(0,pj->limits->lower,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,pj->limits->upper,0));
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,pj->limits->lower));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,pj->limits->upper));
|
||||
}
|
||||
};
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,0));
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,0));
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
|
||||
@@ -752,9 +1088,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
|
||||
//btCompoundShape* comp = new btCompoundShape();
|
||||
//comp->addChildShape(linkInfo->m_localVisualFrame,shape);
|
||||
|
||||
gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
|
||||
compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
col->setCollisionShape(compoundShape);
|
||||
|
||||
btTransform tr;
|
||||
@@ -826,6 +1162,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
gfxBridge.setUpAxis(2);
|
||||
|
||||
this->createEmptyDynamicsWorld();
|
||||
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
||||
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints
|
||||
@@ -833,8 +1170,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
|
||||
|
||||
|
||||
|
||||
btVector3 gravity(0,0,0);
|
||||
gravity[upAxis]=-9.8;
|
||||
|
||||
@@ -915,23 +1251,9 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
useFeatherstone = !useFeatherstone;
|
||||
printf("numJoints/DOFS = %d\n", numJoints);
|
||||
|
||||
if (0)
|
||||
bool createGround=true;
|
||||
if (createGround)
|
||||
{
|
||||
btVector3 halfExtents(1,1,1);
|
||||
btBoxShape* box = new btBoxShape(halfExtents);
|
||||
box->initializePolyhedralFeatures();
|
||||
|
||||
gfxBridge.createCollisionShapeGraphicsObject(box);
|
||||
btTransform start; start.setIdentity();
|
||||
btVector3 origin(0,0,0);
|
||||
origin[upAxis]=5;
|
||||
start.setOrigin(origin);
|
||||
btRigidBody* body = createRigidBody(1,start,box);
|
||||
btVector3 color(0.5,0.5,0.5);
|
||||
gfxBridge.createRigidBodyGraphicsObject(body,color);
|
||||
}
|
||||
|
||||
{
|
||||
btVector3 groundHalfExtents(20,20,20);
|
||||
groundHalfExtents[upAxis]=1.f;
|
||||
btBoxShape* box = new btBoxShape(groundHalfExtents);
|
||||
@@ -949,6 +1271,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
gfxBridge.createRigidBodyGraphicsObject(body,color);
|
||||
}
|
||||
|
||||
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
|
||||
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user