AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

Merge branch 'bulletphysics:master' into master

Browse Source
This commit is contained in:
jingyuc 2022-02-28 14:30:10 -08:00 committed by GitHub
commit ca9d9b3ee6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
30 changed files with 721 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Extras/obj2sdf/obj2sdf.cpp
View File

@ -30,13 +30,13 @@
#include "Bullet3Common/b3HashMap.h" #include "Bullet3Common/b3HashMap.h"
#include "../Utils/b3BulletDefaultFileIO.h" #include "../Utils/b3BulletDefaultFileIO.h"
using tinyobj::index_t; using bt_tinyobj::index_t;
struct ShapeContainer struct ShapeContainer
{ {
std::string m_matName; std::string m_matName;
std::string m_shapeName; std::string m_shapeName;
tinyobj::material_t material; bt_tinyobj::material_t material;
std::vector<float> positions; std::vector<float> positions;
std::vector<float> normals; std::vector<float> normals;
std::vector<float> texcoords; std::vector<float> texcoords;
@ -91,11 +91,11 @@ int main(int argc, char* argv[])
char materialPrefixPath[MAX_PATH_LEN]; char materialPrefixPath[MAX_PATH_LEN];
b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN); b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN);
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
b3BulletDefaultFileIO fileIO; b3BulletDefaultFileIO fileIO;
std::string err = tinyobj::LoadObj(attribute, shapes, fileNameWithPath, materialPrefixPath,&fileIO); std::string err = bt_tinyobj::LoadObj(attribute, shapes, fileNameWithPath, materialPrefixPath, &fileIO);
char sdfFileName[MAX_PATH_LEN]; char sdfFileName[MAX_PATH_LEN];
sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf"); sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf");
@ -110,8 +110,8 @@ int main(int argc, char* argv[])
for (int s = 0; s < (int)shapes.size(); s++) for (int s = 0; s < (int)shapes.size(); s++)
{ {
tinyobj::shape_t& shape = shapes[s]; bt_tinyobj::shape_t& shape = shapes[s];
tinyobj::material_t mat = shape.material; bt_tinyobj::material_t mat = shape.material;
b3HashString key = mat.name.length() ? mat.name.c_str() : ""; b3HashString key = mat.name.length() ? mat.name.c_str() : "";
if (!gMaterialNames.find(key)) if (!gMaterialNames.find(key))
@ -212,7 +212,7 @@ int main(int argc, char* argv[])
int faceCount = shapeCon->indices.size(); int faceCount = shapeCon->indices.size();
int vertexCount = shapeCon->positions.size(); int vertexCount = shapeCon->positions.size();
tinyobj::material_t mat = shapeCon->material; bt_tinyobj::material_t mat = shapeCon->material;
if (shapeCon->m_matName.length()) if (shapeCon->m_matName.length())
{ {
const char* objName = shapeCon->m_matName.c_str(); const char* objName = shapeCon->m_matName.c_str();
@ -317,7 +317,7 @@ int main(int argc, char* argv[])
{ {
for (int s = 0; s < (int)shapes.size(); s++) for (int s = 0; s < (int)shapes.size(); s++)
{ {
tinyobj::shape_t& shape = shapes[s]; bt_tinyobj::shape_t& shape = shapes[s];
if (shape.name.length()) if (shape.name.length())
{ {
@ -351,7 +351,7 @@ int main(int argc, char* argv[])
int faceCount = shape.mesh.indices.size(); int faceCount = shape.mesh.indices.size();
int vertexCount = attribute.vertices.size(); int vertexCount = attribute.vertices.size();
tinyobj::material_t mat = shape.material; bt_tinyobj::material_t mat = shape.material;
if (shape.name.length()) if (shape.name.length())
{ {
const char* objName = shape.name.c_str(); const char* objName = shape.name.c_str();

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

10
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@ -2273,7 +2273,7 @@ int BulletMJCFImporter::getBodyUniqueId() const
return m_data->m_activeBodyUniqueId; return m_data->m_activeBodyUniqueId;
} }
static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin) static btCollisionShape* MjcfCreateConvexHullFromShapes(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
{ {
btCompoundShape* compound = new btCompoundShape(); btCompoundShape* compound = new btCompoundShape();
compound->setMargin(collisionMargin); compound->setMargin(collisionMargin);
@ -2285,7 +2285,7 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t&
{ {
btConvexHullShape* convexHull = new btConvexHullShape(); btConvexHullShape* convexHull = new btConvexHullShape();
convexHull->setMargin(collisionMargin); convexHull->setMargin(collisionMargin);
tinyobj::shape_t& shape = shapes[s]; bt_tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size(); int faceCount = shape.mesh.indices.size();
@ -2399,9 +2399,9 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} }
else else
{ {
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO); std::string err = bt_tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape //create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin); childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);

6
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@ -64,8 +64,8 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
btVector3 shift(0, 0, 0); btVector3 shift(0, 0, 0);
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
{ {
B3_PROFILE("tinyobj::LoadObj"); B3_PROFILE("tinyobj::LoadObj");
std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO); std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO);
@ -79,7 +79,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
//try to load some texture //try to load some texture
for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++) for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++)
{ {
const tinyobj::shape_t& shape = shapes[i]; const bt_tinyobj::shape_t& shape = shapes[i];
meshData.m_rgbaColor[0] = shape.material.diffuse[0]; meshData.m_rgbaColor[0] = shape.material.diffuse[0];
meshData.m_rgbaColor[1] = shape.material.diffuse[1]; meshData.m_rgbaColor[1] = shape.material.diffuse[1];
meshData.m_rgbaColor[2] = shape.material.diffuse[2]; meshData.m_rgbaColor[2] = shape.material.diffuse[2];

16
examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
View File

@ -11,8 +11,8 @@
struct CachedObjResult struct CachedObjResult
{ {
std::string m_msg; std::string m_msg;
std::vector<tinyobj::shape_t> m_shapes; std::vector<bt_tinyobj::shape_t> m_shapes;
tinyobj::attrib_t m_attribute; bt_tinyobj::attrib_t m_attribute;
}; };
static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults; static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
@ -32,8 +32,8 @@ void b3EnableFileCaching(int enable)
} }
std::string LoadFromCachedOrFromObj( std::string LoadFromCachedOrFromObj(
tinyobj::attrib_t& attribute, bt_tinyobj::attrib_t& attribute,
std::vector<tinyobj::shape_t>& shapes, // [output] std::vector<bt_tinyobj::shape_t>& shapes, // [output]
const char* filename, const char* filename,
const char* mtl_basepath, const char* mtl_basepath,
struct CommonFileIOInterface* fileIO) struct CommonFileIOInterface* fileIO)
@ -47,7 +47,7 @@ std::string LoadFromCachedOrFromObj(
return result.m_msg; return result.m_msg;
} }
std::string err = tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO); std::string err = bt_tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO);
CachedObjResult result; CachedObjResult result;
result.m_msg = err; result.m_msg = err;
result.m_shapes = shapes; result.m_shapes = shapes;
@ -62,10 +62,10 @@ std::string LoadFromCachedOrFromObj(
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO) GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO)
{ {
B3_PROFILE("LoadMeshFromObj"); B3_PROFILE("LoadMeshFromObj");
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
{ {
B3_PROFILE("tinyobj::LoadObj2"); B3_PROFILE("bt_tinyobj::LoadObj2");
std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO); std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO);
} }

4
examples/Importers/ImportObjDemo/LoadMeshFromObj.h
View File

@ -9,8 +9,8 @@ int b3IsFileCachingEnabled();
void b3EnableFileCaching(int enable); void b3EnableFileCaching(int enable);
std::string LoadFromCachedOrFromObj( std::string LoadFromCachedOrFromObj(
tinyobj::attrib_t& attribute, bt_tinyobj::attrib_t& attribute,
std::vector<tinyobj::shape_t>& shapes, // [output] std::vector<bt_tinyobj::shape_t>& shapes, // [output]
const char* filename, const char* filename,
const char* mtl_basepath, const char* mtl_basepath,
struct CommonFileIOInterface* fileIO); struct CommonFileIOInterface* fileIO);

10
examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
View File

@ -9,7 +9,7 @@
#include "../../OpenGLWindow/GLInstancingRenderer.h" #include "../../OpenGLWindow/GLInstancingRenderer.h"
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h" #include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading) GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, bool flatShading)
{ {
b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>; b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
{ {
@ -19,7 +19,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
for (int s = 0; s < (int)shapes.size(); s++) for (int s = 0; s < (int)shapes.size(); s++)
{ {
tinyobj::shape_t& shape = shapes[s]; bt_tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size(); int faceCount = shape.mesh.indices.size();
for (int f = 0; f < faceCount; f += 3) for (int f = 0; f < faceCount; f += 3)
@ -36,7 +36,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
} }
GLInstanceVertex vtx0; GLInstanceVertex vtx0;
tinyobj::index_t v_0 = shape.mesh.indices[f]; bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
vtx0.xyzw[0] = attribute.vertices[3 * v_0.vertex_index]; vtx0.xyzw[0] = attribute.vertices[3 * v_0.vertex_index];
vtx0.xyzw[1] = attribute.vertices[3 * v_0.vertex_index + 1]; vtx0.xyzw[1] = attribute.vertices[3 * v_0.vertex_index + 1];
vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2]; vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2];
@ -65,7 +65,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
} }
GLInstanceVertex vtx1; GLInstanceVertex vtx1;
tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
vtx1.xyzw[0] = attribute.vertices[3 * v_1.vertex_index]; vtx1.xyzw[0] = attribute.vertices[3 * v_1.vertex_index];
vtx1.xyzw[1] = attribute.vertices[3 * v_1.vertex_index + 1]; vtx1.xyzw[1] = attribute.vertices[3 * v_1.vertex_index + 1];
vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2]; vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2];
@ -94,7 +94,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
} }
GLInstanceVertex vtx2; GLInstanceVertex vtx2;
tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
vtx2.xyzw[0] = attribute.vertices[3 * v_2.vertex_index]; vtx2.xyzw[0] = attribute.vertices[3 * v_2.vertex_index];
vtx2.xyzw[1] = attribute.vertices[3 * v_2.vertex_index + 1]; vtx2.xyzw[1] = attribute.vertices[3 * v_2.vertex_index + 1];
vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2]; vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2];

2
examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
View File

@ -4,6 +4,6 @@
#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include <vector> #include <vector>
struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading = false); struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, bool flatShading = false);
#endif //WAVEFRONT2GRAPHICS_H #endif //WAVEFRONT2GRAPHICS_H

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

