Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-11-08 00:45:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3061 from fuchuyuan/addAttributes

Browse Source 
add configurable parameters to urdf
This commit is contained in:
erwincoumans
2020-09-18 16:59:43 -07:00
committed by GitHub
parent 5ce97ffdb1 f51e92000a
commit 7f4fdb16b3
5 changed files with 57 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/Importers/ImportURDFDemo/UrdfParser.cpp
+11
View File
@@ -1186,6 +1186,12 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
deformable.m_gravFactor = urdfLexicalCast<double>(grav_xml->Attribute("value"));
}
XMLElement* cache_barycenter = config->FirstChildElement("cache_barycenter");
if (cache_barycenter)
{
deformable.m_cache_barycenter = true;
}
XMLElement* spring_xml = config->FirstChildElement("spring");
if (spring_xml)
{
@@ -1199,7 +1205,12 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
deformable.m_springCoefficients.damping_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("damping_stiffness"));
if (spring_xml->Attribute("bending_stiffness"))
{
deformable.m_springCoefficients.bending_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("bending_stiffness"));
if (spring_xml->Attribute("bending_stride"))
deformable.m_springCoefficients.bending_stride = urdfLexicalCast<int>(spring_xml->Attribute("bending_stride"));
}
}
XMLElement* corotated_xml = config->FirstChildElement("corotated");
examples/Importers/ImportURDFDemo/UrdfParser.h
+13 -11
View File
@@ -200,17 +200,18 @@ struct UrdfJoint
}
};
struct SpringCoeffcients{
double elastic_stiffness;
double damping_stiffness;
double bending_stiffness;
struct SpringCoeffcients
{
double elastic_stiffness;
double damping_stiffness;
double bending_stiffness;
int damp_all_directions;
SpringCoeffcients():
elastic_stiffness(0.),
damping_stiffness(0.),
bending_stiffness(0.),
damp_all_directions(0){}
int bending_stride;
SpringCoeffcients() : elastic_stiffness(0.),
damping_stiffness(0.),
bending_stiffness(0.),
damp_all_directions(0),
bending_stride(2) {}
};
struct LameCoefficients
@@ -229,6 +230,7 @@ struct UrdfDeformable
double m_friction;
double m_repulsionStiffness;
double m_gravFactor;
bool m_cache_barycenter;
SpringCoeffcients m_springCoefficients;
LameCoefficients m_corotatedCoefficients;
@@ -238,7 +240,7 @@ struct UrdfDeformable
std::string m_simFileName;
btHashMap<btHashString, std::string> m_userData;
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_visualFileName(""), m_simFileName("")
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_cache_barycenter(false), m_visualFileName(""), m_simFileName("")
{
}
};
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+2 -1
View File
@@ -8785,7 +8785,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
psb->m_cfg.kDF = deformable.m_friction;
if (deformable.m_springCoefficients.bending_stiffness)
{
psb->generateBendingConstraints(2);
psb->generateBendingConstraints(deformable.m_springCoefficients.bending_stride);
}
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
@@ -8804,6 +8804,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
psb->setSelfCollision(useSelfCollision);
psb->setSpringStiffness(deformable.m_repulsionStiffness);
psb->setGravityFactor(deformable.m_gravFactor);
psb->setCacheBarycenter(deformable.m_cache_barycenter);
psb->initializeFaceTree();
}
#endif //SKIP_DEFORMABLE_BODY
src/BulletSoftBody/btSoftBody.cpp
+29 -23
View File
@@ -229,6 +229,7 @@ void btSoftBody::initDefaults()
m_maxSpeedSquared = 0;
m_repulsionStiffness = 0.5;
m_gravityFactor = 1;
m_cacheBarycenter = false;
m_fdbvnt = 0;
}
@@ -749,7 +750,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex
fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
// Check angle of attack
// cos(10º) = 0.98480
// cos(10º) = 0.98480
if (0 < n_dot_v && n_dot_v < 0.98480f)
fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
@@ -835,7 +836,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex
fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
// Check angle of attack
// cos(10º) = 0.98480
// cos(10º) = 0.98480
if (0 < n_dot_v && n_dot_v < 0.98480f)
fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
@@ -2821,7 +2822,6 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
btGjkEpaSolver2::sResults results;
// #define USE_QUADRATURE 1
//#define CACHE_PREV_COLLISION
// use collision quadrature point
#ifdef USE_QUADRATURE
@@ -2872,30 +2872,31 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
if (dst >= 0)
return false;
// Use consistent barycenter to recalculate distance.
#ifdef CACHE_PREV_COLLISION
if (f.m_pcontact[3] != 0)
// Use consistent barycenter to recalculate distance.
if (this->m_cacheBarycenter)
{
for (int i = 0; i < 3; ++i)
bary[i] = f.m_pcontact[i];
contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
cti.m_colObj = colObjWrap->getCollisionObject();
dst = results.distance;
cti.m_normal = results.normal;
cti.m_offset = dst;
if (f.m_pcontact[3] != 0)
{
for (int i = 0; i < 3; ++i)
bary[i] = f.m_pcontact[i];
contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
cti.m_colObj = colObjWrap->getCollisionObject();
dst = results.distance;
cti.m_normal = results.normal;
cti.m_offset = dst;
//point-convex CD
wtr = colObjWrap->getWorldTransform();
btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
triangle_transform.setOrigin(f.m_n[0]->m_x);
btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
//point-convex CD
wtr = colObjWrap->getWorldTransform();
btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
triangle_transform.setOrigin(f.m_n[0]->m_x);
btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
dst = results.distance - csh->getMargin() - margin;
return true;
dst = results.distance - csh->getMargin() - margin;
return true;
}
}
#endif
// Use triangle-convex CD.
wtr = colObjWrap->getWorldTransform();
@@ -3445,6 +3446,11 @@ void btSoftBody::setGravityFactor(btScalar gravFactor)
m_gravityFactor = gravFactor;
}
void btSoftBody::setCacheBarycenter(bool cacheBarycenter)
{
m_cacheBarycenter = cacheBarycenter;
}
void btSoftBody::initializeDmInverse()
{
btScalar unit_simplex_measure = 1. / 6.;
src/BulletSoftBody/btSoftBody.h
+2
View File
@@ -840,6 +840,7 @@ public:
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
btScalar m_repulsionStiffness;
btScalar m_gravityFactor;
bool m_cacheBarycenter;
btAlignedObjectArray<btVector3> m_X; // initial positions
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
@@ -1189,6 +1190,7 @@ public:
void dampClusters();
void setSpringStiffness(btScalar k);
void setGravityFactor(btScalar gravFactor);
void setCacheBarycenter(bool cacheBarycenter);
void initializeDmInverse();
void updateDeformation();
void advanceDeformation();