From a9449601cd8bec279f2e34c96f1e237529e83c97 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sun, 3 May 2020 00:51:46 -0700 Subject: [PATCH 01/25] add multibody split impulse when solving contact with deformable --- src/BulletDynamics/Dynamics/btRigidBody.h | 6 ++ .../Featherstone/btMultiBody.cpp | 2 + src/BulletDynamics/Featherstone/btMultiBody.h | 26 ++++++ .../Featherstone/btMultiBodyDynamicsWorld.cpp | 2 + .../btDeformableContactConstraint.cpp | 79 ++++++++++++++++++- .../btDeformableContactConstraint.h | 3 + .../btDeformableMultiBodyConstraintSolver.cpp | 4 +- 7 files changed, 119 insertions(+), 3 deletions(-) diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 943d724cc..0a597d90c 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -465,6 +465,12 @@ public: //for kinematic objects, we could also use use: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } + + btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const + { + //we also calculate lin/ang velocity for kinematic objects + return m_pushVelocity + m_turnVelocity.cross(rel_pos); + } void translate(const btVector3& v) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a1d5bb9ca..9862bd2e2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -344,6 +344,8 @@ void btMultiBody::finalizeMultiDof() { m_deltaV.resize(0); m_deltaV.resize(6 + m_dofCount); + m_splitV.resize(0); + m_splitV.resize(6 + m_dofCount); m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) m_matrixBuf.resize(m_links.size() + 1); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index be795633f..f2acfab9b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -278,6 +278,11 @@ public: { return &m_deltaV[0]; } + + const btScalar *getSplitVelocityVector() const + { + return &m_splitV[0]; + } /* btScalar * getVelocityVector() { return &real_buf[0]; @@ -397,6 +402,26 @@ public: m_deltaV[dof] += delta_vee[dof] * multiplier; } } + void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] += delta_vee[dof] * multiplier; + } + } + void addSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], 1); + } + void substractSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], -1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] = 0.f; + } + } void processDeltaVeeMultiDof2() { applyDeltaVeeMultiDof(&m_deltaV[0], 1); @@ -711,6 +736,7 @@ private: // offset size array // 0 num_links+1 rot_from_parent // + btAlignedObjectArray m_splitV; btAlignedObjectArray m_deltaV; btAlignedObjectArray m_realBuf; btAlignedObjectArray m_vectorBuf; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index cd1bad089..c41c96a02 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -579,6 +579,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) for (int b = 0; b < m_multiBodies.size(); b++) { btMultiBody* bod = m_multiBodies[b]; + bod->addSplitV(); bool isSleeping = false; if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { @@ -615,6 +616,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) { bod->clearVelocities(); } + bod->substractSplitV(); } } diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index dda9d5ef6..8d6d0173d 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -210,6 +210,57 @@ btVector3 btDeformableRigidContactConstraint::getVa() const return va; } +btVector3 btDeformableRigidContactConstraint::getSplitVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { const btSoftBody::sCti& cti = m_contact->m_cti; @@ -312,8 +363,10 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo btScalar MAX_PENETRATION_CORRECTION = 0.1; const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 vb = getSplitVb(); + btVector3 va = getSplitVa(); btScalar p = m_penetration; - btScalar dn = btDot(vb, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; if (dn > 0) { return 0; @@ -329,8 +382,30 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo m_total_split_impulse += dn; btScalar residualSquare = dn*dn; - const btVector3 impulse = 1.0/m_contact->m_c2 * (cti.m_normal * dn); + const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn); applySplitImpulse(impulse); + + // apply split impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyPushImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); + } + } return residualSquare; } /* ================ Node vs. Rigid =================== */ diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 9c4f58ca1..7997c762a 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -165,6 +165,9 @@ public: // get the split impulse velocity of the deformable face at the contact point virtual btVector3 getSplitVb() const = 0; + // get the split impulse velocity of the rigid/multibdoy at the contaft + virtual btVector3 getSplitVa() const; + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual void setPenetrationScale(btScalar scale) diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index e289d87a5..8c1f4bd7b 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -24,6 +24,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + maxIterations = 150; for (int iteration = 0; iteration < maxIterations; iteration++) { // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body @@ -113,7 +114,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI if (infoGlobal.m_splitImpulse) { { - for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + for (iteration = 0; iteration < 150; iteration++) +// for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { btScalar leastSquaresResidual = 0.f; { From 9a35ccfd5f23d0b94dd6aa3ac2e9c70dfec92b00 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sun, 3 May 2020 00:51:53 -0700 Subject: [PATCH 02/25] demo changes --- examples/DeformableDemo/DeformableRigid.cpp | 44 ++++++++++++++++++++- examples/DeformableDemo/GraspDeformable.cpp | 3 +- 2 files changed, 45 insertions(+), 2 deletions(-) diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 4e5191caf..1d223ff0d 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -53,6 +53,48 @@ public: //use a smaller internal timestep, there are stability issues float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + +// +// btCollisionShape* boxShape = new btBoxShape(btVector3(1, 1, 1)); +// boxShape->setMargin(1e-3); +// if (0) +// { +// btVector3 p(0.99,1.01,0.99); +// for (int i = 0; i < 40; ++i) +// { +// p[1] -= 0.001; +// btScalar margin(.000001); +// btTransform trans; +// trans.setIdentity(); +// btGjkEpaSolver2::sResults results; +// const btConvexShape* csh = static_cast(boxShape); +// btScalar d = btGjkEpaSolver2::SignedDistance(p, margin, csh, trans, results); +// printf("d = %f\n", d); +// printf("----\n"); +// } +// } +// +// btVector3 p(.991,1.01,.99); +// for (int i = 0; i < 40; ++i) +// { +// p[1] -= 0.001; +// btScalar margin(.006); +// btTransform trans; +// trans.setIdentity(); +// btScalar dst; +// btGjkEpaSolver2::sResults results; +// btTransform point_transform; +// point_transform.setIdentity(); +// point_transform.setOrigin(p); +// btSphereShape sphere(margin); +// btVector3 guess(0,0,0); +// const btConvexShape* csh = static_cast(boxShape); +// btGjkEpaSolver2::SignedDistance(&sphere, point_transform, csh, trans, guess, results); +// dst = results.distance-csh->getMargin(); +// dst -= margin; +// printf("d = %f\n", dst); +// printf("----\n"); +// } } void Ctor_RbUpStack(int count) @@ -214,7 +256,7 @@ void DeformableRigid::initPhysics() 2,2, 0, true); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.5); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 0fede1b56..178e8c9c2 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -361,8 +361,9 @@ void GraspDeformable::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 1; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; +// psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true)); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1)); From 7069d51eae64acf894b83fc1a32934aedcfa238e Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 8 May 2020 16:37:17 -0700 Subject: [PATCH 03/25] Reduce deformable erp for more stability --- src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 4356c12ab..b830c1318 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -83,7 +83,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.1); + m_deformable_erp = btScalar(0.01); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); From cb1cd969a8d62e6ed775649d61c2f3a7f94edf8f Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 8 May 2020 16:51:53 -0700 Subject: [PATCH 04/25] allow positive penetration to pose inactive constraints for more stability --- .../btDeformableContactConstraint.cpp | 50 +++++++++++-------- .../btDeformableContactProjection.cpp | 10 +--- .../btDeformableMultiBodyConstraintSolver.cpp | 4 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 3 +- src/BulletSoftBody/btSoftBody.cpp | 4 +- 5 files changed, 34 insertions(+), 37 deletions(-) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 8d6d0173d..5ef809384 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -267,28 +267,30 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - btScalar dn = btDot(vr, cti.m_normal); - if (!infoGlobal.m_splitImpulse) - { - dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; - } + btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.deformable_cfm; + if (m_penetration > 0) + { + dn += m_penetration / infoGlobal.m_timeStep; + } + if (!infoGlobal.m_splitImpulse) + { + dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + } // 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; - if (!infoGlobal.m_splitImpulse) - { - impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); - } + btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0,0,0))); + if (!infoGlobal.m_splitImpulse) + { + impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); + } btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); btVector3 impulse_tangent = impulse - impulse_normal; - if (dn > 0) - { - dn = 0; - impulse_normal.setZero(); - m_binding = false; - return 0; - } - m_binding = true; - btScalar residualSquare = dn*dn; + if (dn > 0) + { + m_binding = false; + return 0; + } + m_binding = true; + btScalar residualSquare = dn*dn; btVector3 old_total_tangent_dv = m_total_tangent_dv; // m_c2 is the inverse mass of the deformable node/face m_total_normal_dv -= impulse_normal * m_contact->m_c2; @@ -360,12 +362,16 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { - btScalar MAX_PENETRATION_CORRECTION = 0.1; + btScalar MAX_PENETRATION_CORRECTION = 0.01; const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 vb = getSplitVb(); - btVector3 va = getSplitVa(); + btVector3 va = getSplitVa(); btScalar p = m_penetration; - btVector3 vr = vb - va; + if (p > 0) + { + return 0; + } + btVector3 vr = vb - va; btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; if (dn > 0) { diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 4e88c79d9..a0e184f43 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -126,15 +126,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_nodeRigidConstraints[i].push_back(constraint); - } + m_nodeRigidConstraints[i].push_back(constraint); } // set Deformable Face vs. Rigid constraint diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 8c1f4bd7b..e289d87a5 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -24,7 +24,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - maxIterations = 150; for (int iteration = 0; iteration < maxIterations; iteration++) { // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body @@ -114,8 +113,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI if (infoGlobal.m_splitImpulse) { { - for (iteration = 0; iteration < 150; iteration++) -// for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { btScalar leastSquaresResidual = 0.f; { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 3fc3ce964..969ff1ad0 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -532,12 +532,13 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) if (m_useProjection) { m_deformableBodySolver->m_useProjection = true; -// m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; } else { m_deformableBodySolver->m_useProjection = false; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index e70e94f5f..1616340ee 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2943,8 +2943,8 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btVector3 guess(0,0,0); const btConvexShape* csh = static_cast(shp); btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance-csh->getMargin(); - dst -= margin; + dst = results.distance-2.0*csh->getMargin(); + dst -= 2.0*margin; if (dst >= 0) return false; wtr = colObjWrap->getWorldTransform(); From e4df217284ad73ed94c1af8e87c961d23f03a6b4 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 12:05:02 -0700 Subject: [PATCH 05/25] bug fix --- src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h | 2 ++ src/BulletSoftBody/btDeformableContactConstraint.cpp | 6 +++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index b830c1318..0fe4969ac 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -47,6 +47,7 @@ struct btContactSolverInfoData btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_cfm; //constraint force mixing for deformable constraints btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -84,6 +85,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); m_deformable_erp = btScalar(0.01); + m_deformable_cfm = btScalar(0.01); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 5ef809384..907639bb2 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -267,7 +267,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.deformable_cfm; + btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm; if (m_penetration > 0) { dn += m_penetration / infoGlobal.m_timeStep; @@ -277,7 +277,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; } // 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.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) { impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); @@ -362,7 +362,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { - btScalar MAX_PENETRATION_CORRECTION = 0.01; + btScalar MAX_PENETRATION_CORRECTION = 0.1; const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 vb = getSplitVb(); btVector3 va = getSplitVa(); From 3054cc01a163282a951335364a69dfbd37ac1d03 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 12:05:35 -0700 Subject: [PATCH 06/25] demo parameter tune --- examples/DeformableDemo/GraspDeformable.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 178e8c9c2..fecbf1432 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -198,6 +198,9 @@ void GraspDeformable::initPhysics() btVector3 gravity = btVector3(0, -9.81, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_maxPickingForce = 0.001; // build a gripper @@ -253,7 +256,7 @@ void GraspDeformable::initPhysics() //create a ground { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.))); - + groundShape->setMargin(0.001); m_collisionShapes.push_back(groundShape); btTransform groundTransform; @@ -354,14 +357,13 @@ void GraspDeformable::initPhysics() psb->getCollisionShape()->setMargin(0.003); psb->generateBendingConstraints(2); - psb->setTotalMass(0.005); + psb->setTotalMass(0.01); psb->setSpringStiffness(10); psb->setDampingCoefficient(0.05); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 1; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; -// psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); From 607ddc25b7e74df9a56b9a051d7a423b5b56cb57 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 15:53:54 -0700 Subject: [PATCH 07/25] make deformable collision detection wider --- src/BulletSoftBody/btSoftBody.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 1616340ee..c619191d8 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2944,7 +2944,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO const btConvexShape* csh = static_cast(shp); btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); dst = results.distance-2.0*csh->getMargin(); - dst -= 2.0*margin; + dst -= 4.0*margin; if (dst >= 0) return false; wtr = colObjWrap->getWorldTransform(); @@ -4085,7 +4085,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap { const btTransform wtr = pcoWrap->getWorldTransform(); const btScalar timemargin = 0; - const btScalar basemargin = getCollisionShape()->getMargin(); + const btScalar basemargin = 3.0*getCollisionShape()->getMargin(); btVector3 mins; btVector3 maxs; ATTRIBUTE_ALIGNED16(btDbvtVolume) From 5f2ac5e922548e32157b004a6ba4c70cfe440a13 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 15:54:15 -0700 Subject: [PATCH 08/25] parameter tuning --- examples/DeformableDemo/ClothFriction.cpp | 4 ++-- examples/DeformableDemo/DeformableContact.cpp | 4 ++-- examples/DeformableDemo/DeformableMultibody.cpp | 3 ++- examples/DeformableDemo/DeformableRigid.cpp | 2 +- examples/DeformableDemo/GraspDeformable.cpp | 2 +- examples/DeformableDemo/MultibodyClothAnchor.cpp | 2 +- examples/DeformableDemo/Pinch.cpp | 2 +- examples/DeformableDemo/PinchFriction.cpp | 6 +++--- examples/DeformableDemo/SplitImpulse.cpp | 2 +- examples/DeformableDemo/VolumetricDeformable.cpp | 2 +- src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h | 2 +- src/BulletSoftBody/btConjugateResidual.h | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- src/BulletSoftBody/btPreconditioner.h | 2 +- 14 files changed, 19 insertions(+), 18 deletions(-) diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp index ef4908766..0533ac1d0 100644 --- a/examples/DeformableDemo/ClothFriction.cpp +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -144,7 +144,7 @@ void ClothFriction::initPhysics() 10,10, 0, true); - psb->getCollisionShape()->setMargin(0.05); + psb->getCollisionShape()->setMargin(0.04); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->setSpringStiffness(100); @@ -173,7 +173,7 @@ void ClothFriction::initPhysics() btVector3(+s, h, +s), 5,5, 0, true); - psb2->getCollisionShape()->setMargin(0.05); + psb2->getCollisionShape()->setMargin(0.04); psb2->generateBendingConstraints(2); psb2->setTotalMass(1); psb2->setSpringStiffness(100); diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index cb83d1441..b1ad0b67b 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -143,7 +143,7 @@ void DeformableContact::initPhysics() 20,20, 1 + 2 + 4 + 8, true); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.05); psb->generateBendingConstraints(2); psb->setSpringStiffness(10); psb->setTotalMass(1); @@ -172,7 +172,7 @@ void DeformableContact::initPhysics() btVector3(+s, h, +s), 10,10, 0, true); - psb2->getCollisionShape()->setMargin(0.1); + psb2->getCollisionShape()->setMargin(0.05); psb2->generateBendingConstraints(2); psb2->setSpringStiffness(10); psb2->setTotalMass(1); diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index b8d4a86ad..041624b00 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -192,7 +192,7 @@ void DeformableMultibody::initPhysics() // 3,3, 1 + 2 + 4 + 8, true); - psb->getCollisionShape()->setMargin(0.25); + psb->getCollisionShape()->setMargin(0.025); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects @@ -341,6 +341,7 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; btCollisionShape* box = new btBoxShape(baseHalfExtents); + box->setMargin(0.01); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); col->setCollisionShape(box); diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 1d223ff0d..7efe118a6 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -256,7 +256,7 @@ void DeformableRigid::initPhysics() 2,2, 0, true); - psb->getCollisionShape()->setMargin(0.5); + psb->getCollisionShape()->setMargin(0.05); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index fecbf1432..f52f67fe9 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -355,7 +355,7 @@ void GraspDeformable::initPhysics() 2,2, 0, true); - psb->getCollisionShape()->setMargin(0.003); + psb->getCollisionShape()->setMargin(0.001); psb->generateBendingConstraints(2); psb->setTotalMass(0.01); psb->setSpringStiffness(10); diff --git a/examples/DeformableDemo/MultibodyClothAnchor.cpp b/examples/DeformableDemo/MultibodyClothAnchor.cpp index 534cad828..ab78efa68 100644 --- a/examples/DeformableDemo/MultibodyClothAnchor.cpp +++ b/examples/DeformableDemo/MultibodyClothAnchor.cpp @@ -143,7 +143,7 @@ void MultibodyClothAnchor::initPhysics() btVector3(+s, h, -s), btVector3(-s, h, +s), btVector3(+s, h, +s), r, r, 4 + 8, true); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.01); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index 4f294c7b6..dbf1e6c80 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -296,7 +296,7 @@ void Pinch::initPhysics() psb->scale(btVector3(2, 2, 2)); psb->translate(btVector3(0, 4, 0)); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.01); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index 38ca03e23..5d31331b6 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -265,7 +265,7 @@ void PinchFriction::initPhysics() psb->scale(btVector3(2, 2, 1)); psb->translate(btVector3(0, 2.1, 2.2)); - psb->getCollisionShape()->setMargin(0.05); + psb->getCollisionShape()->setMargin(0.025); psb->setSpringStiffness(10); psb->setTotalMass(.6); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects @@ -296,7 +296,7 @@ void PinchFriction::initPhysics() psb2->scale(btVector3(2, 2, 1)); psb2->translate(btVector3(0, 2.1, -2.2)); - psb2->getCollisionShape()->setMargin(0.05); + psb2->getCollisionShape()->setMargin(0.025); psb2->setTotalMass(.6); psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body @@ -327,7 +327,7 @@ void PinchFriction::initPhysics() psb3->scale(btVector3(2, 2, 1)); psb3->translate(btVector3(0, 2.1, 0)); - psb3->getCollisionShape()->setMargin(0.05); + psb3->getCollisionShape()->setMargin(0.025); psb3->setTotalMass(.6); psb3->setSpringStiffness(10); psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp index 306f98f96..962d49d66 100644 --- a/examples/DeformableDemo/SplitImpulse.cpp +++ b/examples/DeformableDemo/SplitImpulse.cpp @@ -160,7 +160,7 @@ void SplitImpulse::initPhysics() // 0, true); - psb->getCollisionShape()->setMargin(0.15); + psb->getCollisionShape()->setMargin(0.015); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index e37c17e0a..a60bb10d8 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -208,7 +208,7 @@ void VolumetricDeformable::initPhysics() getDeformableDynamicsWorld()->addSoftBody(psb); psb->scale(btVector3(2, 2, 2)); psb->translate(btVector3(0, 5, 0)); - psb->getCollisionShape()->setMargin(0.25); + psb->getCollisionShape()->setMargin(0.025); psb->setTotalMass(0.5); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 0fe4969ac..6968a3310 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -84,7 +84,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.01); + m_deformable_erp = btScalar(0.06); m_deformable_cfm = btScalar(0.01); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index d4739da83..1c1dcd876 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -29,7 +29,7 @@ class btConjugateResidual : public btKrylovSolver btScalar best_r; public: btConjugateResidual(const int max_it_in) - : Base(max_it_in, 1e-8) + : Base(max_it_in, 1e-6) { } diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 736fe4695..d5d72569c 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -18,7 +18,7 @@ #include "btDeformableBodySolver.h" #include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" -static const int kMaxConjugateGradientIterations = 50; +static const int kMaxConjugateGradientIterations = 300; btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) , m_cg(kMaxConjugateGradientIterations) diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index c2db448ef..4dc6fcd71 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -178,7 +178,7 @@ public: } } } -#define USE_FULL_PRECONDITIONER +//#define USE_FULL_PRECONDITIONER #ifndef USE_FULL_PRECONDITIONER virtual void operator()(const TVStack& x, TVStack& b) { From 644fd5f3112aaa9f7bedb12d1c68e0cc7e749824 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 16:42:17 -0700 Subject: [PATCH 09/25] add API for two different damping modes for mass spring --- examples/Importers/ImportURDFDemo/UrdfParser.h | 8 +++++--- examples/SharedMemory/PhysicsClientC_API.cpp | 7 ++++--- examples/SharedMemory/PhysicsClientC_API.h | 2 +- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 3 ++- examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/pybullet/pybullet.c | 7 ++++--- 6 files changed, 17 insertions(+), 11 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index d873e4d30..7cd87c49a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -205,10 +205,12 @@ 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.){} + elastic_stiffness(0.), + damping_stiffness(0.), + bending_stiffness(0.), + damp_all_directions(0){} }; struct LameCoefficients diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e65a7be93..6035d74e8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -377,12 +377,13 @@ B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle c return 0; } -B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness) +B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness, int dampAllDirections) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); - command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness; - command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness; + command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness; + command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness; + command->m_loadSoftBodyArguments.m_dampAllDirections = dampAllDirections; command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE; return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 41d1a9fba..ad8df27aa 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -647,7 +647,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping); - B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); + B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness, int dampAllDirections = 0); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 309fbc574..0c4ebe5af 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8362,6 +8362,7 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe #ifndef SKIP_DEFORMABLE_BODY deformable.m_springCoefficients.elastic_stiffness = loadSoftBodyArgs.m_springElasticStiffness; deformable.m_springCoefficients.damping_stiffness = loadSoftBodyArgs.m_springDampingStiffness; + deformable.m_springCoefficients.damp_all_directions = loadSoftBodyArgs.m_dampAllDirections; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) { deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness; @@ -8472,7 +8473,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness, deformable.m_springCoefficients.damping_stiffness, - true, deformable.m_springCoefficients.bending_stiffness); + deformable.m_springCoefficients.damp_all_directions, deformable.m_springCoefficients.bending_stiffness); deformWorld->addForce(psb, springForce); m_data->m_lf.push_back(springForce); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 3ab8776c9..f568b985b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -532,6 +532,7 @@ struct LoadSoftBodyArgs double m_initialOrientation[4]; double m_springElasticStiffness; double m_springDampingStiffness; + int m_dampAllDirections; double m_springBendingStiffness; double m_corotatedMu; double m_corotatedLambda; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e5c97d7fd..b84ffc5e6 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2038,7 +2038,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* int physicsClientId = 0; int flags = 0; - static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL}; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springDampingAllDirections", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL}; int bodyUniqueId = -1; const char* fileName = ""; @@ -2050,6 +2050,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* int useNeoHookean = 0; double springElasticStiffness = 1; double springDampingStiffness = 0.1; + int springDampingAllDirections = 0; double springBendingStiffness = 0.1; double NeoHookeanMu = 1; double NeoHookeanLambda = 1; @@ -2068,7 +2069,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* PyObject* basePosObj = 0; PyObject* baseOrnObj = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiidddddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiddidddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springDampingAllDirections, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId)) { return NULL; } @@ -2123,7 +2124,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* } if (useMassSpring) { - b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness); + b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness, springDampingAllDirections); b3LoadSoftBodyUseBendingSprings(command, useBendingSprings, springBendingStiffness); } if (useNeoHookean) From ce237ec7ec6d4a547665f6a28975f88e2dd00b22 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 17:08:33 -0700 Subject: [PATCH 10/25] fix API for different damping modes for mass spring --- examples/SharedMemory/PhysicsClientC_API.cpp | 12 ++++++++++-- examples/SharedMemory/PhysicsClientC_API.h | 3 ++- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 5 +++++ examples/SharedMemory/SharedMemoryCommands.h | 1 + 4 files changed, 18 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 6035d74e8..87e152e96 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -377,13 +377,12 @@ B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle c return 0; } -B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness, int dampAllDirections) +B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness; command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness; - command->m_loadSoftBodyArguments.m_dampAllDirections = dampAllDirections; command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE; return 0; } @@ -442,6 +441,15 @@ B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle co return 0; } +B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int allDirectionDamping) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_dampAllDirections = allDirectionDamping; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE; + return 0; +} + B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index ad8df27aa..aa0cbe967 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -647,7 +647,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping); - B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness, int dampAllDirections = 0); + B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision); @@ -655,6 +655,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness); + B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int useAllDirectionDamping); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0c4ebe5af..5f60b8b58 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8368,6 +8368,11 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness; } + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE) + { + deformable.m_springCoefficients.damp_all_directions = loadSoftBodyArgs.m_dampAllDirections; + } + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) { deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f568b985b..6a8221565 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -512,6 +512,7 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14, LOAD_SOFT_BODY_SIM_MESH = 1<<15, LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16, + LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE = 1<<17, }; enum EnumSimParamInternalSimFlags From f07042723c4027471c283a98c8fdac17182306c7 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 17:40:00 -0700 Subject: [PATCH 11/25] bug fix in damping mode --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5f60b8b58..19a14cbff 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8362,7 +8362,6 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe #ifndef SKIP_DEFORMABLE_BODY deformable.m_springCoefficients.elastic_stiffness = loadSoftBodyArgs.m_springElasticStiffness; deformable.m_springCoefficients.damping_stiffness = loadSoftBodyArgs.m_springDampingStiffness; - deformable.m_springCoefficients.damp_all_directions = loadSoftBodyArgs.m_dampAllDirections; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) { deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness; @@ -8478,7 +8477,8 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness, deformable.m_springCoefficients.damping_stiffness, - deformable.m_springCoefficients.damp_all_directions, deformable.m_springCoefficients.bending_stiffness); + !deformable.m_springCoefficients.damp_all_directions, + deformable.m_springCoefficients.bending_stiffness); deformWorld->addForce(psb, springForce); m_data->m_lf.push_back(springForce); } From b75dd02f682d4c7d1408b130b9a2c8a3c073d511 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 17:41:32 -0700 Subject: [PATCH 12/25] make cloth in deformable anchor come to stop --- examples/pybullet/examples/deformable_anchor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/examples/deformable_anchor.py b/examples/pybullet/examples/deformable_anchor.py index 1c01ed6d4..fa3d75867 100644 --- a/examples/pybullet/examples/deformable_anchor.py +++ b/examples/pybullet/examples/deformable_anchor.py @@ -15,7 +15,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True) -clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1) +clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, springDampingAllDirections = 1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1) p.createSoftBodyAnchor(clothId ,0,-1,-1) From adf9e74dcb10ea211ff23ce617757c8dd109e5c7 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 17:54:05 -0700 Subject: [PATCH 13/25] match API in pybullet.c --- examples/pybullet/pybullet.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b84ffc5e6..0e9420dbd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2124,8 +2124,9 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* } if (useMassSpring) { - b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness, springDampingAllDirections); + b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness); b3LoadSoftBodyUseBendingSprings(command, useBendingSprings, springBendingStiffness); + b3LoadSoftBodyUseAllDirectionDampingSprings(command, springDampingAllDirections); } if (useNeoHookean) { From fb751361a612c67ec71c6e5e5aadd4b47930263e Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 11 May 2020 18:50:03 -0700 Subject: [PATCH 14/25] tune deformable_torus.py so the tori don't sink into each other --- data/torus_deform.urdf | 2 +- examples/pybullet/examples/deformable_torus.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/data/torus_deform.urdf b/data/torus_deform.urdf index 7d09ac281..20fa3e1a3 100644 --- a/data/torus_deform.urdf +++ b/data/torus_deform.urdf @@ -6,7 +6,7 @@ - + diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index 7e2732fb3..0408e8e90 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/examples/pybullet/examples/deformable_torus.py @@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2]) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) -bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 50) +bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800) bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION) From a9030011e7a0de9be2695823eea1c655abe0aa0d Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 18 May 2020 15:23:57 -0700 Subject: [PATCH 15/25] add and substract split impulse velocity after sleeping check --- src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index c41c96a02..f599c9ccb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -579,7 +579,6 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) for (int b = 0; b < m_multiBodies.size(); b++) { btMultiBody* bod = m_multiBodies[b]; - bod->addSplitV(); bool isSleeping = false; if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { @@ -593,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links @@ -611,12 +611,12 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->substractSplitV(); } else { bod->clearVelocities(); } - bod->substractSplitV(); } } From 65978cb58bcd85599423241127ba61fb137ba590 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 20 May 2020 17:55:03 -0700 Subject: [PATCH 16/25] revert experimental changes to collision padding --- src/BulletSoftBody/btSoftBody.cpp | 111 +++++++----------------------- 1 file changed, 23 insertions(+), 88 deletions(-) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 4f5002d04..4382481ea 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2877,93 +2877,28 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO return (dst < 0); } #endif - -// // regular face contact -// { -// btGjkEpaSolver2::sResults results; -// btTransform triangle_transform; -// triangle_transform.setIdentity(); -// triangle_transform.setOrigin(f.m_n[0]->m_x); -// btTriangleShape triangle(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); -// btVector3 guess(0,0,0); -// if (predict) -// { -// triangle_transform.setOrigin(f.m_n[0]->m_q); -// triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); -// } -// const btConvexShape* csh = static_cast(shp); -//// btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); -//// dst = results.distance - margin; -//// contact_point = results.witnesses[0]; -// btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results); -// if (results.status == btGjkEpaSolver2::sResults::Separated) -// return false; -// dst = results.distance - margin; -// contact_point = results.witnesses[1]; -// getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); -// nrm = results.normal; -// for (int i = 0; i < 3; ++i) -// f.m_pcontact[i] = bary[i]; -// } -// -// if (!predict) -// { -// cti.m_colObj = colObjWrap->getCollisionObject(); -// cti.m_normal = nrm; -// cti.m_offset = dst; -// } -// - - // regular face contact - if (0) - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_q); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); - btVector3 guess(0,0,0); - const btConvexShape* csh = static_cast(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance-csh->getMargin(); - dst -= margin; - if (dst >= 0) - return false; - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary); - btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - nrm = results.normal; - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = nrm; - cti.m_offset = dst + (curr - contact_point).dot(nrm); - } - if(1) - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_q); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); - btVector3 guess(0,0,0); - const btConvexShape* csh = static_cast(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance-2.0*csh->getMargin(); - dst -= 4.0*margin; - if (dst >= 0) - return false; - 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); - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - dst = results.distance-csh->getMargin()-margin; - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = results.normal; - cti.m_offset = dst; - return true; - } - return true; + btGjkEpaSolver2::sResults results; + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_q); + btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); + btVector3 guess(0,0,0); + const btConvexShape* csh = static_cast(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + dst = results.distance-2.0*csh->getMargin()-margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable + if (dst >= 0) + return false; + 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); + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + dst = results.distance-csh->getMargin()-margin; + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = results.normal; + cti.m_offset = dst; + return true; } // void btSoftBody::updateNormals() @@ -4089,7 +4024,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap { const btTransform wtr = pcoWrap->getWorldTransform(); const btScalar timemargin = 0; - const btScalar basemargin = 3.0*getCollisionShape()->getMargin(); + const btScalar basemargin = getCollisionShape()->getMargin(); btVector3 mins; btVector3 maxs; ATTRIBUTE_ALIGNED16(btDbvtVolume) From d1d2dad2e66e5a0ea3e2887e1a4d90d68f13e5e8 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 20 May 2020 17:56:20 -0700 Subject: [PATCH 17/25] do not apply deformable contact impulse to points and faces constrained in rigid/deformable solve --- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- .../btDeformableContactProjection.cpp | 28 +++++++++---------- src/BulletSoftBody/btSoftBody.h | 18 ++++++------ 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index d5d72569c..321492882 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -437,7 +437,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d } n.m_q = n.m_x + n.m_v * dt; n.m_splitv.setZero(); - n.m_penetration = 0; + n.m_constrained = false; } /* Nodes */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index a0e184f43..054b92099 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -221,7 +221,7 @@ void btDeformableContactProjection::setProjection() for (int j = 0; j < m_staticConstraints[i].size(); ++j) { int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; + m_staticConstraints[i][j].m_node->m_constrained = true; if (m_projectionsDict.find(index) == NULL) { m_projectionsDict.insert(index, units); @@ -238,7 +238,7 @@ void btDeformableContactProjection::setProjection() for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) { int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; if (m_projectionsDict.find(index) == NULL) { m_projectionsDict.insert(index, units); @@ -255,7 +255,7 @@ void btDeformableContactProjection::setProjection() for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; if (m_nodeRigidConstraints[i][j].m_static) { if (m_projectionsDict.find(index) == NULL) @@ -289,15 +289,16 @@ void btDeformableContactProjection::setProjection() for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) { const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; - for (int k = 0; k < 3; ++k) - { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); - } + if (m_faceRigidConstraints[i][j].m_binding) + { + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + } + } for (int k = 0; k < 3; ++k) { btSoftBody::Node* node = face->m_n[k]; - node->m_penetration = true; int index = node->index; if (m_faceRigidConstraints[i][j].m_static) { @@ -469,7 +470,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() for (int j = 0; j < m_staticConstraints[i].size(); ++j) { int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; + m_staticConstraints[i][j].m_node->m_constrained = true; LagrangeMultiplier lm; lm.m_num_nodes = 1; lm.m_indices[0] = index; @@ -483,7 +484,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) { int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; LagrangeMultiplier lm; lm.m_num_nodes = 1; lm.m_indices[0] = index; @@ -501,7 +502,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() continue; } int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; LagrangeMultiplier lm; lm.m_num_nodes = 1; lm.m_indices[0] = index; @@ -529,12 +530,11 @@ void btDeformableContactProjection::setLagrangeMultiplier() const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; LagrangeMultiplier lm; lm.m_num_nodes = 3; for (int k = 0; k<3; ++k) { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + face->m_n[k]->m_constrained = true; lm.m_indices[k] = face->m_n[k]->index; lm.m_weights[k] = bary[k]; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 954adf4d1..63c2adf9a 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -269,7 +269,7 @@ public: btScalar m_im; // 1/mass btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data - btScalar m_penetration; // depth of penetration + int m_constrained; // depth of penetration int m_battach : 1; // Attached int index; btVector3 m_splitv; // velocity associated with split impulse @@ -1313,20 +1313,22 @@ public: I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); if (vn < 0) I += 0.5 * mass * vn; - btScalar face_penetration = 0, node_penetration = node->m_penetration; + int face_penetration = 0, node_penetration = node->m_constrained; for (int i = 0; i < 3; ++i) - face_penetration = btMax(face_penetration, face->m_n[i]->m_penetration); + face_penetration |= face->m_n[i]->m_constrained; btScalar I_tilde = .5 *I /(1.0+w.length2()); // double the impulse if node or face is constrained. if (face_penetration > 0 || node_penetration > 0) - I_tilde *= 2.0; - if (face_penetration <= node_penetration) + { + I_tilde *= 2.0; + } + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; } - if (face_penetration >= node_penetration) + if (node_penetration <= 0) { node->m_v -= I_tilde*node->m_im*n; } @@ -1344,12 +1346,12 @@ public: // double the impulse if node or face is constrained. // if (face_penetration > 0 || node_penetration > 0) // I_tilde *= 2.0; - if (face_penetration <= node_penetration) + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; } - if (face_penetration >= node_penetration) + if (node_penetration <= 0) { node->m_v -= I_tilde * node->m_im * vt; } From 04efc9124d4cb94b68717921a655f667bf30c656 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 20 May 2020 17:56:41 -0700 Subject: [PATCH 18/25] tune exampleBrowser demo parameters --- examples/DeformableDemo/ClothFriction.cpp | 4 ++-- examples/DeformableDemo/DeformableSelfCollision.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp index 0533ac1d0..1d091fa38 100644 --- a/examples/DeformableDemo/ClothFriction.cpp +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -144,7 +144,7 @@ void ClothFriction::initPhysics() 10,10, 0, true); - psb->getCollisionShape()->setMargin(0.04); + psb->getCollisionShape()->setMargin(0.06); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->setSpringStiffness(100); @@ -173,7 +173,7 @@ void ClothFriction::initPhysics() btVector3(+s, h, +s), 5,5, 0, true); - psb2->getCollisionShape()->setMargin(0.04); + psb2->getCollisionShape()->setMargin(0.06); psb2->generateBendingConstraints(2); psb2->setTotalMass(1); psb2->setSpringStiffness(100); diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index 9806bdb37..576f8f488 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -41,7 +41,7 @@ public: void resetCamera() { - float dist = 1.0; + float dist = 2.0; float pitch = -8; float yaw = 100; float targetPos[3] = {0, -1.0, 0}; @@ -93,7 +93,7 @@ void DeformableSelfCollision::initPhysics() { ///create a ground btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.))); - + groundShape->setMargin(0.0001); m_collisionShapes.push_back(groundShape); btTransform groundTransform; @@ -128,7 +128,7 @@ void DeformableSelfCollision::initPhysics() void DeformableSelfCollision::addCloth(btVector3 origin) // create a piece of cloth { - const btScalar s = 0.3; + const btScalar s = 0.6; const btScalar h = 0; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2*s), @@ -140,7 +140,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin) 0, true, 0.0); - psb->getCollisionShape()->setMargin(0.01); + psb->getCollisionShape()->setMargin(0.02); psb->generateBendingConstraints(2); psb->setTotalMass(.5); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects From 243aa4ac1f34d3830da1a41392f3bcd1245e3bb6 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 20 May 2020 17:57:16 -0700 Subject: [PATCH 19/25] switch to a lower default threshold of CR solver for fewer iterations --- src/BulletSoftBody/btConjugateResidual.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index 1c1dcd876..8fdc5d107 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -29,7 +29,7 @@ class btConjugateResidual : public btKrylovSolver btScalar best_r; public: btConjugateResidual(const int max_it_in) - : Base(max_it_in, 1e-6) + : Base(max_it_in, 1e-4) { } From 829475c38a993907b7547a2e943d5f4142c04498 Mon Sep 17 00:00:00 2001 From: Wilson Date: Sun, 24 May 2020 16:30:47 +0200 Subject: [PATCH 20/25] Set DeepMimic Gym env to query policy at 30Hz Addresses issue #2821 --- .../deep_mimic/gym_env/deep_mimic_env.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py index 55a50ef50..d2a1cd65d 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py @@ -93,6 +93,9 @@ class HumanoidDeepBulletEnv(gym.Env): self.viewer = None self._configure() + + # Query the policy at 30Hz + self.policy_query_30 = True def _configure(self, display=None): self.display = display @@ -110,7 +113,15 @@ class HumanoidDeepBulletEnv(gym.Env): #step sim self._internal_env.update(self._time_step) - + if self.policy_query_30: + # Step simulation until a new action is required + while not self._internal_env.need_new_action(agent_id): + self._p.stepSimulation() + self._internal_env.t += self._time_step + # print("t:", self._internal_env.t) + # print("next update time:", self._internal_env.needs_update_time) + # print("Performing update") + #record state self.state = self._internal_env.record_state(agent_id) From e6fdaebea9a2f6cc3c4789226d8d599315998925 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 28 May 2020 17:58:21 -0700 Subject: [PATCH 21/25] add api for setting non contact iteration for pybullet --- examples/pybullet/pybullet.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e06fe643e..2fc03350c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1649,6 +1649,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double warmStartingFactor = -1; double sparseSdfVoxelSize = -1; + int numNonContactInnerIterations = -1; int physicsClientId = 0; @@ -1678,11 +1679,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar "reportSolverAnalytics", "warmStartingFactor", "sparseSdfVoxelSize", + "numNonContactInnerIterations", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiiddii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, - &reportSolverAnalytics, &warmStartingFactor, &sparseSdfVoxelSize, &physicsClientId)) + &reportSolverAnalytics, &warmStartingFactor, &sparseSdfVoxelSize, &numNonContactInnerIterations, &physicsClientId)) { return NULL; } @@ -1811,6 +1813,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParameterSetSparseSdfVoxelSize(command, sparseSdfVoxelSize); } + if (numNonContactInnerIterations >= 1) + { + b3PhysicsParamSetNumNonContactInnerIterations(command, numNonContactInnerIterations); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } From a33a9412fa106399696ff410d26725a5cd3efb17 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 28 May 2020 19:33:38 -0700 Subject: [PATCH 22/25] add numNonContactInnerIterations to getPhysicsParameters --- examples/pybullet/pybullet.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2fc03350c..40ede7ce5 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1603,14 +1603,15 @@ static PyObject* pybullet_getPhysicsEngineParameters(PyObject* self, PyObject* a b3GetStatusPhysicsSimulationParameters(statusHandle, ¶ms); //for now, return a subset, expose more/all on request - val = Py_BuildValue("{s:d,s:i,s:i,s:i,s:d,s:d,s:d}", + val = Py_BuildValue("{s:d,s:i,s:i,s:i,s:d,s:d,s:d, s:i}", "fixedTimeStep", params.m_deltaTime, "numSubSteps", params.m_numSimulationSubSteps, "numSolverIterations", params.m_numSolverIterations, "useRealTimeSimulation", params.m_useRealTimeSimulation, "gravityAccelerationX", params.m_gravityAcceleration[0], "gravityAccelerationY", params.m_gravityAcceleration[1], - "gravityAccelerationZ", params.m_gravityAcceleration[2]); + "gravityAccelerationZ", params.m_gravityAcceleration[2], + "numNonContactInnerIterations", params.m_numNonContactInnerIterations); return val; } //"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", From 409b0eaa3942dc055eb3835566c70ede77f0976b Mon Sep 17 00:00:00 2001 From: Daniele Reda Date: Sun, 31 May 2020 21:38:38 -0700 Subject: [PATCH 23/25] fix double humanoid instantiation --- examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index 746b79781..ba22fa61c 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -165,8 +165,11 @@ class AntBulletEnv(WalkerBaseBulletEnv): class HumanoidBulletEnv(WalkerBaseBulletEnv): - def __init__(self, robot=Humanoid(), render=False): - self.robot = robot + def __init__(self, robot=None, render=False): + if robot is None: + self.robot = Humanoid() + else: + self.robot = robot WalkerBaseBulletEnv.__init__(self, self.robot, render) self.electricity_cost = 4.25 * WalkerBaseBulletEnv.electricity_cost self.stall_torque_cost = 4.25 * WalkerBaseBulletEnv.stall_torque_cost From 55fd613352f55b4db3d0d953ea85fa0340740fc4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Jun 2020 21:34:25 -0700 Subject: [PATCH 24/25] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 7a42a3b0b..bae42f2f8 100644 --- a/setup.py +++ b/setup.py @@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.7.8', + version='2.8.0', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= From d1ded7d704e01b2118914b3e4fdfbb1aa3ea6f41 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Jun 2020 23:38:59 -0700 Subject: [PATCH 25/25] use pybullet.URDF_GOOGLEY_UNDEFINED_COLORS for standard gym envs --- examples/pybullet/gym/pybullet_envs/robot_bases.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 51c2791c1..1e22f1bbd 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -130,12 +130,13 @@ class MJCFBasedRobot(XmlBasedRobot): self.objects = self._p.loadMJCF(os.path.join(pybullet_data.getDataPath(), "mjcf", self.model_xml), flags=pybullet.URDF_USE_SELF_COLLISION | - pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) + pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS | + pybullet.URDF_GOOGLEY_UNDEFINED_COLORS ) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self._p, self.objects) else: self.objects = self._p.loadMJCF( - os.path.join(pybullet_data.getDataPath(), "mjcf", self.model_xml)) + os.path.join(pybullet_data.getDataPath(), "mjcf", self.model_xml, flags = pybullet.URDF_GOOGLEY_UNDEFINED_COLORS)) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self._p, self.objects) self.robot_specific_reset(self._p) @@ -183,14 +184,14 @@ class URDFBasedRobot(XmlBasedRobot): basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base, - flags=pybullet.URDF_USE_SELF_COLLISION)) + flags=pybullet.URDF_USE_SELF_COLLISION | pybullet.URDF_GOOGLEY_UNDEFINED_COLORS)) else: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self._p, self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, - useFixedBase=self.fixed_base)) + useFixedBase=self.fixed_base, flags = pybullet.URDF_GOOGLEY_UNDEFINED_COLORS)) self.robot_specific_reset(self._p)