@ -483,6 +483,12 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass, btVect
} }
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 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
{
btScalar twistLimit;
return getJointInfo3(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity, twistLimit);
}
bool BulletURDFImporter::getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const
{ {
jointLowerLimit = 0.f; jointLowerLimit = 0.f;
jointUpperLimit = 0.f; jointUpperLimit = 0.f;
@ -510,6 +516,7 @@ bool BulletURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2jo
jointFriction = pj->m_jointFriction; jointFriction = pj->m_jointFriction;
jointMaxForce = pj->m_effortLimit; jointMaxForce = pj->m_effortLimit;
jointMaxVelocity = pj->m_velocityLimit; jointMaxVelocity = pj->m_velocityLimit;
twistLimit = pj->m_twistLimit;
return true; return true;
} }
else else
@ -540,7 +547,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true; return true;
} }
static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags) static btCollisionShape* createConvexHullFromShapes(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
{ {
B3_PROFILE("createConvexHullFromShapes"); B3_PROFILE("createConvexHullFromShapes");
btCompoundShape* compound = new btCompoundShape(); btCompoundShape* compound = new btCompoundShape();
@ -553,7 +560,7 @@ static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& att
{ {
btConvexHullShape* convexHull = new btConvexHullShape(); btConvexHullShape* convexHull = new btConvexHullShape();
convexHull->setMargin(gUrdfDefaultCollisionMargin); convexHull->setMargin(gUrdfDefaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s]; bt_tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size(); int faceCount = shape.mesh.indices.size();
for (int f = 0; f < faceCount; f += 3) for (int f = 0; f < faceCount; f += 3)
@ -747,9 +754,9 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
} }
else else
{ {
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO); std::string err = bt_tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape //create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags); shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision); m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);

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

@ -60,6 +60,7 @@ public:
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) 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 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 getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld); virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);

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

@ -10,6 +10,9 @@
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "URDF2Bullet.h" #include "URDF2Bullet.h"
#include "URDFImporterInterface.h" #include "URDFImporterInterface.h"
@ -259,8 +262,8 @@ btTransform ConvertURDF2BulletInternal(
btScalar jointFriction; btScalar jointFriction;
btScalar jointMaxForce; btScalar jointMaxForce;
btScalar jointMaxVelocity; btScalar jointMaxVelocity;
btScalar twistLimit;
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity); bool hasParentJoint = u2b.getJointInfo3(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity, twistLimit);
std::string linkName = u2b.getLinkName(urdfLinkIndex); std::string linkName = u2b.getLinkName(urdfLinkIndex);
if (flags & CUF_USE_SDF) if (flags & CUF_USE_SDF)
@ -422,6 +425,20 @@ btTransform ConvertURDF2BulletInternal(
cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(), parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(),
disableParentCollision); disableParentCollision);
//create a spherical joint limit, swing_x,. swing_y and twist
//jointLowerLimit <= jointUpperLimit)
if (jointUpperLimit > 0 && jointLowerLimit> 0 && twistLimit > 0 && jointMaxForce>0)
{
btMultiBodySphericalJointLimit* con = new btMultiBodySphericalJointLimit(cache.m_bulletMultiBody, mbLinkIndex,
jointLowerLimit,
jointUpperLimit,
twistLimit,
jointMaxForce);
world1->addMultiBodyConstraint(con);
}
} }
else else
{ {

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

@ -64,6 +64,13 @@ public:
return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction); return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
}; };
virtual bool getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const
{
//backwards compatibility for custom file importers
twistLimit = 0;
return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
};
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const = 0; virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const = 0;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) {} virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) {}

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

