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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-03-29 15:06:15 -07:00
parent 64573c38e4 30c1923b00
commit 66a8685816
77 changed files with 2213 additions and 1218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -365,12 +365,14 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
}
}
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
bool BulletURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
jointDamping = 0.f;
jointFriction = 0.f;
jointMaxForce = 0.f;
jointMaxVelocity = 0.f;
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
btAssert(linkPtr);
@@ -389,7 +391,8 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
jointUpperLimit = pj->m_upperLimit;
jointDamping = pj->m_jointDamping;
jointFriction = pj->m_jointFriction;
jointMaxForce = pj->m_effortLimit;
jointMaxVelocity = pj->m_velocityLimit;
return true;
} else
{
@@ -399,6 +402,14 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
}
return false;
};
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
btScalar jointMaxForce;
btScalar jointMaxVelocity;
return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
}
@@ -558,9 +569,9 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
case URDF_GEOM_CAPSULE:
{
btScalar radius = collision->m_geometry.m_capsuleRadius;
btScalar height = btScalar(2.f)*collision->m_geometry.m_capsuleHalfHeight;
btScalar height = collision->m_geometry.m_capsuleHeight;
btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius,height);
shape = capsuleShape;
shape = capsuleShape;
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
}
@@ -568,7 +579,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
case URDF_GEOM_CYLINDER:
{
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
btScalar cylLength = collision->m_geometry.m_capsuleHeight;
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
@@ -791,7 +802,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
{
btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight;
btScalar cylLength = visual->m_geometry.m_capsuleHeight;
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
vertices.push_back(vert);

3
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -48,7 +48,8 @@ public:
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;

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

@@ -232,9 +232,11 @@ void ConvertURDF2BulletInternal(
btScalar jointUpperLimit;
btScalar jointDamping;
btScalar jointFriction;
btScalar jointMaxForce;
btScalar jointMaxVelocity;
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
std::string linkName = u2b.getLinkName(urdfLinkIndex);
if (flags & CUF_USE_SDF)
@@ -344,6 +346,17 @@ void ConvertURDF2BulletInternal(
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
bool disableParentCollision = true;
if (createMultiBody && cache.m_bulletMultiBody)
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
}
switch (jointType)
{
case URDFFixedJoint:
@@ -374,8 +387,6 @@ void ConvertURDF2BulletInternal(
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
//std::string name = u2b.getLinkName(urdfLinkIndex);

8
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -46,6 +46,14 @@ public:
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
{
//backwards compatibility for custom file importers
jointMaxForce = 0;
jointMaxVelocity = 0;
return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
};
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;

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

@@ -403,7 +403,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
}
else if (type_name == "capsule")
@@ -417,7 +417,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length"));
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
}
else if (type_name == "mesh")
{
@@ -1679,7 +1679,17 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
std::string UrdfParser::sourceFileLocation(TiXmlElement* e)
{
#if 0
//no C++11 etc, no snprintf
char buf[1024];
snprintf(buf, sizeof(buf), "%s:%i", m_urdf2Model.m_sourceFile.c_str(), e->Row());
return buf;
#else
char row[1024];
sprintf(row,"%d",e->Row());
std::string str = m_urdf2Model.m_sourceFile.c_str() + std::string(":") + std::string(row);
return str;
#endif
}

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

@@ -65,7 +65,7 @@ struct UrdfGeometry
btVector3 m_boxSize;
double m_capsuleRadius;
double m_capsuleHalfHeight;
double m_capsuleHeight;
int m_hasFromTo;
btVector3 m_capsuleFrom;
btVector3 m_capsuleTo;