mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-12 12:50:08 +00:00
Merge branch 'bulletphysics:master' into master
This commit is contained in:
jingyuc
GitHub
commit
ca9d9b3ee6
@ -30,13 +30,13 @@
|
||||
#include "Bullet3Common/b3HashMap.h"
|
||||
#include "../Utils/b3BulletDefaultFileIO.h"
|
||||
|
||||
using tinyobj::index_t;
|
||||
using bt_tinyobj::index_t;
|
||||
|
||||
struct ShapeContainer
|
||||
{
|
||||
std::string m_matName;
|
||||
std::string m_shapeName;
|
||||
tinyobj::material_t material;
|
||||
bt_tinyobj::material_t material;
|
||||
std::vector<float> positions;
|
||||
std::vector<float> normals;
|
||||
std::vector<float> texcoords;
|
||||
@ -91,11 +91,11 @@ int main(int argc, char* argv[])
|
||||
char materialPrefixPath[MAX_PATH_LEN];
|
||||
b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN);
|
||||
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
|
||||
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];
|
||||
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++)
|
||||
{
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
tinyobj::material_t mat = shape.material;
|
||||
bt_tinyobj::shape_t& shape = shapes[s];
|
||||
bt_tinyobj::material_t mat = shape.material;
|
||||
|
||||
b3HashString key = mat.name.length() ? mat.name.c_str() : "";
|
||||
if (!gMaterialNames.find(key))
|
||||
@ -212,7 +212,7 @@ int main(int argc, char* argv[])
|
||||
|
||||
int faceCount = shapeCon->indices.size();
|
||||
int vertexCount = shapeCon->positions.size();
|
||||
tinyobj::material_t mat = shapeCon->material;
|
||||
bt_tinyobj::material_t mat = shapeCon->material;
|
||||
if (shapeCon->m_matName.length())
|
||||
{
|
||||
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++)
|
||||
{
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
bt_tinyobj::shape_t& shape = shapes[s];
|
||||
|
||||
if (shape.name.length())
|
||||
{
|
||||
@ -351,7 +351,7 @@ int main(int argc, char* argv[])
|
||||
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
int vertexCount = attribute.vertices.size();
|
||||
tinyobj::material_t mat = shape.material;
|
||||
bt_tinyobj::material_t mat = shape.material;
|
||||
if (shape.name.length())
|
||||
{
|
||||
const char* objName = shape.name.c_str();
|
||||
|
||||
Binary file not shown.
@ -2273,7 +2273,7 @@ int BulletMJCFImporter::getBodyUniqueId() const
|
||||
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();
|
||||
compound->setMargin(collisionMargin);
|
||||
@ -2285,7 +2285,7 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t&
|
||||
{
|
||||
btConvexHullShape* convexHull = new btConvexHullShape();
|
||||
convexHull->setMargin(collisionMargin);
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
bt_tinyobj::shape_t& shape = shapes[s];
|
||||
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
|
||||
@ -2399,9 +2399,9 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
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
|
||||
|
||||
childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
|
||||
|
||||
@ -64,8 +64,8 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
|
||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||
btVector3 shift(0, 0, 0);
|
||||
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
{
|
||||
B3_PROFILE("tinyobj::LoadObj");
|
||||
std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO);
|
||||
@ -79,7 +79,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
|
||||
//try to load some texture
|
||||
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[1] = shape.material.diffuse[1];
|
||||
meshData.m_rgbaColor[2] = shape.material.diffuse[2];
|
||||
|
||||
@ -11,8 +11,8 @@
|
||||
struct CachedObjResult
|
||||
{
|
||||
std::string m_msg;
|
||||
std::vector<tinyobj::shape_t> m_shapes;
|
||||
tinyobj::attrib_t m_attribute;
|
||||
std::vector<bt_tinyobj::shape_t> m_shapes;
|
||||
bt_tinyobj::attrib_t m_attribute;
|
||||
};
|
||||
|
||||
static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
|
||||
@ -32,8 +32,8 @@ void b3EnableFileCaching(int enable)
|
||||
}
|
||||
|
||||
std::string LoadFromCachedOrFromObj(
|
||||
tinyobj::attrib_t& attribute,
|
||||
std::vector<tinyobj::shape_t>& shapes, // [output]
|
||||
bt_tinyobj::attrib_t& attribute,
|
||||
std::vector<bt_tinyobj::shape_t>& shapes, // [output]
|
||||
const char* filename,
|
||||
const char* mtl_basepath,
|
||||
struct CommonFileIOInterface* fileIO)
|
||||
@ -47,7 +47,7 @@ std::string LoadFromCachedOrFromObj(
|
||||
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;
|
||||
result.m_msg = err;
|
||||
result.m_shapes = shapes;
|
||||
@ -62,10 +62,10 @@ std::string LoadFromCachedOrFromObj(
|
||||
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO)
|
||||
{
|
||||
B3_PROFILE("LoadMeshFromObj");
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
{
|
||||
B3_PROFILE("tinyobj::LoadObj2");
|
||||
B3_PROFILE("bt_tinyobj::LoadObj2");
|
||||
std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO);
|
||||
}
|
||||
|
||||
|
||||
@ -9,8 +9,8 @@ int b3IsFileCachingEnabled();
|
||||
void b3EnableFileCaching(int enable);
|
||||
|
||||
std::string LoadFromCachedOrFromObj(
|
||||
tinyobj::attrib_t& attribute,
|
||||
std::vector<tinyobj::shape_t>& shapes, // [output]
|
||||
bt_tinyobj::attrib_t& attribute,
|
||||
std::vector<bt_tinyobj::shape_t>& shapes, // [output]
|
||||
const char* filename,
|
||||
const char* mtl_basepath,
|
||||
struct CommonFileIOInterface* fileIO);
|
||||
|
||||
@ -9,7 +9,7 @@
|
||||
#include "../../OpenGLWindow/GLInstancingRenderer.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>;
|
||||
{
|
||||
@ -19,7 +19,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
|
||||
|
||||
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();
|
||||
|
||||
for (int f = 0; f < faceCount; f += 3)
|
||||
@ -36,7 +36,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
|
||||
}
|
||||
|
||||
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[1] = attribute.vertices[3 * v_0.vertex_index + 1];
|
||||
vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2];
|
||||
@ -65,7 +65,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
|
||||
}
|
||||
|
||||
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[1] = attribute.vertices[3 * v_1.vertex_index + 1];
|
||||
vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2];
|
||||
@ -94,7 +94,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
|
||||
}
|
||||
|
||||
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[1] = attribute.vertices[3 * v_2.vertex_index + 1];
|
||||
vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2];
|
||||
|
||||
@ -4,6 +4,6 @@
|
||||
#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
|
||||
#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
|
||||
|
||||
@ -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
|
||||
{
|
||||
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;
|
||||
jointUpperLimit = 0.f;
|
||||
@ -510,6 +516,7 @@ bool BulletURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2jo
|
||||
jointFriction = pj->m_jointFriction;
|
||||
jointMaxForce = pj->m_effortLimit;
|
||||
jointMaxVelocity = pj->m_velocityLimit;
|
||||
twistLimit = pj->m_twistLimit;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
@ -540,7 +547,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
|
||||
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");
|
||||
btCompoundShape* compound = new btCompoundShape();
|
||||
@ -553,7 +560,7 @@ static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& att
|
||||
{
|
||||
btConvexHullShape* convexHull = new btConvexHullShape();
|
||||
convexHull->setMargin(gUrdfDefaultCollisionMargin);
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
bt_tinyobj::shape_t& shape = shapes[s];
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
|
||||
for (int f = 0; f < faceCount; f += 3)
|
||||
@ -747,9 +754,9 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
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
|
||||
shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
|
||||
m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
|
||||
|
||||
@ -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 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 void setRootTransformInWorld(const btTransform& rootTransformInWorld);
|
||||
|
||||
@ -10,6 +10,9 @@
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h"
|
||||
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
#include "URDF2Bullet.h"
|
||||
#include "URDFImporterInterface.h"
|
||||
@ -259,8 +262,8 @@ btTransform ConvertURDF2BulletInternal(
|
||||
btScalar jointFriction;
|
||||
btScalar jointMaxForce;
|
||||
btScalar jointMaxVelocity;
|
||||
|
||||
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
|
||||
btScalar twistLimit;
|
||||
bool hasParentJoint = u2b.getJointInfo3(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity, twistLimit);
|
||||
std::string linkName = u2b.getLinkName(urdfLinkIndex);
|
||||
|
||||
if (flags & CUF_USE_SDF)
|
||||
@ -422,6 +425,20 @@ btTransform ConvertURDF2BulletInternal(
|
||||
cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(),
|
||||
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
|
||||
{
|
||||
|
||||
@ -64,6 +64,13 @@ public:
|
||||
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 void setRootTransformInWorld(const btTransform& rootTransformInWorld) {}
|
||||
|
||||
|
||||
@ -1455,6 +1455,7 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
|
||||
joint.m_velocityLimit = 0.f;
|
||||
joint.m_jointDamping = 0.f;
|
||||
joint.m_jointFriction = 0.f;
|
||||
joint.m_twistLimit = -1;
|
||||
|
||||
if (m_parseSDF)
|
||||
{
|
||||
@ -1470,6 +1471,12 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
|
||||
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");
|
||||
if (effort_xml)
|
||||
{
|
||||
@ -1502,6 +1509,14 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
|
||||
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
|
||||
const char* effort_str = config->Attribute("effort");
|
||||
if (effort_str)
|
||||
@ -1509,6 +1524,7 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
|
||||
joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
|
||||
}
|
||||
|
||||
|
||||
// Get joint velocity limit
|
||||
const char* velocity_str = config->Attribute("velocity");
|
||||
if (velocity_str)
|
||||
|
||||
@ -189,13 +189,15 @@ struct UrdfJoint
|
||||
|
||||
double m_jointDamping;
|
||||
double m_jointFriction;
|
||||
double m_twistLimit;
|
||||
UrdfJoint()
|
||||
: m_lowerLimit(0),
|
||||
m_upperLimit(-1),
|
||||
m_effortLimit(0),
|
||||
m_velocityLimit(0),
|
||||
m_jointDamping(0),
|
||||
m_jointFriction(0)
|
||||
m_jointFriction(0),
|
||||
m_twistLimit(-1)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
@ -3455,34 +3455,46 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
|
||||
}
|
||||
}
|
||||
|
||||
// Because the link order between UrdfModel and MultiBody may be different,
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
// Add user data specified in URDF to the added body.
|
||||
const UrdfModel* urdfModel = u2b.getUrdfModel();
|
||||
if (urdfModel)
|
||||
{
|
||||
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);
|
||||
int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
|
||||
if (linkIndex)
|
||||
btMultiBody* mb = bodyHandle->m_multiBody;
|
||||
// Because the link order between UrdfModel and MultiBody may be different,
|
||||
// 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)
|
||||
{
|
||||
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
|
||||
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
|
||||
|
||||
//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");
|
||||
if (compound == 0)
|
||||
{
|
||||
@ -5319,7 +5331,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
|
||||
{
|
||||
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
|
||||
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();
|
||||
|
||||
for (int f = 0; f < faceCount; f += 3)
|
||||
@ -8100,26 +8112,21 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
|
||||
{
|
||||
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--)
|
||||
{
|
||||
btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
|
||||
btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
|
||||
btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
|
||||
for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
|
||||
{
|
||||
const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
|
||||
// calculate normal and tangent impulse
|
||||
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
|
||||
//convert rigidbody contact
|
||||
int linkIndexA = -1;
|
||||
int linkIndexB = -1;
|
||||
int objectIndexA = psb->getUserIndex2();
|
||||
@ -8136,6 +8143,8 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
|
||||
linkIndexB = mblB->m_link;
|
||||
objectIndexB = mblB->m_multiBody->getUserIndex2();
|
||||
}
|
||||
|
||||
//apply the filter, if the user provides it
|
||||
bool swap = false;
|
||||
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
|
||||
{
|
||||
@ -8181,87 +8190,37 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (idx < 0)
|
||||
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++)
|
||||
{
|
||||
// add new node and contact point
|
||||
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)
|
||||
{
|
||||
if (swap)
|
||||
{
|
||||
pt.m_contactNormalOnBInWS[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_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
|
||||
pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j];
|
||||
pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j];
|
||||
}
|
||||
pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm();
|
||||
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++)
|
||||
else
|
||||
{
|
||||
pt.m_linearFrictionDirection1[j] = contact->t1[j];
|
||||
pt.m_linearFrictionDirection2[j] = contact->t2[j];
|
||||
pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[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
|
||||
b3ContactPointData* pt = &distinctContactPoints[idx];
|
||||
// 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();
|
||||
pt.m_linearFrictionDirection1[j] = 0;
|
||||
pt.m_linearFrictionDirection2[j] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
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]);
|
||||
m_data->m_cachedContactPoints.push_back(pt);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@ -9055,12 +9014,12 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
|
||||
}
|
||||
if (out_sim_type == UrdfGeometry::FILE_OBJ)
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
|
||||
if (!shapes.empty())
|
||||
{
|
||||
const tinyobj::shape_t& shape = shapes[0];
|
||||
const bt_tinyobj::shape_t& shape = shapes[0];
|
||||
btAlignedObjectArray<btScalar> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
for (int i = 0; i < attribute.vertices.size(); i++)
|
||||
@ -9204,14 +9163,14 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
|
||||
}
|
||||
else
|
||||
{
|
||||
tinyobj::attrib_t attribute;
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
bt_tinyobj::attrib_t attribute;
|
||||
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++)
|
||||
{
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
bt_tinyobj::shape_t& shape = shapes[s];
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
int vertexCount = attribute.vertices.size() / 3;
|
||||
for (int v = 0; v < vertexCount; v++)
|
||||
@ -9226,9 +9185,9 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
|
||||
{
|
||||
continue;
|
||||
}
|
||||
tinyobj::index_t v_0 = shape.mesh.indices[f];
|
||||
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_0 = shape.mesh.indices[f];
|
||||
bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
|
||||
bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
|
||||
btSoftBody::RenderFace ff;
|
||||
ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
|
||||
ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];
|
||||
|
||||
@ -38,8 +38,8 @@ TestLoadObj(
|
||||
|
||||
std::cout << "Loading " << fullPath << std::endl;
|
||||
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, fullPath, prefix[index]);
|
||||
std::vector<bt_tinyobj::shape_t> shapes;
|
||||
std::string err = bt_tinyobj::LoadObj(shapes, fullPath, prefix[index]);
|
||||
|
||||
if (!err.empty())
|
||||
{
|
||||
|
||||
@ -31,7 +31,7 @@
|
||||
#include "tiny_obj_loader.h"
|
||||
#include <stdio.h>
|
||||
|
||||
namespace tinyobj
|
||||
namespace bt_tinyobj
|
||||
{
|
||||
#ifdef USE_STREAM
|
||||
//See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
|
||||
@ -880,4 +880,4 @@ LoadObj(
|
||||
return err.str();
|
||||
}
|
||||
|
||||
}; // namespace tinyobj
|
||||
}; // namespace bt_tinyobj
|
||||
|
||||
@ -3,8 +3,8 @@
|
||||
//
|
||||
// Licensed under 2-clause BSD liecense.
|
||||
//
|
||||
#ifndef _TINY_OBJ_LOADER_H
|
||||
#define _TINY_OBJ_LOADER_H
|
||||
#ifndef _BT_TINY_OBJ_LOADER_H
|
||||
#define _BT_TINY_OBJ_LOADER_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
@ -12,7 +12,7 @@
|
||||
|
||||
struct CommonFileIOInterface;
|
||||
|
||||
namespace tinyobj
|
||||
namespace bt_tinyobj
|
||||
{
|
||||
struct vertex_index_t
|
||||
{
|
||||
@ -94,6 +94,6 @@ LoadObj(
|
||||
CommonFileIOInterface* fileIO);
|
||||
#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
56
examples/pybullet/examples/spherical_joint_limit.py
Normal 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.)
|
||||
@ -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>
|
||||
@ -25,11 +25,28 @@ class EnvRandomizerBase(object):
|
||||
pass
|
||||
|
||||
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:
|
||||
env: The Minitaur gym environment to be randomized.
|
||||
"""
|
||||
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
|
||||
|
||||
@ -10,7 +10,9 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
|
||||
parentdir = os.path.dirname(os.path.dirname(parentdir))
|
||||
os.sys.path.insert(0, parentdir)
|
||||
|
||||
import functools
|
||||
import math
|
||||
import gin
|
||||
import numpy as np
|
||||
from pybullet_envs.minitaur.envs import env_randomizer_base
|
||||
|
||||
@ -23,6 +25,7 @@ _VERTICAL_FORCE_UPPER_BOUND = 300
|
||||
_VERTICAL_FORCE_LOWER_BOUND = 500
|
||||
|
||||
|
||||
@gin.configurable
|
||||
class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
|
||||
"""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])
|
||||
self._vertical_force_bound = (vertical_force_bound if vertical_force_bound else
|
||||
[_VERTICAL_FORCE_LOWER_BOUND, _VERTICAL_FORCE_UPPER_BOUND])
|
||||
self._perturbation_parameter_dict = None
|
||||
|
||||
def randomize_env(self, env):
|
||||
"""Randomizes the simulation environment.
|
||||
@ -64,9 +68,10 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
|
||||
pass
|
||||
|
||||
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:
|
||||
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 <
|
||||
self._perturbation_duration_steps) and (env.env_step_counter >=
|
||||
self._perturbation_start_step):
|
||||
env.pybullet_client.applyExternalForce(objectUniqueId=env.minitaur.quadruped,
|
||||
linkIndex=self._applied_link_id,
|
||||
forceObj=self._applied_force,
|
||||
posObj=[0.0, 0.0, 0.0],
|
||||
flags=env.pybullet_client.LINK_FRAME)
|
||||
# Parameter of pybullet_client.applyExternalForce()
|
||||
self._perturbation_parameter_dict = dict(objectUniqueId=env.minitaur.quadruped,
|
||||
linkIndex=self._applied_link_id,
|
||||
forceObj=self._applied_force,
|
||||
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)
|
||||
|
||||
@ -457,6 +457,8 @@ class LocomotionGymEnv(gym.Env):
|
||||
for obj in self._dynamic_objects():
|
||||
obj.pre_control_step(autonomous_object.AUTONOMOUS_ACTION)
|
||||
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)
|
||||
for obj in self._dynamic_objects():
|
||||
obj.update(self.get_time_since_reset(), self._observation_dict)
|
||||
|
||||
@ -42,6 +42,7 @@ SET(BulletDynamics_SRCS
|
||||
Featherstone/btMultiBodyPoint2Point.cpp
|
||||
Featherstone/btMultiBodySliderConstraint.cpp
|
||||
Featherstone/btMultiBodySphericalJointMotor.cpp
|
||||
Featherstone/btMultiBodySphericalJointLimit.cpp
|
||||
MLCPSolvers/btDantzigLCP.cpp
|
||||
MLCPSolvers/btMLCPSolver.cpp
|
||||
MLCPSolvers/btLemkeAlgorithm.cpp
|
||||
@ -105,6 +106,9 @@ SET(Featherstone_HDRS
|
||||
Featherstone/btMultiBodyPoint2Point.h
|
||||
Featherstone/btMultiBodySliderConstraint.h
|
||||
Featherstone/btMultiBodySolverConstraint.h
|
||||
Featherstone/btMultiBodySphericalJointMotor.h
|
||||
Featherstone/btMultiBodySphericalJointLimit.h
|
||||
|
||||
)
|
||||
|
||||
SET(MLCPSolvers_HDRS
|
||||
|
||||
@ -31,7 +31,7 @@ enum btTypedMultiBodyConstraintType
|
||||
MULTIBODY_CONSTRAINT_SLIDER,
|
||||
MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
|
||||
MULTIBODY_CONSTRAINT_FIXED,
|
||||
|
||||
MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT,
|
||||
MAX_MULTIBODY_CONSTRAINT_TYPE,
|
||||
};
|
||||
|
||||
|
||||
@ -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
115
src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h
Normal 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
|
||||
@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
|
||||
{
|
||||
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)));
|
||||
if (!infoGlobal.m_splitImpulse)
|
||||
{
|
||||
@ -487,9 +487,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
|
||||
btVector3 dv = impulse * contact->m_c2;
|
||||
btSoftBody::Face* face = contact->m_face;
|
||||
|
||||
// save applied impulse
|
||||
contact->m_cti.m_impulse = impulse;
|
||||
|
||||
btVector3& v0 = face->m_n[0]->m_v;
|
||||
btVector3& v1 = face->m_n[1]->m_v;
|
||||
btVector3& v2 = face->m_n[2]->m_v;
|
||||
|
||||
@ -223,12 +223,10 @@ public:
|
||||
/* sCti is Softbody contact info */
|
||||
struct sCti
|
||||
{
|
||||
const btCollisionObject* m_colObj; /* Rigid body */
|
||||
btVector3 m_normal; /* Outward normal */
|
||||
mutable btVector3 m_impulse; /* Applied impulse */
|
||||
btScalar m_offset; /* Offset from origin */
|
||||
const btCollisionObject* m_colObj; /* Rigid body */
|
||||
btVector3 m_normal; /* Outward normal */
|
||||
btScalar m_offset; /* Offset from origin */
|
||||
btVector3 m_bary; /* Barycentric weights for faces */
|
||||
sCti() : m_impulse(0, 0, 0) {}
|
||||
};
|
||||
|
||||
/* sMedium */
|
||||
@ -897,7 +895,7 @@ public:
|
||||
int node1) const;
|
||||
bool checkLink(const Node* node0,
|
||||
const Node* node1) const;
|
||||
/* Check for existing face */
|
||||
/* Check for existring face */
|
||||
bool checkFace(int node0,
|
||||
int node1,
|
||||
int node2) const;
|
||||
|
||||
@ -36,6 +36,7 @@
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp"
|
||||
#include "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
|
||||
#include "BulletDynamics/Vehicle/btWheelInfo.cpp"
|
||||
#include "BulletDynamics/Character/btKinematicCharacterController.cpp"
|
||||
|
||||
Loading…
Reference in New Issue
Block a user