@ -1455,6 +1455,7 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
joint.m_velocityLimit = 0.f; joint.m_velocityLimit = 0.f;
joint.m_jointDamping = 0.f; joint.m_jointDamping = 0.f;
joint.m_jointFriction = 0.f; joint.m_jointFriction = 0.f;
joint.m_twistLimit = -1;
if (m_parseSDF) if (m_parseSDF)
{ {
@ -1469,13 +1470,19 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
{ {
joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText()); joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText());
} }
XMLElement* twist_xml = config->FirstChildElement("twist");
if (twist_xml)
{
joint.m_twistLimit = urdfLexicalCast<double>(twist_xml->GetText());
}
XMLElement* effort_xml = config->FirstChildElement("effort"); XMLElement* effort_xml = config->FirstChildElement("effort");
if (effort_xml) if (effort_xml)
{ {
joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText()); joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText());
} }
XMLElement* velocity_xml = config->FirstChildElement("velocity"); XMLElement* velocity_xml = config->FirstChildElement("velocity");
if (velocity_xml) if (velocity_xml)
{ {
@ -1502,6 +1509,14 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
joint.m_upperLimit *= m_urdfScaling; joint.m_upperLimit *= m_urdfScaling;
} }
const char* twist_str = config->Attribute("twist");
if (twist_str)
{
joint.m_twistLimit = urdfLexicalCast<double>(twist_str);
}
// Get joint effort limit // Get joint effort limit
const char* effort_str = config->Attribute("effort"); const char* effort_str = config->Attribute("effort");
if (effort_str) if (effort_str)
@ -1509,6 +1524,7 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
joint.m_effortLimit = urdfLexicalCast<double>(effort_str); joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
} }
// Get joint velocity limit // Get joint velocity limit
const char* velocity_str = config->Attribute("velocity"); const char* velocity_str = config->Attribute("velocity");
if (velocity_str) if (velocity_str)

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

@ -189,13 +189,15 @@ struct UrdfJoint
double m_jointDamping; double m_jointDamping;
double m_jointFriction; double m_jointFriction;
double m_twistLimit;
UrdfJoint() UrdfJoint()
: m_lowerLimit(0), : m_lowerLimit(0),
m_upperLimit(-1), m_upperLimit(-1),
m_effortLimit(0), m_effortLimit(0),
m_velocityLimit(0), m_velocityLimit(0),
m_jointDamping(0), m_jointDamping(0),
m_jointFriction(0) m_jointFriction(0),
m_twistLimit(-1)
{ {
} }
}; };

