From 4efc983ca529e09c2e3da6afe968fe07b4cc3022 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 4 Mar 2020 17:22:46 -0800 Subject: [PATCH 01/16] bug fixes and parameter tuning --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 1 + src/BulletSoftBody/btSoftBody.cpp | 4 ++-- src/BulletSoftBody/btSoftBodyHelpers.cpp | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d0dcd445b..49e580534 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8327,6 +8327,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar if (use_bending_spring) { psb->generateBendingConstraints(2); + psb->generateBendingConstraints(2); } } btSoftBody::Material* pm = psb->appendMaterial(); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 1cbebfb82..70765a2d3 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3942,8 +3942,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap docollideFace.psb = this; docollideFace.m_colObj1Wrap = pcoWrap; docollideFace.m_rigidBody = prb1; - docollideFace.dynmargin = 0.05*(basemargin + timemargin); - docollideFace.stamargin = 0.05*basemargin; + docollideFace.dynmargin = 0.5*(basemargin + timemargin); + docollideFace.stamargin = 0.5*basemargin; m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); } } diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 9f64de77d..6e8fccc92 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1581,7 +1581,7 @@ void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) const btSoftBody::Face& f = psb->m_faces[j]; btVector3 n = btCross(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 unit_n = n.normalized(); - dist = p.dot(unit_n); + dist = (p-f.m_n[0]->m_x).dot(unit_n); btVector3 proj_p = p - dist*unit_n; getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); btScalar new_min_bary_weight = bary[0]; From ec0e9892b52b02a48d85ab4c7e636cb24d7c41eb Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 6 Mar 2020 16:04:34 -0800 Subject: [PATCH 02/16] add slider demo --- .../CommonInterfaces/CommonRigidBodyBase.h | 17 ++- .../DeformableDemo/VolumetricDeformable.cpp | 94 +++++++++++- .../CollisionDispatch/btCollisionWorld.h | 30 ++++ .../btDeformableMultiBodyDynamicsWorld.h | 143 ++++++++++++++++++ .../btDeformableNeoHookeanForce.h | 48 +++++- 5 files changed, 327 insertions(+), 5 deletions(-) diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h index fea34aec0..daaae2ba4 100644 --- a/examples/CommonInterfaces/CommonRigidBodyBase.h +++ b/examples/CommonInterfaces/CommonRigidBodyBase.h @@ -7,7 +7,7 @@ #include "CommonGUIHelperInterface.h" #include "CommonRenderInterface.h" #include "CommonCameraInterface.h" - +#include "BulletSoftBody/btSoftBody.h" #include "CommonGraphicsAppInterface.h" #include "CommonWindowInterface.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" @@ -343,6 +343,21 @@ struct CommonRigidBodyBase : public CommonExampleInterface p2p->m_setting.m_tau = 0.001f; } } + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (psb) + { + m_savedState = psb->getActivationState(); + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_dynamicsWorld->addConstraint(p2p, true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } // pickObject(pickPos, rayCallback.m_collisionObject); m_oldPickingPos = rayToWorld; diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 1f23071d4..4be1e090f 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -20,12 +20,16 @@ #include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "../CommonInterfaces/CommonParameterInterface.h" #include //printf debugging #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects. +static btScalar E = 100; +static btScalar nu = 0.3; +static btScalar damping = 0.1; struct TetraCube { @@ -35,6 +39,7 @@ struct TetraCube class VolumetricDeformable : public CommonRigidBodyBase { btAlignedObjectArray m_forces; + btDeformableNeoHookeanForce* m_neohookean; public: VolumetricDeformable(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper) @@ -58,6 +63,9 @@ public: void stepSimulation(float deltaTime) { + m_neohookean->setPoissonRatio(nu); + m_neohookean->setYoungsModulus(E); + m_neohookean->setDamping(damping); //use a smaller internal timestep, there are stability issues float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); @@ -146,6 +154,8 @@ public: } } } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); }; void VolumetricDeformable::initPhysics() @@ -220,6 +230,7 @@ void VolumetricDeformable::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0.5; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(psb); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); @@ -227,6 +238,7 @@ void VolumetricDeformable::initPhysics() m_forces.push_back(gravity_force); btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.05); + m_neohookean = neohookean; getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); @@ -234,9 +246,87 @@ void VolumetricDeformable::initPhysics() getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); // add a few rigid bodies - Ctor_RbUpStack(4); - + Ctor_RbUpStack(4); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + { + SliderParams slider("Young's Modulus", &E); + slider.m_minVal = 0; + slider.m_maxVal = 200; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Poisson Ratio", &nu); + slider.m_minVal = 0; + slider.m_maxVal = 0.4; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Damping", &damping); + slider.m_minVal = 0.01; + slider.m_maxVal = 1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } +} + +bool VolumetricDeformable::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + if (getDeformableDynamicsWorld() == 0) + return false; + + btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); + + rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; + getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback); + if (rayCallback.hasHit()) + { + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + if (body) + { + //other exclusions? + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_pickedBody = body; + m_savedState = m_pickedBody->getActivationState(); + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_dynamicsWorld->addConstraint(p2p, true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (psb) + { + m_savedState = psb->getActivationState(); + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); +// btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; +// btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); +// m_dynamicsWorld->addConstraint(p2p, true); +// m_pickedConstraint = p2p; +// btScalar mousePickClamping = 30.f; +// p2p->m_setting.m_impulseClamp = mousePickClamping; +// //very weak constraint for picking +// p2p->m_setting.m_tau = 0.001f; + } + + // pickObject(pickPos, rayCallback.m_collisionObject); + m_oldPickingPos = rayToWorld; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos - rayFromWorld).length(); + // printf("hit !\n"); + //add p2p + } + return false; } void VolumetricDeformable::exitPhysics() diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index fd0e5b9bb..5f3e8c70a 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -263,6 +263,36 @@ public: return rayResult.m_hitFraction; } }; + + struct ClosestRayResultCallbackWithInfo : public ClosestRayResultCallback + { + ClosestRayResultCallbackWithInfo(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : ClosestRayResultCallback(rayFromWorld, rayToWorld) + { + } + LocalShapeInfo* m_localShapeInfo; + + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) + { + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_collisionObject = rayResult.m_collisionObject; + m_localShapeInfo = rayResult.m_localShapeInfo; + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } + else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; + } + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } + }; struct AllHitsRayResultCallback : public RayResultCallback { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index e55b02d01..5d78d4097 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -161,6 +161,149 @@ public: void applyRepulsionForce(btScalar timeStep); void performGeometricCollisions(btScalar timeStep); + + struct btDeformableSingleRayCallback : public btBroadphaseRayCallback + { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btDeformableMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); + + btVector3 rayDir = (rayToWorld - rayFromWorld); + + rayDir.normalize(); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; +#if 0 +#ifdef RECALCULATE_AABB + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); +#else + //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); + const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; + const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; +#endif +#endif + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) + { + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); + } + } + return true; + } + }; + + + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const + { + BT_PROFILE("rayTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); +#else + for (int i = 0; i < this->getNumCollisionObjects(); i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE + } + + void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) const + { + if (collisionShape->isSoftBody()) + { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) + { + btSoftBody::sRayCast softResult; + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + if (softResult.fraction <= resultCallback.m_closestHitFraction) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal = -rayDir; + normal.normalize(); + + if (softResult.feature == btSoftBody::eFeature::Face) + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) + { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); + } + } + } + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); + } + } }; #endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 3d06e304d..2fb1c0b99 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -24,21 +24,65 @@ class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { public: typedef btAlignedObjectArray TVStack; - btScalar m_mu, m_lambda; + btScalar m_mu, m_lambda; // Lame Parameters + btScalar m_E, m_nu; // Young's modulus and Poisson ratio btScalar m_mu_damp, m_lambda_damp; btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) { btScalar damping = 0.05; m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); } btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) { m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); } - + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1- 2*m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); + } + virtual void addScaledForces(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force); From 7762b2957eea8260a349a30cca3192865c3c178e Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 6 Mar 2020 16:23:14 -0800 Subject: [PATCH 03/16] fix error in barycentric weight calculation --- src/BulletSoftBody/btSoftBodyInternals.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index e08f650b6..5a29fe3aa 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -78,9 +78,9 @@ static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3 btScalar det = a11*a22 - a12*a12; if (det < SIMD_EPSILON) return false; - btScalar w2 = (b1*a22-b2*a12)/det; - btScalar w3 = (b2*a11-b1*a12)/det; - btScalar w1 = 1-w2-w3; + btScalar w1 = (b1*a22-b2*a12)/det; + btScalar w2 = (b2*a11-b1*a12)/det; + btScalar w3 = 1-w1-w2; btScalar delta = mrg / std::sqrt(0.5*std::abs(x13.cross(x23).safeNorm())); bary = btVector3(w1,w2,w3); for (int i = 0; i < 3; ++i) From 41ca8b9e59f1b5d6f7e7a4cd62de92bae1d1e587 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 6 Mar 2020 16:29:32 -0800 Subject: [PATCH 04/16] More parameter tuning for cloth-like behavior --- examples/DeformableDemo/DeformableSelfCollision.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index 7b8fd9c3a..a5999084c 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -155,8 +155,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin) psb->getCollisionShape()->setMargin(0.0075); - psb->generateBendingConstraints(2); - psb->generateBendingConstraints(2); + psb->generateBendingConstraints(3); psb->setTotalMass(.5); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body @@ -171,8 +170,8 @@ void DeformableSelfCollision::addCloth(btVector3 origin) getDeformableDynamicsWorld()->addSoftBody(psb); psb->setSelfCollision(true); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(3,0.2, true); - psb->setSpringStiffness(3); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1.5,0.1, true); + psb->setSpringStiffness(1); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); btVector3 gravity = btVector3(0, -9.8, 0); From 7dfef922673c8ce2cd4bc612184ae4c84810c261 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 9 Mar 2020 11:19:08 -0700 Subject: [PATCH 05/16] Add support for picking deformable with mouse --- .../CommonDeformableBodyBase.h | 178 ++++++++++++++++++ .../CommonInterfaces/CommonMultiBodyBase.h | 1 + examples/DeformableDemo/ClothFriction.cpp | 23 +-- .../DeformableDemo/DeformableClothAnchor.cpp | 27 +-- examples/DeformableDemo/DeformableContact.cpp | 21 +-- .../DeformableDemo/DeformableMultibody.cpp | 22 +-- examples/DeformableDemo/DeformableRigid.cpp | 11 +- .../DeformableSelfCollision.cpp | 25 +-- examples/DeformableDemo/GraspDeformable.cpp | 40 ++-- .../DeformableDemo/MultibodyClothAnchor.cpp | 25 +-- examples/DeformableDemo/Pinch.cpp | 21 +-- examples/DeformableDemo/PinchFriction.cpp | 30 +-- examples/DeformableDemo/SplitImpulse.cpp | 25 +-- .../DeformableDemo/VolumetricDeformable.cpp | 99 ++-------- .../btDeformableBackwardEulerObjective.h | 1 + .../btDeformableLagrangianForce.h | 8 +- .../btDeformableMousePickingForce.h | 143 ++++++++++++++ .../btDeformableMultiBodyDynamicsWorld.cpp | 18 ++ .../btDeformableMultiBodyDynamicsWorld.h | 6 +- src/BulletSoftBody/btSoftBody.cpp | 38 ++++ src/BulletSoftBody/btSoftBody.h | 5 + 21 files changed, 494 insertions(+), 273 deletions(-) create mode 100644 examples/CommonInterfaces/CommonDeformableBodyBase.h create mode 100644 src/BulletSoftBody/btDeformableMousePickingForce.h diff --git a/examples/CommonInterfaces/CommonDeformableBodyBase.h b/examples/CommonInterfaces/CommonDeformableBodyBase.h new file mode 100644 index 000000000..456c25bfe --- /dev/null +++ b/examples/CommonInterfaces/CommonDeformableBodyBase.h @@ -0,0 +1,178 @@ + +#ifndef COMMON_DEFORMABLE_BODY_SETUP_H +#define COMMON_DEFORMABLE_BODY_SETUP_H +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "btBulletDynamicsCommon.h" +#include "CommonExampleInterface.h" +#include "CommonGUIHelperInterface.h" +#include "CommonRenderInterface.h" +#include "CommonGraphicsAppInterface.h" +#include "CommonWindowInterface.h" +#include "CommonCameraInterface.h" +#include "CommonMultiBodyBase.h" + +struct CommonDeformableBodyBase : public CommonMultiBodyBase +{ + btAlignedObjectArray m_forces; + btSoftBody* m_pickedSoftBody; + btDeformableMousePickingForce* m_mouseForce; + btScalar m_maxPickingForce; + CommonDeformableBodyBase(GUIHelperInterface* helper) + : CommonMultiBodyBase(helper), + m_pickedSoftBody(0), + m_mouseForce(0), + m_maxPickingForce(0.3) + { + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + if (getDeformableDynamicsWorld() == 0) + return false; + btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); + getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback); + if (rayCallback.hasHit()) + { + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (body) + { + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_pickedBody = body; + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_dynamicsWorld->addConstraint(p2p, true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } + else if (psb) + { + int face_id = rayCallback.m_localShapeInfo->m_triangleIndex; + m_pickedSoftBody = psb; + psb->setActivationState(DISABLE_DEACTIVATION); + const btSoftBody::Face& f = psb->m_faces[face_id]; + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0.2, f, m_hitPos, m_maxPickingForce); + m_mouseForce = mouse_force; + getDeformableDynamicsWorld()->addForce(psb, mouse_force); + } + else + { + btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); + if (multiCol && multiCol->m_multiBody) + { + m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); + multiCol->m_multiBody->setCanSleep(false); + + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody, multiCol->m_link, 0, pivotInA, pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + btScalar scaling = 1; + p2p->setMaxAppliedImpulse(2 * scaling); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_pickingMultiBodyPoint2Point = p2p; + } + } + m_oldPickingPos = rayToWorld; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos - rayFromWorld).length(); + } + return false; + } + + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + if (m_pickedBody && m_pickedConstraint) + { + btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); + if (pickCon) + { + //keep it at the same picking distance + btVector3 newPivotB; + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + newPivotB = rayFromWorld + dir; + pickCon->setPivotB(newPivotB); + return true; + } + } + if (m_pickingMultiBodyPoint2Point) + { + //keep it at the same picking distance + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + btVector3 newPivotB = rayFromWorld + dir; + m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); + } + if (m_pickedSoftBody && m_mouseForce) + { + btVector3 newPivot; + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + newPivot = rayFromWorld + dir; + m_mouseForce->setMousePos(newPivot); + } + return false; + } + + virtual void removePickingConstraint() + { + if (m_pickedConstraint) + { + m_dynamicsWorld->removeConstraint(m_pickedConstraint); + + if (m_pickedBody) + { + m_pickedBody->forceActivationState(ACTIVE_TAG); + m_pickedBody->activate(true); + } + delete m_pickedConstraint; + m_pickedConstraint = 0; + m_pickedBody = 0; + } + if (m_pickingMultiBodyPoint2Point) + { + m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld; + world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point); + delete m_pickingMultiBodyPoint2Point; + m_pickingMultiBodyPoint2Point = 0; + } + if (m_pickedSoftBody) + { + getDeformableDynamicsWorld()->removeForce(m_pickedSoftBody, m_mouseForce); + delete m_mouseForce; + m_mouseForce = 0; + m_pickedSoftBody = 0; + } + } +}; +#endif //COMMON_MULTI_BODY_SETUP_H diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index 7736f51a5..ad6c2a4c6 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -456,6 +456,7 @@ struct CommonMultiBodyBase : public CommonExampleInterface return false; } + virtual void removePickingConstraint() { if (m_pickedConstraint) diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp index e2aa2c0b3..ec1d47a3c 100644 --- a/examples/DeformableDemo/ClothFriction.cpp +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -22,18 +22,17 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The ClothFriction shows the use of deformable friction. -class ClothFriction : public CommonRigidBodyBase +class ClothFriction : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; btDeformableBodySolver* m_deformableBodySolver; public: ClothFriction(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper), - m_deformableBodySolver(0) + : CommonDeformableBodyBase(helper), + m_deformableBodySolver(0) { } virtual ~ClothFriction() @@ -59,19 +58,9 @@ public: m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -210,7 +199,7 @@ void ClothFriction::initPhysics() void ClothFriction::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableClothAnchor.cpp b/examples/DeformableDemo/DeformableClothAnchor.cpp index e7fe6484b..ef2c495d9 100644 --- a/examples/DeformableDemo/DeformableClothAnchor.cpp +++ b/examples/DeformableDemo/DeformableClothAnchor.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableClothAnchor shows contact between deformable objects and rigid objects. -class DeformableClothAnchor : public CommonRigidBodyBase +class DeformableClothAnchor : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableClothAnchor(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~DeformableClothAnchor() @@ -55,24 +54,10 @@ public: float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - + virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -186,7 +171,7 @@ void DeformableClothAnchor::initPhysics() void DeformableClothAnchor::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index 287d150df..37107186d 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -22,17 +22,16 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableContact shows the contact between deformable objects -class DeformableContact : public CommonRigidBodyBase +class DeformableContact : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableContact(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~DeformableContact() @@ -57,19 +56,9 @@ public: m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); @@ -218,7 +207,7 @@ void DeformableContact::initPhysics() void DeformableContact::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 5b342aada..31dfdeda1 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -28,17 +28,16 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" -#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableMultibody demo deformable bodies self-collision static bool g_floatingBase = true; static float friction = 1.; -class DeformableMultibody : public CommonMultiBodyBase +class DeformableMultibody : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableMultibody(struct GUIHelperInterface* helper) - : CommonMultiBodyBase(helper) + :CommonDeformableBodyBase(helper) { } @@ -65,20 +64,9 @@ public: void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); - - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonMultiBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -229,7 +217,7 @@ void DeformableMultibody::initPhysics() void DeformableMultibody::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 47ac3d9ff..f6cd1d5b8 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableRigid shows contact between deformable objects and rigid objects. -class DeformableRigid : public CommonRigidBodyBase +class DeformableRigid : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableRigid(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + :CommonDeformableBodyBase(helper) { } virtual ~DeformableRigid() @@ -115,7 +114,7 @@ public: virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -241,7 +240,7 @@ void DeformableRigid::initPhysics() void DeformableRigid::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index a5999084c..7d3db4965 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableSelfCollision shows deformable self collisions -class DeformableSelfCollision : public CommonRigidBodyBase +class DeformableSelfCollision : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableSelfCollision(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~DeformableSelfCollision() @@ -57,23 +56,9 @@ public: void addCloth(btVector3 origin); - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); } }; @@ -183,7 +168,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin) void DeformableSelfCollision::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index e7d63d52d..8c85083f0 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -23,7 +23,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" @@ -56,12 +56,12 @@ static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex) return canHaveMotor; } -class GraspDeformable : public CommonRigidBodyBase +class GraspDeformable : public CommonDeformableBodyBase { btAlignedObjectArray m_forces; public: GraspDeformable(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + :CommonDeformableBodyBase(helper) { } virtual ~GraspDeformable() @@ -154,7 +154,7 @@ public: virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -166,6 +166,16 @@ public: } } } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual void removePickingConstraint(){} }; @@ -189,7 +199,7 @@ void GraspDeformable::initPhysics() m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - + m_maxPickingForce = 0.001; // build a gripper if(1) { @@ -275,13 +285,13 @@ void GraspDeformable::initPhysics() { char absolute_path[1024]; b3BulletDefaultFileIO fileio; - fileio.findResourcePath("ditto.vtk", absolute_path, 1024); +// fileio.findResourcePath("ditto.vtk", absolute_path, 1024); // fileio.findResourcePath("banana.vtk", absolute_path, 1024); - // fileio.findResourcePath("ball.vtk", absolute_path, 1024); +// fileio.findResourcePath("ball.vtk", absolute_path, 1024); // fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024); // fileio.findResourcePath("single_tet.vtk", absolute_path, 1024); -// fileio.findResourcePath("tube.vtk", absolute_path, 1024); - // fileio.findResourcePath("torus.vtk", absolute_path, 1024); + fileio.findResourcePath("tube.vtk", absolute_path, 1024); +// fileio.findResourcePath("torus.vtk", absolute_path, 1024); // fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024); // fileio.findResourcePath("bread.vtk", absolute_path, 1024); // fileio.findResourcePath("boot.vtk", absolute_path, 1024); @@ -293,17 +303,17 @@ void GraspDeformable::initPhysics() btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), absolute_path); // psb->scale(btVector3(30, 30, 30)); // for banana - psb->scale(btVector3(.7, .7, .7)); +// psb->scale(btVector3(.7, .7, .7)); // psb->scale(btVector3(2, 2, 2)); -// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot - psb->scale(btVector3(.1, .1, .1)); // for ditto + psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot +// psb->scale(btVector3(.1, .1, .1)); // for ditto // psb->translate(btVector3(.25, 10, 0.4)); psb->getCollisionShape()->setMargin(0.0005); psb->setMaxStress(50); psb->setTotalMass(.01); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 20; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); @@ -311,7 +321,7 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,32, .05); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,8,.02); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } @@ -376,7 +386,7 @@ void GraspDeformable::initPhysics() void GraspDeformable::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/MultibodyClothAnchor.cpp b/examples/DeformableDemo/MultibodyClothAnchor.cpp index ba90a9e20..5efdf8d28 100644 --- a/examples/DeformableDemo/MultibodyClothAnchor.cpp +++ b/examples/DeformableDemo/MultibodyClothAnchor.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The MultibodyClothAnchor shows contact between deformable objects and rigid objects. -class MultibodyClothAnchor : public CommonRigidBodyBase +class MultibodyClothAnchor : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: MultibodyClothAnchor(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~MultibodyClothAnchor() @@ -56,23 +55,9 @@ public: m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -234,7 +219,7 @@ void MultibodyClothAnchor::initPhysics() void MultibodyClothAnchor::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index 07c491d89..c6b942fe3 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -22,7 +22,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The Pinch shows the frictional contact between kinematic rigid objects with deformable objects @@ -32,12 +32,11 @@ struct TetraCube #include "../SoftDemo/cube.inl" }; -class Pinch : public CommonRigidBodyBase +class Pinch : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: Pinch(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~Pinch() @@ -81,19 +80,9 @@ public: } } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -337,7 +326,7 @@ void Pinch::initPhysics() void Pinch::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index 24db82111..16b6e30e1 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -21,7 +21,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The PinchFriction shows the frictional contacts among volumetric deformable objects @@ -31,12 +31,12 @@ struct TetraCube #include "../SoftDemo/cube.inl" }; -class PinchFriction : public CommonRigidBodyBase +class PinchFriction : public CommonDeformableBodyBase { btAlignedObjectArray m_forces; public: PinchFriction(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~PinchFriction() @@ -80,20 +80,20 @@ public: } } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual void removePickingConstraint(){} }; void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) @@ -353,7 +353,7 @@ void PinchFriction::initPhysics() void PinchFriction::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp index c7c8088c2..a213918d4 100644 --- a/examples/DeformableDemo/SplitImpulse.cpp +++ b/examples/DeformableDemo/SplitImpulse.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The SplitImpulse shows the effect of split impulse in deformable rigid contact. -class SplitImpulse : public CommonRigidBodyBase +class SplitImpulse : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: SplitImpulse(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~SplitImpulse() @@ -69,23 +68,9 @@ public: createRigidBody(mass, startTransform, shape[0]); } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -202,7 +187,7 @@ void SplitImpulse::initPhysics() void SplitImpulse::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 4be1e090f..d99fc53e9 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -23,27 +23,28 @@ #include "../CommonInterfaces/CommonParameterInterface.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects. static btScalar E = 100; static btScalar nu = 0.3; -static btScalar damping = 0.1; +static btScalar damping = 0.01; struct TetraCube { #include "../SoftDemo/cube.inl" }; -class VolumetricDeformable : public CommonRigidBodyBase +class VolumetricDeformable : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; btDeformableNeoHookeanForce* m_neohookean; + public: VolumetricDeformable(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { + m_neohookean = 0; } virtual ~VolumetricDeformable() { @@ -67,8 +68,8 @@ public: m_neohookean->setYoungsModulus(E); m_neohookean->setDamping(damping); //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + float internalTimeStep = 1. / 600.f; + m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); } void createStaticBox(const btVector3& halfEdge, const btVector3& translation) @@ -125,37 +126,20 @@ public: } } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - ///just make it a btSoftRigidDynamicsWorld please - ///or we will add type checking - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) { btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; - //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) { btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); } } } - - virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); }; void VolumetricDeformable::initPhysics() @@ -237,7 +221,7 @@ void VolumetricDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.05); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.01); m_neohookean = neohookean; getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); @@ -258,7 +242,7 @@ void VolumetricDeformable::initPhysics() } { SliderParams slider("Poisson Ratio", &nu); - slider.m_minVal = 0; + slider.m_minVal = 0.1; slider.m_maxVal = 0.4; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); @@ -266,73 +250,16 @@ void VolumetricDeformable::initPhysics() { SliderParams slider("Damping", &damping); slider.m_minVal = 0.01; - slider.m_maxVal = 1; + slider.m_maxVal = 0.2; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } } -bool VolumetricDeformable::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) -{ - if (getDeformableDynamicsWorld() == 0) - return false; - - btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); - - rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; - getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback); - if (rayCallback.hasHit()) - { - btVector3 pickPos = rayCallback.m_hitPointWorld; - btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); - if (body) - { - //other exclusions? - if (!(body->isStaticObject() || body->isKinematicObject())) - { - m_pickedBody = body; - m_savedState = m_pickedBody->getActivationState(); - m_pickedBody->setActivationState(DISABLE_DEACTIVATION); - //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); - btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); - m_dynamicsWorld->addConstraint(p2p, true); - m_pickedConstraint = p2p; - btScalar mousePickClamping = 30.f; - p2p->m_setting.m_impulseClamp = mousePickClamping; - //very weak constraint for picking - p2p->m_setting.m_tau = 0.001f; - } - } - btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); - if (psb) - { - m_savedState = psb->getActivationState(); - m_pickedBody->setActivationState(DISABLE_DEACTIVATION); -// btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; -// btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); -// m_dynamicsWorld->addConstraint(p2p, true); -// m_pickedConstraint = p2p; -// btScalar mousePickClamping = 30.f; -// p2p->m_setting.m_impulseClamp = mousePickClamping; -// //very weak constraint for picking -// p2p->m_setting.m_tau = 0.001f; - } - - // pickObject(pickPos, rayCallback.m_collisionObject); - m_oldPickingPos = rayToWorld; - m_hitPos = pickPos; - m_oldPickingDist = (pickPos - rayFromWorld).length(); - // printf("hit !\n"); - //add p2p - } - return false; -} - void VolumetricDeformable::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 89cbb38c5..7bd03bd03 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -20,6 +20,7 @@ #include "btDeformableMassSpringForce.h" #include "btDeformableGravityForce.h" #include "btDeformableCorotatedForce.h" +#include "btDeformableMousePickingForce.h" #include "btDeformableLinearElasticityForce.h" #include "btDeformableNeoHookeanForce.h" #include "btDeformableContactProjection.h" diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 64e80e23b..cdd31c7fd 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -26,7 +26,8 @@ enum btDeformableLagrangianForceType BT_MASSSPRING_FORCE = 2, BT_COROTATED_FORCE = 3, BT_NEOHOOKEAN_FORCE = 4, - BT_LINEAR_ELASTICITY_FORCE = 5 + BT_LINEAR_ELASTICITY_FORCE = 5, + BT_MOUSE_PICKING_FORCE = 6 }; static inline double randomDouble(double low, double high) @@ -85,6 +86,11 @@ public: m_softBodies.push_back(psb); } + virtual void removeSoftBody(btSoftBody* psb) + { + m_softBodies.remove(psb); + } + virtual void setIndices(const btAlignedObjectArray* nodes) { m_nodes = nodes; diff --git a/src/BulletSoftBody/btDeformableMousePickingForce.h b/src/BulletSoftBody/btDeformableMousePickingForce.h new file mode 100644 index 000000000..0e2ca064f --- /dev/null +++ b/src/BulletSoftBody/btDeformableMousePickingForce.h @@ -0,0 +1,143 @@ +/* + Written by Xuchen Han + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_MOUSE_PICKING_FORCE_H +#define BT_MOUSE_PICKING_FORCE_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableMousePickingForce : public btDeformableLagrangianForce +{ + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + btScalar m_elasticStiffness, m_dampingStiffness; + const btSoftBody::Face& m_face; + btVector3 m_mouse_pos; + btScalar m_maxForce; +public: + typedef btAlignedObjectArray TVStack; + btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = scaled_stiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int i = 0; i < 3; ++i) + { + btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; + } + df[m_face.m_n[i]->index] -= local_scaled_df; + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = m_elasticStiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + energy += 0.5 * scaled_force.dot(dir); + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; + } + energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + //TODO + } + + void setMousePos(const btVector3& p) + { + m_mouse_pos = p; + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MOUSE_PICKING_FORCE; + } + +}; + +#endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index e09cfb09c..f6a49a934 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -688,6 +688,24 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL } } +void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + int removed_index = -1; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->removeSoftBody(psb); + if (forces[i]->m_softBodies.size() == 0) + removed_index = i; + break; + } + } + if (removed_index >= 0) + forces.removeAtIndex(removed_index); +} + void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) { m_softBodies.remove(body); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 5d78d4097..17512c8db 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -131,6 +131,8 @@ public: void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); + void removeSoftBody(btSoftBody* body); void removeCollisionObject(btCollisionObject* collisionObject); @@ -267,7 +269,7 @@ public: if (softBody) { btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) { if (softResult.fraction <= resultCallback.m_closestHitFraction) { @@ -278,8 +280,6 @@ public: btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); btVector3 normal = -rayDir; normal.normalize(); - - if (softResult.feature == btSoftBody::eFeature::Face) { normal = softBody->m_faces[softResult.index].m_normal; if (normal.dot(rayDir) > 0) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 70765a2d3..1bf68bb2a 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1936,6 +1936,25 @@ bool btSoftBody::rayTest(const btVector3& rayFrom, return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0); } +bool btSoftBody::rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) +{ + if (m_faces.size() == 0) + return false; + else + { + if (m_fdbvt.empty()) + initializeFaceTree(); + } + + results.body = this; + results.fraction = 1.f; + results.index = -1; + + return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); +} + // void btSoftBody::setSolver(eSolverPresets::_ preset) { @@ -2441,6 +2460,25 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, return (cnt); } +int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const +{ + int cnt = 0; + { /* Use dbvt */ + RayFromToCaster collider(rayFrom, rayTo, mint); + + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); + if (collider.m_face) + { + mint = collider.m_mint; + index = (int)(collider.m_face - &m_faces[0]); + cnt = 1; + } + } + return (cnt); +} + + // static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 8bb143681..6b3bb9800 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1031,6 +1031,11 @@ public: bool rayTest(const btVector3& rayFrom, const btVector3& rayTo, sRayCast& results); + bool rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); + int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const; /* Solver presets */ void setSolver(eSolverPresets::_ preset); /* predictMotion */ From 91df837a79e916df6ac2aa22c77a87ba3a6672b8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 29 Feb 2020 22:02:01 -0800 Subject: [PATCH 06/16] use PyBullet's own visualizer for enjoy script, + add a time.sleep since it is way too fast (1000FPS for cart pole) --- .../gym/pybullet_envs/stable_baselines/enjoy.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py b/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py index d887c41d3..3b8aa4098 100644 --- a/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py +++ b/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py @@ -6,7 +6,7 @@ # MIT License import argparse import multiprocessing - +import time import gym import numpy as np import pybullet_envs @@ -35,11 +35,7 @@ if __name__ == '__main__': # Use SubprocVecEnv for rendering if not args.no_render: - # Note: fork is not thread-safe but usually is faster - fork_available = 'fork' in multiprocessing.get_all_start_methods() - start_method = 'fork' if fork_available else 'spawn' - env = SubprocVecEnv([lambda: env], start_method=start_method) - + env.render(mode='human') algo = { 'sac': SAC, @@ -67,6 +63,8 @@ if __name__ == '__main__': episode_length += 1 if not args.no_render: env.render(mode='human') + dt = 1./240. + time.sleep(dt) episode_rewards.append(episode_reward) episode_lengths.append(episode_length) print("Episode {} reward={}, length={}".format(len(episode_rewards), episode_reward, episode_length)) From 55de20b945bebb8d694b258204c7c68cc8976133 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 1 Mar 2020 12:22:47 -0800 Subject: [PATCH 07/16] add flag to assure deterministicOverlappingPairs --- examples/pybullet/gym/pybullet_envs/env_bases.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index ff0589ae0..f9c51c051 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -57,6 +57,7 @@ class MJCFBaseBulletEnv(gym.Env): else: self._p = bullet_client.BulletClient() self._p.resetSimulation() + self._p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) #optionally enable EGL for faster headless rendering try: if os.environ["PYBULLET_EGL"]: From 812c675d4ccfea3f6fee65d193df809213e7d221 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 1 Mar 2020 13:11:47 -0800 Subject: [PATCH 08/16] prepare towards HumanoidDeepMimicBackflipBulletEnv-v1 and HumanoidDeepMimicWalkBulletEnv-v1 remove unused SubprocVecEnv from stable_baselines/enjoy.py --- .../pybullet/gym/pybullet_envs/__init__.py | 11 +- .../deep_mimic/gym_env/__init__.py | 3 + .../deep_mimic/gym_env/deep_mimic_env.py | 175 ++++++++++++++++++ .../deep_mimic/gym_env/deepmimic_test.py | 23 +++ .../pybullet_envs/stable_baselines/enjoy.py | 2 +- 5 files changed, 211 insertions(+), 3 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/__init__.py create mode 100644 examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py create mode 100644 examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index 83b536fdb..ed909978d 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -12,8 +12,15 @@ def register(id, *args, **kvargs): # ------------bullet------------- register( - id='HumanoidDeepMimicBulletEnv-v1', - entry_point='pybullet_envs.deep_mimic:HumanoidDeepMimicGymEnv', + id='HumanoidDeepMimicBackflipBulletEnv-v1', + entry_point='pybullet_envs.deep_mimic.gym_env:HumanoidDeepMimicBackflipBulletEnv', + max_episode_steps=1000, + reward_threshold=20000.0, +) + +register( + id='HumanoidDeepMimicWalkBulletEnv-v1', + entry_point='pybullet_envs.deep_mimic.gym_env:HumanoidDeepMimicWalkBulletEnv', max_episode_steps=1000, reward_threshold=20000.0, ) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/__init__.py new file mode 100644 index 000000000..44f79b499 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/__init__.py @@ -0,0 +1,3 @@ +from pybullet_envs.deep_mimic.gym_env.deep_mimic_env import HumanoidDeepMimicBackflipBulletEnv +from pybullet_envs.deep_mimic.gym_env.deep_mimic_env import HumanoidDeepMimicWalkBulletEnv + 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 new file mode 100644 index 000000000..ba77efddb --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py @@ -0,0 +1,175 @@ +""" +Classic cart-pole system implemented by Rich Sutton et al. +Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c +""" +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0, parentdir) + +import logging +import math +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import time +import subprocess +import pybullet as p2 +import pybullet_data +import pybullet_utils.bullet_client as bc +from pkg_resources import parse_version +from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv +from pybullet_utils.arg_parser import ArgParser +from pybullet_utils.logger import Logger + +logger = logging.getLogger(__name__) + + +class HumanoidDeepBulletEnv(gym.Env): + metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50} + + def __init__(self, renders=False, arg_file=''): + + self._arg_parser = ArgParser() + Logger.print2("===========================================================") + succ = False + if (arg_file != ''): + path = pybullet_data.getDataPath() + "/args/" + arg_file + succ = self._arg_parser.load_file(path) + Logger.print2(arg_file) + assert succ, Logger.print2('Failed to load args from: ' + arg_file) + + self._internal_env = None + self._renders = renders + self._discrete_actions = False + self._arg_file=arg_file + self._render_height = 200 + self._render_width = 320 + + self.theta_threshold_radians = 12 * 2 * math.pi / 360 + self.x_threshold = 0.4 #2.4 + high = np.array([ + self.x_threshold * 2, + np.finfo(np.float32).max, self.theta_threshold_radians * 2, + np.finfo(np.float32).max + ]) + + self.force_mag = 10 + + if self._discrete_actions: + self.action_space = spaces.Discrete(2) + else: + action_dim = 1 + action_high = np.array([self.force_mag] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + + self.observation_space = spaces.Box(-high, high, dtype=np.float32) + + self.seed() + + self.viewer = None + self._configure() + + def _configure(self, display=None): + self.display = display + + def seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def step(self, action): + p = self._p + if self._discrete_actions: + force = self.force_mag if action == 1 else -self.force_mag + else: + force = action[0] + + p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force) + p.stepSimulation() + + self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] + theta, theta_dot, x, x_dot = self.state + + done = x < -self.x_threshold \ + or x > self.x_threshold \ + or theta < -self.theta_threshold_radians \ + or theta > self.theta_threshold_radians + done = bool(done) + reward = 1.0 + #print("state=",self.state) + return np.array(self.state), reward, done, {} + + def reset(self): + # print("-----------reset simulation---------------") + if self._internal_env==None: + self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders) + self._internal_env.reset() + agent_id = -1 #unused here + state = self._internal_env.record_state(agent_id) + return state + + def render(self, mode='human', close=False): + if mode == "human": + self._renders = True + if mode != "rgb_array": + return np.array([]) + base_pos=[0,0,0] + self._cam_dist = 2 + self._cam_pitch = 0.3 + self._cam_yaw = 0 + if (self._physics_client_id>=0): + view_matrix = self._p.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._p.computeProjectionMatrixFOV(fov=60, + aspect=float(self._render_width) / + self._render_height, + nearVal=0.1, + farVal=100.0) + (_, _, px, _, _) = self._p.getCameraImage( + width=self._render_width, + height=self._render_height, + renderer=self._p.ER_BULLET_HARDWARE_OPENGL, + viewMatrix=view_matrix, + projectionMatrix=proj_matrix) + else: + px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8) + rgb_array = np.array(px, dtype=np.uint8) + rgb_array = np.reshape(np.array(px), (self._render_height, self._render_width, -1)) + rgb_array = rgb_array[:, :, :3] + return rgb_array + + def configure(self, args): + pass + + def close(self): + if self._physics_client_id >= 0: + self._p.disconnect() + self._physics_client_id = -1 + +class HumanoidDeepMimicBackflipBulletEnv(HumanoidDeepBulletEnv): + metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50} + + def __init__(self, renders=False): + # start the bullet physics server + HumanoidDeepBulletEnv.__init__(self, renders, arg_file="run_humanoid3d_backflip_args.txt") + + +class HumanoidDeepMimicWalkBulletEnv(HumanoidDeepBulletEnv): + metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50} + + def __init__(self, renders=False): + # start the bullet physics server + HumanoidDeepBulletEnv.__init__(self, renders, arg_file="run_humanoid3d_walk_args.txt") + +class CartPoleContinuousBulletEnv5(HumanoidDeepBulletEnv): + metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50} + + def __init__(self, renders=False): + # start the bullet physics server + HumanoidDeepBulletEnv.__init__(self, renders, arg_file="") diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py new file mode 100644 index 000000000..789a04614 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py @@ -0,0 +1,23 @@ +import gym +import pybullet_envs +import time + +env = gym.make('HumanoidDeepMimicBackflipBulletEnv-v1') +env.render(mode='human') +env.reset() +print("------------------------------------") +print("env=",env) +print(dir(env)) +print(dir(env.env)) +dt = 1./240. +logId = env.env._internal_env._pybullet_client.startStateLogging(env.env._internal_env._pybullet_client.STATE_LOGGING_PROFILE_TIMINGS, "perf.json") +for i in range (100): + env.env._internal_env._pybullet_client.submitProfileTiming("loop") + #time.sleep(dt) + #keys = env.env._internal_env._pybullet_client.getKeyboardEvents() + #if keys: + # print (keys) + env.reset() + env.env._internal_env._pybullet_client.submitProfileTiming() + +env.env._internal_env._pybullet_client.stopStateLogging(logId) \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py b/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py index 3b8aa4098..7f1ea5eae 100644 --- a/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py +++ b/examples/pybullet/gym/pybullet_envs/stable_baselines/enjoy.py @@ -12,7 +12,7 @@ import numpy as np import pybullet_envs from stable_baselines import SAC, TD3 -from stable_baselines.common.vec_env import SubprocVecEnv + from stable_baselines.common.evaluation import evaluate_policy from pybullet_envs.stable_baselines.utils import TimeFeatureWrapper From 9ecd0884d98fc49eb7c86493f6478da363d02496 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 1 Mar 2020 21:13:10 -0800 Subject: [PATCH 09/16] Connect deep_mimic_env.py to internal pybullet_deep_mimic_env.py This adds two untested Gym environments to pybullet_envs. todo: train using PPO2 etc HumanoidDeepMimicBackflipBulletEnv-v1 HumanoidDeepMimicWalkBulletEnv-v1 --- .../deep_mimic/gym_env/deep_mimic_env.py | 84 ++++++++++++------- .../deep_mimic/gym_env/deepmimic_test.py | 25 ++++-- 2 files changed, 72 insertions(+), 37 deletions(-) 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 ba77efddb..55a50ef50 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 @@ -40,6 +40,8 @@ class HumanoidDeepBulletEnv(gym.Env): Logger.print2(arg_file) assert succ, Logger.print2('Failed to load args from: ' + arg_file) + self._p = None + self._time_step = 1./240. self._internal_env = None self._renders = renders self._discrete_actions = False @@ -55,16 +57,37 @@ class HumanoidDeepBulletEnv(gym.Env): np.finfo(np.float32).max ]) - self.force_mag = 10 - - if self._discrete_actions: - self.action_space = spaces.Discrete(2) - else: - action_dim = 1 - action_high = np.array([self.force_mag] * action_dim) - self.action_space = spaces.Box(-action_high, action_high) - self.observation_space = spaces.Box(-high, high, dtype=np.float32) + ctrl_size = 43 #numDof + root_size = 7 # root + + action_dim = ctrl_size - root_size + + action_bound_min = np.array([ + -4.79999999999, -1.00000000000, -1.00000000000, -1.00000000000, -4.00000000000, + -1.00000000000, -1.00000000000, -1.00000000000, -7.77999999999, -1.00000000000, + -1.000000000, -1.000000000, -7.850000000, -6.280000000, -1.000000000, -1.000000000, + -1.000000000, -12.56000000, -1.000000000, -1.000000000, -1.000000000, -4.710000000, + -7.779999999, -1.000000000, -1.000000000, -1.000000000, -7.850000000, -6.280000000, + -1.000000000, -1.000000000, -1.000000000, -8.460000000, -1.000000000, -1.000000000, + -1.000000000, -4.710000000 + ]) + + #print("len(action_bound_min)=",len(action_bound_min)) + action_bound_max = np.array([ + 4.799999999, 1.000000000, 1.000000000, 1.000000000, 4.000000000, 1.000000000, 1.000000000, + 1.000000000, 8.779999999, 1.000000000, 1.0000000, 1.0000000, 4.7100000, 6.2800000, + 1.0000000, 1.0000000, 1.0000000, 12.560000, 1.0000000, 1.0000000, 1.0000000, 7.8500000, + 8.7799999, 1.0000000, 1.0000000, 1.0000000, 4.7100000, 6.2800000, 1.0000000, 1.0000000, + 1.0000000, 10.100000, 1.0000000, 1.0000000, 1.0000000, 7.8500000 + ]) + #print("len(action_bound_max)=",len(action_bound_max)) + + self.action_space = spaces.Box(action_bound_min, action_bound_max) + observation_min = np.array([0.0]+[-100.0]+[-4.0]*105+[-500.0]*90) + observation_max = np.array([1.0]+[100.0]+[4.0]*105+[500.0]*90) + state_size = 197 + self.observation_space = spaces.Box(observation_min, observation_min, dtype=np.float32) self.seed() @@ -79,25 +102,24 @@ class HumanoidDeepBulletEnv(gym.Env): return [seed] def step(self, action): - p = self._p - if self._discrete_actions: - force = self.force_mag if action == 1 else -self.force_mag - else: - force = action[0] + + #apply control action + agent_id = -1 + self._internal_env.set_action(agent_id, action) + + #step sim + self._internal_env.update(self._time_step) + - p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force) - p.stepSimulation() - - self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] - theta, theta_dot, x, x_dot = self.state - - done = x < -self.x_threshold \ - or x > self.x_threshold \ - or theta < -self.theta_threshold_radians \ - or theta > self.theta_threshold_radians - done = bool(done) - reward = 1.0 - #print("state=",self.state) + #record state + self.state = self._internal_env.record_state(agent_id) + + #record reward + reward = self._internal_env.calc_reward(agent_id) + + #record done + done = self._internal_env.is_episode_end() + return np.array(self.state), reward, done, {} def reset(self): @@ -105,6 +127,7 @@ class HumanoidDeepBulletEnv(gym.Env): if self._internal_env==None: self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders) self._internal_env.reset() + self._p = self._internal_env._pybullet_client agent_id = -1 #unused here state = self._internal_env.record_state(agent_id) return state @@ -118,7 +141,7 @@ class HumanoidDeepBulletEnv(gym.Env): self._cam_dist = 2 self._cam_pitch = 0.3 self._cam_yaw = 0 - if (self._physics_client_id>=0): + if (not self._p == None): view_matrix = self._p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, @@ -148,9 +171,8 @@ class HumanoidDeepBulletEnv(gym.Env): pass def close(self): - if self._physics_client_id >= 0: - self._p.disconnect() - self._physics_client_id = -1 + + pass class HumanoidDeepMimicBackflipBulletEnv(HumanoidDeepBulletEnv): metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50} diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py index 789a04614..46c875aba 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deepmimic_test.py @@ -2,7 +2,8 @@ import gym import pybullet_envs import time -env = gym.make('HumanoidDeepMimicBackflipBulletEnv-v1') +#env = gym.make('HumanoidDeepMimicBackflipBulletEnv-v1') +env = gym.make('HumanoidDeepMimicWalkBulletEnv-v1') env.render(mode='human') env.reset() print("------------------------------------") @@ -13,11 +14,23 @@ dt = 1./240. logId = env.env._internal_env._pybullet_client.startStateLogging(env.env._internal_env._pybullet_client.STATE_LOGGING_PROFILE_TIMINGS, "perf.json") for i in range (100): env.env._internal_env._pybullet_client.submitProfileTiming("loop") - #time.sleep(dt) - #keys = env.env._internal_env._pybullet_client.getKeyboardEvents() - #if keys: - # print (keys) + env.reset() env.env._internal_env._pybullet_client.submitProfileTiming() +env.env._internal_env._pybullet_client.stopStateLogging(logId) -env.env._internal_env._pybullet_client.stopStateLogging(logId) \ No newline at end of file +action = env.env.action_space.sample() +while (1): + time.sleep(dt) + #keys = env.env._internal_env._pybullet_client.getKeyboardEvents() + #if keys: + # env.reset() + #action=[0]*36 + action = env.env.action_space.sample() + state, reward, done, info = env.step(action) + #env.render(mode='rgb_array') + if done: + env.reset() + #action = env.env.action_space.sample() + #print("reward=",reward) + \ No newline at end of file From ca5061fac331c5df3e9165190f4242b2f77b5537 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 1 Mar 2020 21:27:30 -0800 Subject: [PATCH 10/16] set correct reward for deep_mimic gymenv (max episode = 1000 steps, 1 reward at each step) terminate testrl.py at 1000, show reward --- examples/pybullet/gym/pybullet_envs/__init__.py | 4 ++-- .../pybullet/gym/pybullet_envs/deep_mimic/testrl.py | 13 ++++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index ed909978d..c49a64166 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -15,14 +15,14 @@ register( id='HumanoidDeepMimicBackflipBulletEnv-v1', entry_point='pybullet_envs.deep_mimic.gym_env:HumanoidDeepMimicBackflipBulletEnv', max_episode_steps=1000, - reward_threshold=20000.0, + reward_threshold=1000.0, ) register( id='HumanoidDeepMimicWalkBulletEnv-v1', entry_point='pybullet_envs.deep_mimic.gym_env:HumanoidDeepMimicWalkBulletEnv', max_episode_steps=1000, - reward_threshold=20000.0, + reward_threshold=1000.0, ) register( diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py index bca52c161..e6be22277 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py @@ -19,14 +19,25 @@ import random update_timestep = 1. / 240. animating = True step = False +total_reward = 0 +steps = 0 def update_world(world, time_elapsed): timeStep = update_timestep world.update(timeStep) reward = world.env.calc_reward(agent_id=0) + global total_reward + total_reward += reward + global steps + steps+=1 + #print("reward=",reward) + #print("steps=",steps) end_episode = world.env.is_episode_end() - if (end_episode): + if (end_episode or steps>= 1000): + print("total_reward=",total_reward) + total_reward=0 + steps = 0 world.end_episode() world.reset() return From 4d0d8a189c64afa7ce98682aa7cfa0fef7586506 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 2 Mar 2020 12:52:14 -0800 Subject: [PATCH 11/16] increase the number of steps from 1000 to 2000, to mimic multiple cycles --- examples/pybullet/gym/pybullet_envs/__init__.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index c49a64166..8f67de5d6 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -14,15 +14,15 @@ def register(id, *args, **kvargs): register( id='HumanoidDeepMimicBackflipBulletEnv-v1', entry_point='pybullet_envs.deep_mimic.gym_env:HumanoidDeepMimicBackflipBulletEnv', - max_episode_steps=1000, - reward_threshold=1000.0, + max_episode_steps=2000, + reward_threshold=2000.0, ) register( id='HumanoidDeepMimicWalkBulletEnv-v1', entry_point='pybullet_envs.deep_mimic.gym_env:HumanoidDeepMimicWalkBulletEnv', - max_episode_steps=1000, - reward_threshold=1000.0, + max_episode_steps=2000, + reward_threshold=2000.0, ) register( From b4bfe4fa99641106b6bdcbbdf55f321a1ab825a8 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 5 Mar 2020 10:48:56 -0800 Subject: [PATCH 12/16] fix rigidbody activation --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 49e580534..1f7099bf3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9046,6 +9046,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) { rb->forceActivationState(ACTIVE_TAG); + rb->setDeactivationTime(0.0); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) { From f43c40f694c2237b386241817bef9718167e1373 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 5 Mar 2020 10:50:54 -0800 Subject: [PATCH 13/16] fix space --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1f7099bf3..ed5435266 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9046,7 +9046,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) { rb->forceActivationState(ACTIVE_TAG); - rb->setDeactivationTime(0.0); + rb->setDeactivationTime(0.0); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) { From 4730dab4bb67e6f188223310787b2e0d4ea39e34 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 11 Mar 2020 11:29:22 -0700 Subject: [PATCH 14/16] Address PR comments --- .../CommonDeformableBodyBase.h | 59 ++++++++++++++++--- .../CommonInterfaces/CommonRigidBodyBase.h | 16 ----- .../CollisionDispatch/btCollisionWorld.h | 30 ---------- 3 files changed, 50 insertions(+), 55 deletions(-) diff --git a/examples/CommonInterfaces/CommonDeformableBodyBase.h b/examples/CommonInterfaces/CommonDeformableBodyBase.h index 456c25bfe..4bfc379d9 100644 --- a/examples/CommonInterfaces/CommonDeformableBodyBase.h +++ b/examples/CommonInterfaces/CommonDeformableBodyBase.h @@ -14,6 +14,7 @@ #include "CommonWindowInterface.h" #include "CommonCameraInterface.h" #include "CommonMultiBodyBase.h" +#include "BulletSoftBody/btSoftBody.h" struct CommonDeformableBodyBase : public CommonMultiBodyBase { @@ -38,12 +39,49 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase { return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; } - + + struct ClosestRayResultCallbackWithInfo : public btCollisionWorld::ClosestRayResultCallback + { + ClosestRayResultCallbackWithInfo(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : ClosestRayResultCallback(rayFromWorld, rayToWorld) + { + } + int m_faceId; + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) + { + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_collisionObject = rayResult.m_collisionObject; + if (rayResult.m_localShapeInfo) + { + m_faceId = rayResult.m_localShapeInfo->m_triangleIndex; + } + else + { + m_faceId = -1; + } + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } + else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; + } + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } + }; + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { if (getDeformableDynamicsWorld() == 0) return false; - btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); + ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback); if (rayCallback.hasHit()) { @@ -68,13 +106,16 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase } else if (psb) { - int face_id = rayCallback.m_localShapeInfo->m_triangleIndex; - m_pickedSoftBody = psb; - psb->setActivationState(DISABLE_DEACTIVATION); - const btSoftBody::Face& f = psb->m_faces[face_id]; - btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0.2, f, m_hitPos, m_maxPickingForce); - m_mouseForce = mouse_force; - getDeformableDynamicsWorld()->addForce(psb, mouse_force); + int face_id = rayCallback.m_faceId; + if (face_id >= 0 && face_id < psb->m_faces.size()) + { + m_pickedSoftBody = psb; + psb->setActivationState(DISABLE_DEACTIVATION); + const btSoftBody::Face& f = psb->m_faces[face_id]; + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0.2, f, m_hitPos, m_maxPickingForce); + m_mouseForce = mouse_force; + getDeformableDynamicsWorld()->addForce(psb, mouse_force); + } } else { diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h index daaae2ba4..98352bcd3 100644 --- a/examples/CommonInterfaces/CommonRigidBodyBase.h +++ b/examples/CommonInterfaces/CommonRigidBodyBase.h @@ -7,7 +7,6 @@ #include "CommonGUIHelperInterface.h" #include "CommonRenderInterface.h" #include "CommonCameraInterface.h" -#include "BulletSoftBody/btSoftBody.h" #include "CommonGraphicsAppInterface.h" #include "CommonWindowInterface.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" @@ -343,21 +342,6 @@ struct CommonRigidBodyBase : public CommonExampleInterface p2p->m_setting.m_tau = 0.001f; } } - btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); - if (psb) - { - m_savedState = psb->getActivationState(); - m_pickedBody->setActivationState(DISABLE_DEACTIVATION); - //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); - btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); - m_dynamicsWorld->addConstraint(p2p, true); - m_pickedConstraint = p2p; - btScalar mousePickClamping = 30.f; - p2p->m_setting.m_impulseClamp = mousePickClamping; - //very weak constraint for picking - p2p->m_setting.m_tau = 0.001f; - } // pickObject(pickPos, rayCallback.m_collisionObject); m_oldPickingPos = rayToWorld; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 5f3e8c70a..fd0e5b9bb 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -263,36 +263,6 @@ public: return rayResult.m_hitFraction; } }; - - struct ClosestRayResultCallbackWithInfo : public ClosestRayResultCallback - { - ClosestRayResultCallbackWithInfo(const btVector3& rayFromWorld, const btVector3& rayToWorld) - : ClosestRayResultCallback(rayFromWorld, rayToWorld) - { - } - LocalShapeInfo* m_localShapeInfo; - - virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) - { - //caller already does the filter on the m_closestHitFraction - btAssert(rayResult.m_hitFraction <= m_closestHitFraction); - - m_closestHitFraction = rayResult.m_hitFraction; - m_collisionObject = rayResult.m_collisionObject; - m_localShapeInfo = rayResult.m_localShapeInfo; - if (normalInWorldSpace) - { - m_hitNormalWorld = rayResult.m_hitNormalLocal; - } - else - { - ///need to transform normal into worldspace - m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; - } - m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); - return rayResult.m_hitFraction; - } - }; struct AllHitsRayResultCallback : public RayResultCallback { From 2d3cd509eb8694185e0a6071cf2ce19fd87344c1 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 13 Mar 2020 14:42:49 -0700 Subject: [PATCH 15/16] Fix divide by zero error in repulsion force --- src/BulletSoftBody/btSoftBody.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 6b3bb9800..d16493dde 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1293,10 +1293,11 @@ public: continue; btVector3 vt = vr - vn*n; btScalar I = 0; + btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im; if (applySpringForce) - I = -btMin(repulsionStiffness * timeStep * d, btScalar(1)/node->m_im * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); + I = -btMin(repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); if (vn < 0) - I += btScalar(0.5)/node->m_im * vn; + I += 0.5 * mass * vn; bool face_constrained = false, node_constrained = node->m_constrained; for (int i = 0; i < 3; ++i) face_constrained |= face->m_n[i]->m_constrained; @@ -1322,7 +1323,7 @@ public: btScalar delta_vn = -2 * I * node->m_im; btScalar mu = c.m_friction; btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm; - I = btScalar(0.5)/node->m_im * (vt_norm-vt_new); + I = 0.5 * mass * (vt_norm-vt_new); vt.safeNormalize(); I_tilde = 2.0*I /(1.0+w.length2()); // double the impulse if node or face is constrained. From 0a668188411ccd5ba2f6afb797f0c46006c69e88 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 13 Mar 2020 14:50:36 -0700 Subject: [PATCH 16/16] Use latest mouse position for picking force --- examples/CommonInterfaces/CommonDeformableBodyBase.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/CommonInterfaces/CommonDeformableBodyBase.h b/examples/CommonInterfaces/CommonDeformableBodyBase.h index 4bfc379d9..686f12a72 100644 --- a/examples/CommonInterfaces/CommonDeformableBodyBase.h +++ b/examples/CommonInterfaces/CommonDeformableBodyBase.h @@ -88,6 +88,9 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase btVector3 pickPos = rayCallback.m_hitPointWorld; btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + m_oldPickingPos = rayToWorld; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos - rayFromWorld).length(); if (body) { if (!(body->isStaticObject() || body->isKinematicObject())) @@ -139,9 +142,6 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase m_pickingMultiBodyPoint2Point = p2p; } } - m_oldPickingPos = rayToWorld; - m_hitPos = pickPos; - m_oldPickingDist = (pickPos - rayFromWorld).length(); } return false; }