diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 1274a3721..5b342aada 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -206,15 +206,15 @@ void DeformableMultibody::initPhysics() psb->getCollisionShape()->setMargin(0.25); psb->generateBendingConstraints(2); - psb->setTotalMass(5); + psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = .1; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->setCollisionFlags(0); getDeformableDynamicsWorld()->addSoftBody(psb); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30, 1, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index f6c087d2a..47ac3d9ff 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -219,7 +219,7 @@ void DeformableRigid::initPhysics() psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 2; + psb->m_cfg.kDF = .4; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 475fa9095..e7d63d52d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -275,12 +275,12 @@ 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("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("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); diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index ed811a417..07c491d89 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -231,7 +231,7 @@ void Pinch::initPhysics() btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); getDeformableDynamicsWorld()->setSolverCallback(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index a07e36c77..e500c412c 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -425,12 +425,12 @@ public: m_robotSim.setGravity(btVector3(0, 0, -10)); b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006); - args.m_springElasticStiffness = .1; - args.m_springDampingStiffness = .0004; - args.m_springBendingStiffness = 1; - args.m_frictionCoeff = 1; + args.m_springElasticStiffness = 1; + args.m_springDampingStiffness = .01; + args.m_springBendingStiffness = .1; + args.m_frictionCoeff = 10; args.m_useSelfCollision = false; -// args.m_useFaceContact = true; + args.m_useFaceContact = true; args.m_useBendingSprings = true; args.m_startPosition.setValue(0, 0, 0); args.m_startOrientation.setValue(0, 0, 1, 1); @@ -476,7 +476,7 @@ public: revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); - m_robotSim.setNumSimulationSubSteps(8); + m_robotSim.setNumSimulationSubSteps(2); } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index bc484b8d0..0aa4ca89a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8153,7 +8153,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness; } - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false, spring_bending_stiffness); + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); deformWorld->addForce(psb, springForce); m_data->m_lf.push_back(springForce); } @@ -9353,6 +9353,15 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st if (deformWorld) { deformWorld->getWorldInfo().m_gravity = grav; + for (int i = 0; i < m_data->m_lf.size(); ++i) + { + btDeformableLagrangianForce* force = m_data->m_lf[i]; + if (force->getForceType() == BT_GRAVITY_FORCE) + { + btDeformableGravityForce* gforce = (btDeformableGravityForce*)force; + gforce->m_gravity = grav; + } + } } diff --git a/examples/pybullet/gym/pybullet_envs/stable_baselines/__init__.py b/examples/pybullet/gym/pybullet_envs/stable_baselines/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/pybullet/gym/pybullet_envs/stable_baselines/train.py b/examples/pybullet/gym/pybullet_envs/stable_baselines/train.py new file mode 100644 index 000000000..4390b7bb9 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/stable_baselines/train.py @@ -0,0 +1,60 @@ +# Code adapted from https://github.com/araffin/rl-baselines-zoo +# it requires stable-baselines to be installed +# Colab Notebook: https://colab.research.google.com/drive/1nZkHO4QTYfAksm9ZTaZ5vXyC7szZxC3F +# Author: Antonin RAFFIN +# MIT License +import argparse + +import pybullet_envs + +import gym +import numpy as np +from stable_baselines import SAC, TD3 +from stable_baselines.common.noise import NormalActionNoise + +from utils import TimeFeatureWrapper, EvalCallback + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--algo', help='RL Algorithm (Soft Actor-Critic by default)', default='sac', + type=str, required=False, choices=['sac', 'td3']) + parser.add_argument('--env', type=str, default='HalfCheetahBulletEnv-v0', help='environment ID') + parser.add_argument('-n', '--n-timesteps', help='Number of training timesteps', default=int(1e6), + type=int) + args = parser.parse_args() + + env_id = args.env + n_timesteps = args.n_timesteps + save_path = '{}_{}'.format(args.algo, env_id) + + # Instantiate and wrap the environment + env = TimeFeatureWrapper(gym.make(env_id)) + + # Create the evaluation environment and callback + eval_env = TimeFeatureWrapper(gym.make(env_id)) + callback = EvalCallback(eval_env, best_model_save_path=save_path + '_best') + + algo = { + 'sac': SAC, + 'td3': TD3 + }[args.algo] + + n_actions = env.action_space.shape[0] + + # Tuned hyperparameters from https://github.com/araffin/rl-baselines-zoo + hyperparams = { + 'sac': dict(batch_size=256, gamma=0.98, policy_kwargs=dict(layers=[256, 256]), + learning_starts=10000, buffer_size=int(2e5), tau=0.01), + + 'td3': dict(batch_size=100, policy_kwargs=dict(layers=[400, 300]), + learning_rate=1e-3, learning_starts=10000, buffer_size=int(1e6), + train_freq=1000, gradient_steps=1000, + action_noise=NormalActionNoise(mean=np.zeros(n_actions), sigma=0.1 * np.ones(n_actions))) + }[args.algo] + + model = algo('MlpPolicy', env, verbose=1, **hyperparams) + model.learn(n_timesteps, callback=callback) + + print("Saving to {}.zip".format(save_path)) + model.save(save_path) diff --git a/examples/pybullet/gym/pybullet_envs/stable_baselines/utils.py b/examples/pybullet/gym/pybullet_envs/stable_baselines/utils.py new file mode 100644 index 000000000..1e2c054c8 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/stable_baselines/utils.py @@ -0,0 +1,114 @@ +# Code adapted from https://github.com/araffin/rl-baselines-zoo +# it requires stable-baselines to be installed +# Author: Antonin RAFFIN +# MIT License +import gym +import numpy as np +from gym.wrappers import TimeLimit + +from stable_baselines.common.evaluation import evaluate_policy + + +class TimeFeatureWrapper(gym.Wrapper): + """ + Add remaining time to observation space for fixed length episodes. + See https://arxiv.org/abs/1712.00378 and https://github.com/aravindr93/mjrl/issues/13. + + :param env: (gym.Env) + :param max_steps: (int) Max number of steps of an episode + if it is not wrapped in a TimeLimit object. + :param test_mode: (bool) In test mode, the time feature is constant, + equal to zero. This allow to check that the agent did not overfit this feature, + learning a deterministic pre-defined sequence of actions. + """ + def __init__(self, env, max_steps=1000, test_mode=False): + assert isinstance(env.observation_space, gym.spaces.Box) + # Add a time feature to the observation + low, high = env.observation_space.low, env.observation_space.high + low, high= np.concatenate((low, [0])), np.concatenate((high, [1.])) + env.observation_space = gym.spaces.Box(low=low, high=high, dtype=np.float32) + + super(TimeFeatureWrapper, self).__init__(env) + + if isinstance(env, TimeLimit): + self._max_steps = env._max_episode_steps + else: + self._max_steps = max_steps + self._current_step = 0 + self._test_mode = test_mode + + def reset(self): + self._current_step = 0 + return self._get_obs(self.env.reset()) + + def step(self, action): + self._current_step += 1 + obs, reward, done, info = self.env.step(action) + return self._get_obs(obs), reward, done, info + + def _get_obs(self, obs): + """ + Concatenate the time feature to the current observation. + + :param obs: (np.ndarray) + :return: (np.ndarray) + """ + # Remaining time is more general + time_feature = 1 - (self._current_step / self._max_steps) + if self._test_mode: + time_feature = 1.0 + # Optionnaly: concatenate [time_feature, time_feature ** 2] + return np.concatenate((obs, [time_feature])) + + +class EvalCallback(object): + """ + Callback for evaluating an agent. + + :param eval_env: (gym.Env) The environment used for initialization + :param n_eval_episodes: (int) The number of episodes to test the agent + :param eval_freq: (int) Evaluate the agent every eval_freq call of the callback. + :param deterministic: (bool) + :param best_model_save_path: (str) + :param verbose: (int) + """ + def __init__(self, eval_env, n_eval_episodes=5, eval_freq=10000, + deterministic=True, best_model_save_path=None, verbose=1): + super(EvalCallback, self).__init__() + self.n_eval_episodes = n_eval_episodes + self.eval_freq = eval_freq + self.best_mean_reward = -np.inf + self.deterministic = deterministic + self.eval_env = eval_env + self.verbose = verbose + self.model, self.num_timesteps = None, 0 + self.best_model_save_path = best_model_save_path + self.n_calls = 0 + + def __call__(self, locals_, globals_): + """ + :param locals_: (dict) + :param globals_: (dict) + :return: (bool) + """ + self.n_calls += 1 + self.model = locals_['self'] + self.num_timesteps = self.model.num_timesteps + + if self.n_calls % self.eval_freq == 0: + episode_rewards, _ = evaluate_policy(self.model, self.eval_env, n_eval_episodes=self.n_eval_episodes, + deterministic=self.deterministic, return_episode_rewards=True) + + + mean_reward, std_reward = np.mean(episode_rewards), np.std(episode_rewards) + if self.verbose > 0: + print("Eval num_timesteps={}, " + "episode_reward={:.2f} +/- {:.2f}".format(self.num_timesteps, mean_reward, std_reward)) + + if mean_reward > self.best_mean_reward: + if self.best_model_save_path is not None: + print("Saving best model") + self.model.save(self.best_model_save_path) + self.best_mean_reward = mean_reward + + return True diff --git a/setup.py b/setup.py index bfadab97a..a6caac784 100644 --- a/setup.py +++ b/setup.py @@ -491,7 +491,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.6.3', + version='2.6.4', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp index 6fe56538d..89bc8d920 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp @@ -28,6 +28,7 @@ subject to the following restrictions: btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize) : btCollisionDispatcher(config) { + m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads()); m_batchUpdating = false; m_grainSize = grainSize; // iterations per task } @@ -65,6 +66,10 @@ btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionO manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); } + else + { + m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold); + } return manifold; } @@ -121,7 +126,7 @@ struct CollisionDispatcherUpdater : public btIParallelForBody void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) { - int pairCount = pairCache->getNumOverlappingPairs(); + const int pairCount = pairCache->getNumOverlappingPairs(); if (pairCount == 0) { return; @@ -136,16 +141,17 @@ void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* btParallelFor(0, pairCount, m_grainSize, updater); m_batchUpdating = false; - // reconstruct the manifolds array to ensure determinism - m_manifoldsPtr.resizeNoInitialize(0); - - btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr(); - for (int i = 0; i < pairCount; ++i) + // merge new manifolds, if any + for (int i = 0; i < m_batchManifoldsPtr.size(); ++i) { - if (btCollisionAlgorithm* algo = pairs[i].m_algorithm) + btAlignedObjectArray& batchManifoldsPtr = m_batchManifoldsPtr[i]; + + for (int j = 0; j < batchManifoldsPtr.size(); ++j) { - algo->getAllContactManifolds(m_manifoldsPtr); + m_manifoldsPtr.push_back(batchManifoldsPtr[j]); } + + batchManifoldsPtr.resizeNoInitialize(0); } // update the indices (used when releasing manifolds) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h index 28eba7f32..1155de2cf 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h @@ -30,6 +30,7 @@ public: virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE; protected: + btAlignedObjectArray > m_batchManifoldsPtr; bool m_batchUpdating; int m_grainSize; }; diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 1bb21104c..b5f4a3c86 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -139,7 +139,12 @@ public: if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, childTrans, -1, index); + btTransform preTransform = childTrans; + if (this->m_compoundColObjWrap->m_preTransform) + { + preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform)); + } + btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index); btCollisionAlgorithm* algo = 0; bool allocatedAlgorithm = false; diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index e82d1b139..50c09d83e 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -46,7 +46,7 @@ struct btContactSolverInfoData btScalar m_sor; //successive over-relaxation term btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints - btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_erp; //error reduction for deformable constraints btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -82,7 +82,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.); + m_deformable_erp = btScalar(0.3); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 1b247641a..eb50f5a56 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -186,9 +186,9 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack } //set constraints as projections -void btDeformableBackwardEulerObjective::setConstraints() +void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) { - m_projection.setConstraints(); + m_projection.setConstraints(infoGlobal); } void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 05ab42ff0..89cbb38c5 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -79,7 +79,7 @@ public: void updateVelocity(const TVStack& dv); //set constraints as projections - void setConstraints(); + void setConstraints(const btContactSolverInfo& infoGlobal); // update the projections and project the residual void project(TVStack& r) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 7724a8ec6..6404ce6ee 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -228,16 +228,16 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArrayreinitialize(nodeUpdated, dt); } -void btDeformableBodySolver::setConstraints() +void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) { BT_PROFILE("setConstraint"); - m_objective->setConstraints(); + m_objective->setConstraints(infoGlobal); } -btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies) +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal); return maxSquaredResidual; } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index f78a8f696..bce0d30b3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -65,7 +65,7 @@ public: virtual void solveDeformableConstraints(btScalar solverdt); // solve the contact between deformable and rigid as well as among deformables - btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies); + btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); // solve the position error between deformable and rigid as well as among deformables; btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); @@ -77,7 +77,7 @@ public: void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); // set up contact constraints - void setConstraints(); + void setConstraints(const btContactSolverInfo& infoGlobal); // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion virtual void predictMotion(btScalar solverdt); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index e8219dc50..3a5dfc88d 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -15,9 +15,9 @@ #include "btDeformableContactConstraint.h" /* ================ Deformable Node Anchor =================== */ -btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a) +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal) : m_anchor(&a) -, btDeformableContactConstraint(a.m_cti.m_normal) +, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) { } @@ -79,7 +79,7 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const return va; } -btScalar btDeformableNodeAnchorConstraint::solveConstraint() +btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { const btSoftBody::sCti& cti = m_anchor->m_cti; btVector3 va = getVa(); @@ -134,14 +134,14 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) } /* ================ Deformable vs. Rigid =================== */ -btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c) +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal) : m_contact(&c) -, btDeformableContactConstraint(c.m_cti.m_normal) +, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) { m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); - // penetration is non-positive. The magnitude of penetration is the depth of penetration. - m_penetration = btMin(btScalar(0), c.m_cti.m_offset); + // The magnitude of penetration is the depth of penetration. + m_penetration = btMin(btScalar(0),c.m_cti.m_offset); } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) @@ -206,16 +206,16 @@ btVector3 btDeformableRigidContactConstraint::getVa() const return va; } -btScalar btDeformableRigidContactConstraint::solveConstraint() +btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); + const btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * vr; + btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); btVector3 impulse_tangent = impulse - impulse_normal; btVector3 old_total_tangent_dv = m_total_tangent_dv; @@ -256,6 +256,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() impulse = impulse_normal + impulse_tangent; // apply impulse to deformable nodes involved and change their velocities applyImpulse(impulse); + if (residualSquare < 1e-7) + return residualSquare; // apply impulse to the rigid/multibodies involved and change their velocities if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -319,9 +321,9 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo } /* ================ Node vs. Rigid =================== */ -btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact) +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) - , btDeformableRigidContactConstraint(contact) + , btDeformableRigidContactConstraint(contact, infoGlobal) { } @@ -357,9 +359,9 @@ void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& }; /* ================ Face vs. Rigid =================== */ -btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact) +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal) : m_face(contact.m_face) -, btDeformableRigidContactConstraint(contact) +, btDeformableRigidContactConstraint(contact, infoGlobal) { } @@ -411,19 +413,60 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul v1 -= dv * contact->m_weights[1]; if (im2 > 0) v2 -= dv * contact->m_weights[2]; - - // apply strain limiting to prevent undamped modes - btScalar m01 = (btScalar(1)/(im0 + im1)); - btScalar m02 = (btScalar(1)/(im0 + im2)); - btScalar m12 = (btScalar(1)/(im1 + im2)); - - btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); - btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); - btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); - - v0 += dv0; - v1 += dv1; - v2 += dv2; + + btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations); + btScalar m01 = (relaxation/(im0 + im1)); + btScalar m02 = (relaxation/(im0 + im2)); + btScalar m12 = (relaxation/(im1 + im2)); + #ifdef USE_STRAIN_RATE_LIMITING + // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. + btScalar p = 0.01; + btVector3& x0 = face->m_n[0]->m_x; + btVector3& x1 = face->m_n[1]->m_x; + btVector3& x2 = face->m_n[2]->m_x; + const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; + const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; + btVector3 u[3]; + btScalar x_diff_dot_u, dn[3]; + btScalar dt = m_infoGlobal->m_timeStep; + for (int i = 0; i < 3; ++i) + { + btScalar x_diff_norm = x_diff[i].safeNorm(); + btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); + btScalar strainRate = x_diff_norm_new/x_diff_norm; + u[i] = v_diff[i]; + u[i].safeNormalize(); + if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) + { + dn[i] = 0; + continue; + } + x_diff_dot_u = btDot(x_diff[i], u[i]); + btScalar s; + if (1-p > strainRate) + { + s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); + } + else + { + s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); + } + // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); + // strainRate = x_diff_norm_new/x_diff_norm; + dn[i] = s - v_diff[i].safeNorm(); + } + btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); +#else + // apply strain limiting to prevent undamped modes + btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); + btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); + btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); +#endif + v0 += dv0; + v1 += dv1; + v2 += dv2; } void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) @@ -447,11 +490,11 @@ void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& } /* ================ Face vs. Node =================== */ -btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact) +btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) , m_face(contact.m_face) , m_contact(&contact) -, btDeformableContactConstraint(contact.m_normal) +, btDeformableContactConstraint(contact.m_normal, infoGlobal) { m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); @@ -487,7 +530,7 @@ btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n return dv * contact->m_weights[2]; } -btScalar btDeformableFaceNodeContactConstraint::solveConstraint() +btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { btVector3 va = getVa(); btVector3 vb = getVb(); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 912119e7c..7acf9aa69 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -24,31 +24,33 @@ public: // True if the friction is static // False if the friction is dynamic bool m_static; - - // normal of the contact - btVector3 m_normal; - - btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal) - { - } - - btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal) - { - } - - btDeformableContactConstraint(const btDeformableContactConstraint& other) - : m_static(other.m_static) - , m_normal(other.m_normal) - { - - } - btDeformableContactConstraint(){} - + const btContactSolverInfo* m_infoGlobal; + + // normal of the contact + btVector3 m_normal; + + btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint(){} + + btDeformableContactConstraint(const btDeformableContactConstraint& other) + : m_static(other.m_static) + , m_normal(other.m_normal) + , m_infoGlobal(other.m_infoGlobal) + { + } + virtual ~btDeformableContactConstraint(){} // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact - virtual btScalar solveConstraint() = 0; + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; // solve the position error by applying an inelastic impulse that changes only the position (not velocity) virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0; @@ -79,22 +81,19 @@ class btDeformableStaticConstraint : public btDeformableContactConstraint public: const btSoftBody::Node* m_node; - btDeformableStaticConstraint(){} - - btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0)) + btDeformableStaticConstraint(const btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) { } - + btDeformableStaticConstraint(){} btDeformableStaticConstraint(const btDeformableStaticConstraint& other) : m_node(other.m_node) , btDeformableContactConstraint(other) { - } virtual ~btDeformableStaticConstraint(){} - virtual btScalar solveConstraint() + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) { return 0; } @@ -130,14 +129,14 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint { public: const btSoftBody::DeformableNodeRigidAnchor* m_anchor; - - btDeformableNodeAnchorConstraint(){} - btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c); + + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + btDeformableNodeAnchorConstraint(){} virtual ~btDeformableNodeAnchorConstraint() { } - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) { // todo xuchenhan@ @@ -169,10 +168,10 @@ public: btVector3 m_total_tangent_dv; btScalar m_penetration; const btSoftBody::DeformableRigidContact* m_contact; - - btDeformableRigidContactConstraint(){} - btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c); + + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + btDeformableRigidContactConstraint(){} virtual ~btDeformableRigidContactConstraint() { } @@ -180,7 +179,7 @@ public: // object A is the rigid/multi body, and object B is the deformable node/face virtual btVector3 getVa() const; - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); @@ -197,11 +196,10 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo public: // the deformable node in contact const btSoftBody::Node* m_node; - - btDeformableNodeRigidContactConstraint(){} - btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact); + + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); - + btDeformableNodeRigidContactConstraint(){} virtual ~btDeformableNodeRigidContactConstraint() { } @@ -228,10 +226,9 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo { public: const btSoftBody::Face* m_face; - btDeformableFaceRigidContactConstraint(){} - btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact); + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); - + btDeformableFaceRigidContactConstraint(){} virtual ~btDeformableFaceRigidContactConstraint() { } @@ -263,13 +260,11 @@ public: btVector3 m_total_normal_dv; btVector3 m_total_tangent_dv; - btDeformableFaceNodeContactConstraint(){} - - btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact); - + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableFaceNodeContactConstraint(){} virtual ~btDeformableFaceNodeContactConstraint(){} - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) { diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 5a4f3241b..a59177e16 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include #include -btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies) +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; for (int i = 0; i < numDeformableBodies; ++i) @@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) { btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k) { btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) { btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_deformableConstraints[j].size(); ++k) { btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } } @@ -108,7 +108,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI return residualSquare; } -void btDeformableContactProjection::setConstraints() +void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) { BT_PROFILE("setConstraints"); for (int i = 0; i < m_softBodies.size(); ++i) @@ -124,7 +124,7 @@ void btDeformableContactProjection::setConstraints() { if (psb->m_nodes[j].m_im == 0) { - btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal); m_staticConstraints[i].push_back(static_constraint); } } @@ -139,7 +139,7 @@ void btDeformableContactProjection::setConstraints() continue; } anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; - btDeformableNodeAnchorConstraint constraint(anchor); + btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); m_nodeAnchorConstraints[i].push_back(constraint); } @@ -152,7 +152,7 @@ void btDeformableContactProjection::setConstraints() { continue; } - btDeformableNodeRigidContactConstraint constraint(contact); + btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; @@ -173,7 +173,7 @@ void btDeformableContactProjection::setConstraints() { continue; } - btDeformableFaceRigidContactConstraint constraint(contact); + btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; @@ -190,7 +190,7 @@ void btDeformableContactProjection::setConstraints() { const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; - btDeformableFaceNodeContactConstraint constraint(contact); + btDeformableFaceNodeContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 3c4490765..32d6c4d75 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -72,13 +72,13 @@ public: virtual void applyDynamicFriction(TVStack& f); // update and solve the constraints - virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies); + virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); // solve the position error using split impulse virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. - virtual void setConstraints(); + virtual void setConstraints(const btContactSolverInfo& infoGlobal); // Set up projections for each vertex by adding the projection direction to virtual void setProjection(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 06f95d69f..c8cc47923 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); // solver body velocity -> rigid body velocity solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal); // update rigid body velocity in rigid/deformable contact m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); // solver body velocity <- rigid body velocity @@ -112,7 +112,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI if (infoGlobal.m_splitImpulse) { { - m_deformableSolver->splitImpulseSetup(infoGlobal); +// m_deformableSolver->splitImpulseSetup(infoGlobal); for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { btScalar leastSquaresResidual = 0.f; @@ -127,8 +127,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } // solve the position correction between deformable and rigid/multibody - btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); - leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); +// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); +// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index af6f9806c..f8de4b11e 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -285,7 +285,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::setupConstraints() { // set up constraints between multibody and deformable bodies - m_deformableBodySolver->setConstraints(); + m_deformableBodySolver->setConstraints(m_solverInfo); // set up constraints among multibodies { diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 2a458b1d8..634778d00 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -53,6 +53,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV n.m_material = pm; } updateBounds(); + setCollisionQuadrature(3); } btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) @@ -2403,10 +2404,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect // but resolve contact at x_n -// btTransform wtr = (predict) ? -// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) -// : colObjWrap->getWorldTransform(); - const btTransform& wtr = colObjWrap->getWorldTransform(); + btTransform wtr = (predict) ? + (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), @@ -2457,10 +2457,9 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) : colObjWrap->getWorldTransform(); -// const btTransform& wtr = colObjWrap->getWorldTransform(); btScalar dst; -//#define USE_QUADRATURE 1 +#define USE_QUADRATURE 1 //#define CACHE_PREV_COLLISION // use the contact position of the previous collision @@ -2476,6 +2475,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO nrm, margin); nrm = wtr.getBasis() * nrm; + cti.m_colObj = colObjWrap->getCollisionObject(); // use cached contact point } else @@ -2492,10 +2492,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO contact_point = results.witnesses[0]; getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); nrm = results.normal; + cti.m_colObj = colObjWrap->getCollisionObject(); for (int i = 0; i < 3; ++i) f.m_pcontact[i] = bary[i]; } - + return (dst < 0); #endif // use collision quadrature point @@ -2505,7 +2506,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btVector3 local_nrm; for (int q = 0; q < m_quads.size(); ++q) { - btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); + btVector3 p; + if (predict) + p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); + else + p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(p), shp, @@ -2513,12 +2518,21 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO margin); if (local_dst < dst) { + if (local_dst < 0 && predict) + return true; dst = local_dst; contact_point = p; bary = m_quads[q]; - nrm = wtr.getBasis() * local_nrm; + nrm = local_nrm; + } + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; } } + return (dst < 0); } #endif @@ -2530,6 +2544,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO triangle_transform.setOrigin(f.m_n[0]->m_x); btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); btVector3 guess(0,0,0); + if (predict) + { + triangle_transform.setOrigin(f.m_n[0]->m_q); + triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); + } const btConvexShape* csh = static_cast(shp); btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); dst = results.distance - margin; @@ -2547,9 +2566,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO cti.m_offset = dst; } - if (dst < 0) - return true; - return (false); + return (dst < 0); } // @@ -3681,8 +3698,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap docollideFace.psb = this; docollideFace.m_colObj1Wrap = pcoWrap; docollideFace.m_rigidBody = prb1; - docollideFace.dynmargin = basemargin + timemargin; - docollideFace.stamargin = basemargin; + docollideFace.dynmargin = 0.05*(basemargin + timemargin); + docollideFace.stamargin = 0.05*basemargin; m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); } } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index cde4746d5..6ef0ee367 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1070,8 +1070,8 @@ struct btSoftColliders if (!n.m_battach) { - // check for collision at x_{n+1}^* as well at x_n - if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) + // check for collision at x_{n+1}^* + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; // todo: collision between multibody and fixed deformable node will be missed. @@ -1159,7 +1159,6 @@ struct btSoftColliders btSoftBody::Node* n0 = f.m_n[0]; btSoftBody::Node* n1 = f.m_n[1]; btSoftBody::Node* n2 = f.m_n[2]; - const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; btSoftBody::DeformableFaceRigidContact c; btVector3 contact_point; @@ -1181,6 +1180,8 @@ struct btSoftColliders // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary; c.m_face = &f; + // friction is handled by the nodes to prevent sticking +// const btScalar fc = 0; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf