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

remove that odd triangle in the origin of samurai castle (VR)

Browse Source 
add rolling/spinning friction to cube, remove it from plane/samurai.urdf
URDF2Bullet: support joint limits for revolute and prismatic, only if defined (if upper < lower, disable limit)
add some profiling markers to improve performance
This commit is contained in:
erwin coumans
2016-09-19 07:02:43 -07:00
parent 48d42c7c6e
commit db3122233f
23 changed files with 297 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
#include "BulletUrdfImporter.h"
#include "../../CommonInterfaces/CommonRenderInterface.h"
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "URDFImporterInterface.h"
#include "btBulletCollisionCommon.h"
#include "../ImportObjDemo/LoadMeshFromObj.h"
@@ -360,7 +360,45 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true;
}
static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes)
{
btCompoundShape* compound = new btCompoundShape();
btTransform identity;
identity.setIdentity();
for (int s = 0; s<(int)shapes.size(); s++)
{
btConvexHullShape* convexHull = new btConvexHullShape();
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f = 0; f<faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(pt,false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(identity,convexHull);
}
return compound;
}
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
{
@@ -467,7 +505,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
{
case FILE_OBJ:
{
glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
//create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes);
return shape;
}
break;
}
case FILE_STL:

25
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -162,8 +162,6 @@ void ConvertURDF2BulletInternal(
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0;
//std::string name = u2b.getLinkName(urdfLinkIndex);
//b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
//b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
@@ -322,6 +320,14 @@ void ConvertURDF2BulletInternal(
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (jointLowerLimit <= jointUpperLimit)
{
//std::string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
}
} else
{
@@ -343,8 +349,14 @@ void ConvertURDF2BulletInternal(
-offsetInB.getOrigin(),
disableParentCollision);
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
if (jointLowerLimit <= jointUpperLimit)
{
//std::string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
}
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
} else
@@ -386,8 +398,9 @@ void ConvertURDF2BulletInternal(
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
col->setWorldTransform(tr);
bool isDynamic = true;
//base and fixed? -> static, otherwise flag as dynamic
bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true;
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);

2
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -816,7 +816,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
{
joint.m_lowerLimit = 0.f;
joint.m_upperLimit = 0.f;
joint.m_upperLimit = -1.f;
joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f;
joint.m_jointDamping = 0.f;

2
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -138,7 +138,7 @@ struct UrdfJoint
double m_jointFriction;
UrdfJoint()
:m_lowerLimit(0),
m_upperLimit(0),
m_upperLimit(-1),
m_effortLimit(0),
m_velocityLimit(0),
m_jointDamping(0),