203
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -3455,34 +3455,46 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
} }
} }
// Because the link order between UrdfModel and MultiBody may be different, // Add user data specified in URDF to the added body.
// create a mapping from link name to link index in order to apply the user
// data to the correct link in the MultiBody.
btHashMap<btHashString, int> linkNameToIndexMap;
if (bodyHandle->m_multiBody)
{
btMultiBody* mb = bodyHandle->m_multiBody;
linkNameToIndexMap.insert(mb->getBaseName(), -1);
for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
{
linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
}
}
const UrdfModel* urdfModel = u2b.getUrdfModel(); const UrdfModel* urdfModel = u2b.getUrdfModel();
if (urdfModel) if (urdfModel)
{ {
addUserData(urdfModel->m_userData, bodyUniqueId); addUserData(urdfModel->m_userData, bodyUniqueId);
for (int i = 0; i < urdfModel->m_links.size(); ++i) if (bodyHandle->m_multiBody)
{ {
const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); btMultiBody* mb = bodyHandle->m_multiBody;
int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str()); // Because the link order between UrdfModel and MultiBody may be different,
if (linkIndex) // create a mapping from link name to link index in order to apply the user
// data to the correct link in the MultiBody.
btHashMap<btHashString, int> linkNameToIndexMap;
linkNameToIndexMap.insert(mb->getBaseName(), -1);
for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
{ {
addUserData(link->m_userData, bodyUniqueId, *linkIndex); linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
}
for (int i = 0; i < urdfModel->m_links.size(); ++i)
{
const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
if (linkIndex)
{
addUserData(link->m_userData, bodyUniqueId, *linkIndex);
for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
{
addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex);
}
}
}
}
else if (bodyHandle->m_rigidBody)
{
for (int i = 0; i < urdfModel->m_links.size(); ++i)
{
const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
addUserData(link->m_userData, bodyUniqueId, -1);
for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
{ {
addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex); addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, -1, visualShapeIndex);
} }
} }
} }
@ -5302,12 +5314,12 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{ {
//create a convex hull for each shape, and store it in a btCompoundShape //create a convex hull for each shape, and store it in a btCompoundShape
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale) //static btCollisionShape* createConvexHullFromShapes(std::vector<bt_tinyobj::shape_t>& shapes, const btVector3& geomScale)
B3_PROFILE("createConvexHullFromShapes"); B3_PROFILE("createConvexHullFromShapes");
if (compound == 0) if (compound == 0)
{ {
@ -5319,7 +5331,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{ {
btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(m_data->m_defaultCollisionMargin); convexHull->setMargin(m_data->m_defaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s]; bt_tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size(); int faceCount = shape.mesh.indices.size();
for (int f = 0; f < faceCount; f += 3) for (int f = 0; f < faceCount; f += 3)
@ -8100,26 +8112,21 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
{ {
return false; return false;
} }
int numSoftbodyContact = 0;
for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{
numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size();
}
int num_contact_points = m_data->m_cachedContactPoints.size();
m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact);
for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{ {
btSoftBody* psb = deformWorld->getSoftBodyArray()[i]; btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
for (int c = 0; c < psb->m_faceRigidContacts.size(); c++) for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
{ {
const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c]; const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
// calculate normal and tangent impulse //convert rigidbody contact
btVector3 impulse = contact->m_cti.m_impulse;
btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal;
btVector3 impulseTangent = impulse - impulseNormal;
// get node in contact
int contactNodeIdx = contact->m_bary.maxAxis();
btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx];
// check if node is already in the list
int idx = nodesInContact.findLinearSearch2(node);
//apply the filter, if the user provides it
int linkIndexA = -1; int linkIndexA = -1;
int linkIndexB = -1; int linkIndexB = -1;
int objectIndexA = psb->getUserIndex2(); int objectIndexA = psb->getUserIndex2();
@ -8136,6 +8143,8 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
linkIndexB = mblB->m_link; linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2(); objectIndexB = mblB->m_multiBody->getUserIndex2();
} }
//apply the filter, if the user provides it
bool swap = false; bool swap = false;
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
{ {
@ -8181,87 +8190,37 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
{ {
continue; continue;
} }
b3ContactPointData pt;
if (idx < 0) pt.m_bodyUniqueIdA = objectIndexA;
pt.m_bodyUniqueIdB = objectIndexB;
pt.m_contactDistance = contact->m_cti.m_offset;
pt.m_contactFlags = 0;
pt.m_linkIndexA = linkIndexA;
pt.m_linkIndexB = linkIndexB;
for (int j = 0; j < 3; j++)
{ {
// add new node and contact point if (swap)
nodesInContact.push_back(node);
b3ContactPointData pt;
pt.m_bodyUniqueIdA = objectIndexA;
pt.m_bodyUniqueIdB = objectIndexB;
pt.m_contactDistance = -contact->m_cti.m_offset;
pt.m_contactFlags = 0;
pt.m_linkIndexA = linkIndexA;
pt.m_linkIndexB = linkIndexB;
for (int j = 0; j < 3; j++)
{ {
if (swap) pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
{ pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j];
pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j]; pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j];
pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
pt.m_positionOnBInWS[j] = node->m_x[j];
}
else
{
pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
pt.m_positionOnAInWS[j] = node->m_x[j];
pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
}
} }
pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm(); else
pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm();
pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm();
for (int j = 0; j < 3; j++)
{ {
pt.m_linearFrictionDirection1[j] = contact->t1[j]; pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
pt.m_linearFrictionDirection2[j] = contact->t2[j]; pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j];
pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j];
} }
distinctContactPoints.push_back(pt);
} }
else pt.m_normalForce = 1;
pt.m_linearFrictionForce1 = 0;
pt.m_linearFrictionForce2 = 0;
for (int j = 0; j < 3; j++)
{ {
// add values to existing contact point pt.m_linearFrictionDirection1[j] = 0;
b3ContactPointData* pt = &distinctContactPoints[idx]; pt.m_linearFrictionDirection2[j] = 0;
// current normal force of node
btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]),
btScalar(pt->m_contactNormalOnBInWS[1]),
btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce;
// add normal force of additional node contact
btScalar swapFactor = swap ? -1.0 : 1.0;
normalForce += swapFactor * contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm();
// get magnitude of normal force
pt->m_normalForce = normalForce.norm();
// get direction of normal force
if (!normalForce.fuzzyZero())
{
// normalize for unit vectors if above numerical threshold
normalForce.normalize();
for (int j = 0; j < 3; j++)
{
pt->m_contactNormalOnBInWS[j] = normalForce[j];
}
}
// add magnitudes of tangential forces in existing directions
btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]),
btScalar(pt->m_linearFrictionDirection1[1]),
btScalar(pt->m_linearFrictionDirection1[2]));
btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]),
btScalar(pt->m_linearFrictionDirection2[1]),
btScalar(pt->m_linearFrictionDirection2[2]));
pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm();
pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm();
} }
} m_data->m_cachedContactPoints.push_back(pt);
int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size();
m_data->m_cachedContactPoints.reserve(num_contact_points);
// add points to contact points cache
for (int p = 0; p < distinctContactPoints.size(); p++)
{
m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]);
} }
} }
#endif #endif
@ -9055,12 +9014,12 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
} }
if (out_sim_type == UrdfGeometry::FILE_OBJ) if (out_sim_type == UrdfGeometry::FILE_OBJ)
{ {
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO); std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
if (!shapes.empty()) if (!shapes.empty())
{ {
const tinyobj::shape_t& shape = shapes[0]; const bt_tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices; btAlignedObjectArray<btScalar> vertices;
btAlignedObjectArray<int> indices; btAlignedObjectArray<int> indices;
for (int i = 0; i < attribute.vertices.size(); i++) for (int i = 0; i < attribute.vertices.size(); i++)
@ -9204,14 +9163,14 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
} }
else else
{ {
tinyobj::attrib_t attribute; bt_tinyobj::attrib_t attribute;
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface()); std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
for (int s = 0; s < (int)shapes.size(); s++) for (int s = 0; s < (int)shapes.size(); s++)
{ {
tinyobj::shape_t& shape = shapes[s]; bt_tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size(); int faceCount = shape.mesh.indices.size();
int vertexCount = attribute.vertices.size() / 3; int vertexCount = attribute.vertices.size() / 3;
for (int v = 0; v < vertexCount; v++) for (int v = 0; v < vertexCount; v++)
@ -9226,9 +9185,9 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
{ {
continue; continue;
} }
tinyobj::index_t v_0 = shape.mesh.indices[f]; bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
btSoftBody::RenderFace ff; btSoftBody::RenderFace ff;
ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index]; ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index]; ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];

4
examples/ThirdPartyLibs/Wavefront/main.cpp
View File

@ -38,8 +38,8 @@ TestLoadObj(
std::cout << "Loading " << fullPath << std::endl; std::cout << "Loading " << fullPath << std::endl;
std::vector<tinyobj::shape_t> shapes; std::vector<bt_tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, prefix[index]); std::string err = bt_tinyobj::LoadObj(shapes, fullPath, prefix[index]);
if (!err.empty()) if (!err.empty())
{ {

4
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@ -31,7 +31,7 @@
#include "tiny_obj_loader.h" #include "tiny_obj_loader.h"
#include <stdio.h> #include <stdio.h>
namespace tinyobj namespace bt_tinyobj
{ {
#ifdef USE_STREAM #ifdef USE_STREAM
//See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf //See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
@ -880,4 +880,4 @@ LoadObj(
return err.str(); return err.str();
} }
}; // namespace tinyobj }; // namespace bt_tinyobj

10
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
View File

@ -3,8 +3,8 @@
// //
// Licensed under 2-clause BSD liecense. // Licensed under 2-clause BSD liecense.
// //
#ifndef _TINY_OBJ_LOADER_H #ifndef _BT_TINY_OBJ_LOADER_H
#define _TINY_OBJ_LOADER_H #define _BT_TINY_OBJ_LOADER_H
#include <string> #include <string>
#include <vector> #include <vector>
@ -12,7 +12,7 @@
struct CommonFileIOInterface; struct CommonFileIOInterface;
namespace tinyobj namespace bt_tinyobj
{ {
struct vertex_index_t struct vertex_index_t
{ {
@ -94,6 +94,6 @@ LoadObj(
CommonFileIOInterface* fileIO); CommonFileIOInterface* fileIO);
#endif #endif
}; // namespace tinyobj }; // namespace bt_tinyobj
#endif // _TINY_OBJ_LOADER_H #endif // _BT_TINY_OBJ_LOADER_H

56
examples/pybullet/examples/spherical_joint_limit.py Normal file
View File

@ -0,0 +1,56 @@
import pybullet as p
import pybullet_data as pd
#see spherical_joint_limit.urdf
#lower is the swing range in the joint local X axis
#upper is the swing range in the joint local Y axis
#twist is the twist range rotation around the joint local Z axis
#effort is the maximum force impulse to enforce the joint limit
#<limit effort="1000.0" lower="0.2" upper=".8" twist=".3"/>
import time
dt = 1./240.
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
p.setTimeStep(dt)
humanoid = p.loadURDF("spherical_joint_limit.urdf",[0,0,2], useFixedBase=True)
gravxId = p.addUserDebugParameter("grav_x",-20,20,0.3)
gravyId = p.addUserDebugParameter("grav_y",-20,20,0.3)
gravzId = p.addUserDebugParameter("grav_y",-20,20,-10)
index= 0
spherical_index = -1
for j in range(p.getNumJoints(humanoid)):
if index<7:
ji = p.getJointInfo(humanoid, j)
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
index+=4
p.resetJointStateMultiDof(humanoid, j, targetValue=[0,0,0,1], targetVelocity=[0,0,0])
#p.changeDynamics(humanoid,j,angularDamping=0, linearDamping=0)
spherical_index = j
p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL,
targetPosition=[0,0,0,1], positionGain=0.2,
targetVelocity=[0,0,0], velocityGain=0,
force=[0,0,0])
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
index+=1
p.resetJointStateMultiDof(humanoid, j, targetValue=[0], targetVelocity=[0])
p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL,
targetPosition=[0], targetVelocity=[0], force=[0])
p.loadURDF("plane.urdf")
p.setRealTimeSimulation(1)
while p.isConnected():
gravX = p.readUserDebugParameter(gravxId)
gravY = p.readUserDebugParameter(gravyId)
gravZ = p.readUserDebugParameter(gravzId)
p.setGravity(gravX,gravY,gravZ)
#p.stepSimulation()
time.sleep(dt/10.)

36
examples/pybullet/gym/pybullet_data/spherical_joint_limit.urdf Normal file
View File

@ -0,0 +1,36 @@
<robot name="dumpUrdf">
<link name="chest" >
<inertial>
<origin rpy = "0 0 0" xyz = "0.000000 0.00000 0.000000" />
<mass value = "4.000000" />
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
</inertial>
<collision>
<origin rpy = "0 0 0" xyz = "0.000000 0.00000 0.000000" />
<geometry>
<box size="0.6 0.6 0.6"/>
</geometry>
</collision>
</link>
<link name="neck" >
<inertial>
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.30000" />
<mass value = "1.00000" />
<inertia ixx = "0.01" ixy = "0" ixz = "0" iyy = "0.001" iyz = "0" izz = "0.001" />
</inertial>
<collision>
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.30000" />
<geometry>
<box size="0.1 0.2 0.6"/>
</geometry>
</collision>
</link>
<joint name="neck" type="spherical" >
<parent link="chest" />
<limit effort="1000.0" lower="0.2" upper=".8" twist=".3"/>
<child link="neck" />
<origin rpy = "0 0 0" xyz = "0.000000 0.0 1.000000" />
<axis xyz="0 0 1"/>
</joint>
</robot>

21
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
View File

@ -25,11 +25,28 @@ class EnvRandomizerBase(object):
pass pass
def randomize_step(self, env): def randomize_step(self, env):
"""Randomize simulation steps. """Randomize environment steps.
Will be called at every timestep. May add random forces/torques to Minitaur. Will be called at every environment step.
It is NOT recommended to use this for force / torque disturbance because
pybullet applyExternalForce/Torque only persist for single simulation step
not the entire env step which can contain multiple simulation steps.
Args: Args:
env: The Minitaur gym environment to be randomized. env: The Minitaur gym environment to be randomized.
""" """
pass pass
def randomize_sub_step(self, env, sub_step_index, num_sub_steps):
"""Randomize simulation sub steps.
Will be called at every simulation step. This is the correct place to add
random forces/torques.
Args:
env: The Minitaur gym environment to be randomized.
sub_step_index: Index of sub step, from 0 to N-1. N is the action repeat.
num_sub_steps: Number of sub steps, equals to action repeat.
"""
pass

36
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
View File

@ -10,7 +10,9 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
parentdir = os.path.dirname(os.path.dirname(parentdir)) parentdir = os.path.dirname(os.path.dirname(parentdir))
os.sys.path.insert(0, parentdir) os.sys.path.insert(0, parentdir)
import functools
import math import math
import gin
import numpy as np import numpy as np
from pybullet_envs.minitaur.envs import env_randomizer_base from pybullet_envs.minitaur.envs import env_randomizer_base
@ -23,6 +25,7 @@ _VERTICAL_FORCE_UPPER_BOUND = 300
_VERTICAL_FORCE_LOWER_BOUND = 500 _VERTICAL_FORCE_LOWER_BOUND = 500
@gin.configurable
class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase): class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
"""Applies a random impulse to the base of Minitaur.""" """Applies a random impulse to the base of Minitaur."""
@ -54,6 +57,7 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
[_HORIZONTAL_FORCE_LOWER_BOUND, _HORIZONTAL_FORCE_UPPER_BOUND]) [_HORIZONTAL_FORCE_LOWER_BOUND, _HORIZONTAL_FORCE_UPPER_BOUND])
self._vertical_force_bound = (vertical_force_bound if vertical_force_bound else self._vertical_force_bound = (vertical_force_bound if vertical_force_bound else
[_VERTICAL_FORCE_LOWER_BOUND, _VERTICAL_FORCE_UPPER_BOUND]) [_VERTICAL_FORCE_LOWER_BOUND, _VERTICAL_FORCE_UPPER_BOUND])
self._perturbation_parameter_dict = None
def randomize_env(self, env): def randomize_env(self, env):
"""Randomizes the simulation environment. """Randomizes the simulation environment.
@ -64,9 +68,10 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
pass pass
def randomize_step(self, env): def randomize_step(self, env):
"""Randomizes simulation steps. """Randomizes env steps.
Will be called at every timestep. May add random forces/torques to Minitaur. Will be called at every env step. Called to generate randomized force and
torque to apply. Application of forces are done in randomize_sub_step.
Args: Args:
env: The Minitaur gym environment to be randomized. env: The Minitaur gym environment to be randomized.
@ -85,8 +90,25 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
if (env.env_step_counter % self._perturbation_interval_steps < if (env.env_step_counter % self._perturbation_interval_steps <
self._perturbation_duration_steps) and (env.env_step_counter >= self._perturbation_duration_steps) and (env.env_step_counter >=
self._perturbation_start_step): self._perturbation_start_step):
env.pybullet_client.applyExternalForce(objectUniqueId=env.minitaur.quadruped, # Parameter of pybullet_client.applyExternalForce()
linkIndex=self._applied_link_id, self._perturbation_parameter_dict = dict(objectUniqueId=env.minitaur.quadruped,
forceObj=self._applied_force, linkIndex=self._applied_link_id,
posObj=[0.0, 0.0, 0.0], forceObj=self._applied_force,
flags=env.pybullet_client.LINK_FRAME) posObj=[0.0, 0.0, 0.0],
flags=env.pybullet_client.LINK_FRAME)
else:
self._perturbation_parameter_dict = None
def randomize_sub_step(self, env, sub_step_index, num_sub_steps):
"""Randomize simulation steps per sub steps (simulation step).
Will be called at every simulation step. This is the correct place to add
random forces/torques to Minitaur.
Args:
env: The Minitaur gym environment to be randomized.
sub_step_index: Index of sub step, from 0 to N-1. N is the action repeat.
num_sub_steps: Number of sub steps, equals to action repeat.
"""
if self._perturbation_parameter_dict is not None:
env.pybullet_client.applyExternalForce(**self._perturbation_parameter_dict)

2
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
View File

@ -457,6 +457,8 @@ class LocomotionGymEnv(gym.Env):
for obj in self._dynamic_objects(): for obj in self._dynamic_objects():
obj.pre_control_step(autonomous_object.AUTONOMOUS_ACTION) obj.pre_control_step(autonomous_object.AUTONOMOUS_ACTION)
for _ in range(self._num_action_repeat): for _ in range(self._num_action_repeat):
for env_randomizer in self._env_randomizers:
env_randomizer.randomize_sub_step(self, i, self._num_action_repeat)
self._robot.apply_action(action) self._robot.apply_action(action)
for obj in self._dynamic_objects(): for obj in self._dynamic_objects():
obj.update(self.get_time_since_reset(), self._observation_dict) obj.update(self.get_time_since_reset(), self._observation_dict)

4
src/BulletDynamics/CMakeLists.txt
View File

@ -42,6 +42,7 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyPoint2Point.cpp Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodySliderConstraint.cpp Featherstone/btMultiBodySliderConstraint.cpp
Featherstone/btMultiBodySphericalJointMotor.cpp Featherstone/btMultiBodySphericalJointMotor.cpp
Featherstone/btMultiBodySphericalJointLimit.cpp
MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp MLCPSolvers/btMLCPSolver.cpp
MLCPSolvers/btLemkeAlgorithm.cpp MLCPSolvers/btLemkeAlgorithm.cpp
@ -105,6 +106,9 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyPoint2Point.h Featherstone/btMultiBodyPoint2Point.h
Featherstone/btMultiBodySliderConstraint.h Featherstone/btMultiBodySliderConstraint.h
Featherstone/btMultiBodySolverConstraint.h Featherstone/btMultiBodySolverConstraint.h
Featherstone/btMultiBodySphericalJointMotor.h
Featherstone/btMultiBodySphericalJointLimit.h
) )
SET(MLCPSolvers_HDRS SET(MLCPSolvers_HDRS

2
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@ -31,7 +31,7 @@ enum btTypedMultiBodyConstraintType
MULTIBODY_CONSTRAINT_SLIDER, MULTIBODY_CONSTRAINT_SLIDER,
MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
MULTIBODY_CONSTRAINT_FIXED, MULTIBODY_CONSTRAINT_FIXED,
MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT,
MAX_MULTIBODY_CONSTRAINT_TYPE, MAX_MULTIBODY_CONSTRAINT_TYPE,
}; };

270
src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp Normal file
View File

@ -0,0 +1,270 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2018 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodySphericalJointLimit.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "LinearMath/btTransformUtil.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "LinearMath/btIDebugDraw.h"
btMultiBodySphericalJointLimit::btMultiBodySphericalJointLimit(btMultiBody* body, int link,
btScalar swingxRange,
btScalar swingyRange,
btScalar twistRange,
btScalar maxAppliedImpulse)
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT),
m_desiredVelocity(0, 0, 0),
m_desiredPosition(0,0,0,1),
m_use_multi_dof_params(false),
m_kd(1., 1., 1.),
m_kp(0.2, 0.2, 0.2),
m_erp(1),
m_rhsClamp(SIMD_INFINITY),
m_maxAppliedImpulseMultiDof(maxAppliedImpulse, maxAppliedImpulse, maxAppliedImpulse),
m_pivotA(m_bodyA->getLink(link).m_eVector),
m_pivotB(m_bodyB->getLink(link).m_eVector),
m_swingxRange(swingxRange),
m_swingyRange(swingyRange),
m_twistRange(twistRange)
{
m_maxAppliedImpulse = maxAppliedImpulse;
}
void btMultiBodySphericalJointLimit::finalizeMultiDof()
{
allocateJacobiansMultiDof();
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
jacobianB(1)[offset] = -1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodySphericalJointLimit::~btMultiBodySphericalJointLimit()
{
}
int btMultiBodySphericalJointLimit::getIslandIdA() const
{
if (this->m_linkA < 0)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
{
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodySphericalJointLimit::getIslandIdB() const
{
if (m_linkB < 0)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
{
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}
void btMultiBodySphericalJointLimit::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse == 0.f)
return;
const btScalar posError = 0;
const btVector3 zero(0, 0, 0);
btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
m_bodyA->getJointPosMultiDof(m_linkA)[1],
m_bodyA->getJointPosMultiDof(m_linkA)[2],
m_bodyA->getJointPosMultiDof(m_linkA)[3]);
btQuaternion refQuat = m_desiredPosition;
btVector3 vTwist(0,0,1);
btVector3 vConeNoTwist = quatRotate(currentQuat, vTwist);
vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * currentQuat;
qABTwist.normalize();
btQuaternion desiredQuat = qABTwist;
btQuaternion relRot = currentQuat.inverse() * desiredQuat;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
btScalar limitRanges[3] = {m_swingxRange, m_swingyRange, m_twistRange};
/// twist axis/angle
btQuaternion qMinTwist = qABTwist;
btScalar twistAngle = qABTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qABTwist);
twistAngle = qMinTwist.getAngle();
}
btVector3 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
if (twistAngle > SIMD_EPSILON)
vTwistAxis.normalize();
if (vTwistAxis.dot(vTwist)<0)
twistAngle*=-1.;
angleDiff[2] = twistAngle;
for (int row = 0; row < getNumRows(); row++)
{
btScalar allowed = limitRanges[row];
btScalar damp = 1;
if((angleDiff[row]>-allowed)&&(angleDiff[row]<allowed))
{
angleDiff[row]=0;
damp=0;
} else
{
if (angleDiff[row]>allowed)
{
angleDiff[row]-=allowed;
}
if (angleDiff[row]<-allowed)
{
angleDiff[row]+=allowed;
}
}
int dof = row;
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar desiredVelocity = this->m_desiredVelocity[row];
double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
btMatrix3x3 frameAworld;
frameAworld.setIdentity();
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
btScalar posError = 0;
{
btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eSpherical:
{
btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
posError = kp*angleDiff[row % 3];
double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
//should multiply by time step
//max_applied_impulse *= infoGlobal.m_timeStep
double min_applied_impulse = -max_applied_impulse;
if (posError>0)
max_applied_impulse=0;
else
min_applied_impulse=0;
if (btFabs(posError)>SIMD_EPSILON)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
zero, zero, zero,//pure angular, so zero out linear parts
posError,
infoGlobal,
min_applied_impulse, max_applied_impulse, true,
1.0, false, 0, 0,
damp);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
}
break;
}
default:
{
btAssert(0);
}
};
}
}
}
void btMultiBodySphericalJointLimit::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

