Create project file for BussIK inverse kinematics library (premake, cmake)
URDF/SDF: add a flag to force concave mesh collisiofor static objects. <collision concave="yes" name="pod_collision"> VR: support teleporting using buttong, allow multiple controllers to be used, fast wireframe rendering, Turn off warnings about deprecated C routine in btScalar.h/b3Scalar.h Add a dummy return to stop a warning Expose defaultContactERP in shared memory api/pybullet. First start to expose IK in shared memory api/pybullet (not working yet)
This commit is contained in:
@@ -577,13 +577,32 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[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;
|
||||
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
{
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
shape = trimesh;
|
||||
} else
|
||||
{
|
||||
//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
|
||||
{
|
||||
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
|
||||
|
||||
@@ -489,6 +489,9 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
|
||||
if (name_char)
|
||||
collision.m_name = name_char;
|
||||
|
||||
const char *concave_char = config->Attribute("concave");
|
||||
if (concave_char)
|
||||
collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -75,11 +75,22 @@ struct UrdfVisual
|
||||
UrdfMaterial m_localMaterial;
|
||||
};
|
||||
|
||||
enum UrdfCollisionFlags
|
||||
{
|
||||
URDF_FORCE_CONCAVE_TRIMESH=1,
|
||||
};
|
||||
|
||||
|
||||
struct UrdfCollision
|
||||
{
|
||||
btTransform m_linkLocalFrame;
|
||||
UrdfGeometry m_geometry;
|
||||
std::string m_name;
|
||||
int m_flags;
|
||||
UrdfCollision()
|
||||
:m_flags(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct UrdfJoint;
|
||||
|
||||
Reference in New Issue
Block a user