115
src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h Normal file
View File

@ -0,0 +1,115 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2018 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H
#define BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodySphericalJointLimit : public btMultiBodyConstraint
{
protected:
btVector3 m_desiredVelocity;
btQuaternion m_desiredPosition;
bool m_use_multi_dof_params;
btVector3 m_kd;
btVector3 m_kp;
btScalar m_erp;
btScalar m_rhsClamp; //maximum error
btVector3 m_maxAppliedImpulseMultiDof;
btVector3 m_pivotA;
btVector3 m_pivotB;
btScalar m_swingxRange;
btScalar m_swingyRange;
btScalar m_twistRange;
public:
btMultiBodySphericalJointLimit(btMultiBody* body, int link,
btScalar swingxRange,
btScalar swingyRange,
btScalar twistRange,
btScalar maxAppliedImpulse);
virtual ~btMultiBodySphericalJointLimit();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0)
{
m_desiredVelocity = velTarget;
m_kd = btVector3(kd, kd, kd);
m_use_multi_dof_params = false;
}
virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0))
{
m_desiredVelocity = velTarget;
m_kd = kd;
m_use_multi_dof_params = true;
}
virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f)
{
m_desiredPosition = posTarget;
m_kp = btVector3(kp, kp, kp);
m_use_multi_dof_params = false;
}
virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f))
{
m_desiredPosition = posTarget;
m_kp = kp;
m_use_multi_dof_params = true;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void setRhsClamp(btScalar rhsClamp)
{
m_rhsClamp = rhsClamp;
}
btScalar getMaxAppliedImpulseMultiDof(int i) const
{
return m_maxAppliedImpulseMultiDof[i];
}
void setMaxAppliedImpulseMultiDof(const btVector3& maxImp)
{
m_maxAppliedImpulseMultiDof = maxImp;
m_use_multi_dof_params = true;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H

5
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
{ {
dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
} }
// dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
if (!infoGlobal.m_splitImpulse) if (!infoGlobal.m_splitImpulse)
{ {
@ -487,9 +487,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
btVector3 dv = impulse * contact->m_c2; btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face; btSoftBody::Face* face = contact->m_face;
// save applied impulse
contact->m_cti.m_impulse = impulse;
btVector3& v0 = face->m_n[0]->m_v; btVector3& v0 = face->m_n[0]->m_v;
btVector3& v1 = face->m_n[1]->m_v; btVector3& v1 = face->m_n[1]->m_v;
btVector3& v2 = face->m_n[2]->m_v; btVector3& v2 = face->m_n[2]->m_v;

10
src/BulletSoftBody/btSoftBody.h
View File

@ -223,12 +223,10 @@ public:
/* sCti is Softbody contact info */ /* sCti is Softbody contact info */
struct sCti struct sCti
{ {
const btCollisionObject* m_colObj; /* Rigid body */ const btCollisionObject* m_colObj; /* Rigid body */
btVector3 m_normal; /* Outward normal */ btVector3 m_normal; /* Outward normal */
mutable btVector3 m_impulse; /* Applied impulse */ btScalar m_offset; /* Offset from origin */
btScalar m_offset; /* Offset from origin */
btVector3 m_bary; /* Barycentric weights for faces */ btVector3 m_bary; /* Barycentric weights for faces */
sCti() : m_impulse(0, 0, 0) {}
}; };
/* sMedium */ /* sMedium */
@ -897,7 +895,7 @@ public:
int node1) const; int node1) const;
bool checkLink(const Node* node0, bool checkLink(const Node* node0,
const Node* node1) const; const Node* node1) const;
/* Check for existing face */ /* Check for existring face */
bool checkFace(int node0, bool checkFace(int node0,
int node1, int node1,
int node2) const; int node2) const;

1
src/btBulletDynamicsAll.cpp
View File

@ -36,6 +36,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp" #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp"
#include "BulletDynamics/Vehicle/btRaycastVehicle.cpp" #include "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
#include "BulletDynamics/Vehicle/btWheelInfo.cpp" #include "BulletDynamics/Vehicle/btWheelInfo.cpp"
#include "BulletDynamics/Character/btKinematicCharacterController.cpp" #include "BulletDynamics/Character/btKinematicCharacterController.cpp"