From a018d63fb3944fb5e45e042ad2b1ba4ad3173050 Mon Sep 17 00:00:00 2001
From: jyc-n <chenjy@g.ucla.edu>
Date: Wed, 1 Dec 2021 22:39:23 -0500
Subject: [PATCH] debug pybullet setup

---
 .../RobotSimulator/HelloBulletRobotics.cpp    | 12 ++++++-----
 .../RobotSimulator/RobotSimulatorMain.cpp     | 20 +++++++++++++++----
 .../PhysicsServerCommandProcessor.cpp         | 10 +++++++++-
 3 files changed, 32 insertions(+), 10 deletions(-)

diff --git a/examples/RobotSimulator/HelloBulletRobotics.cpp b/examples/RobotSimulator/HelloBulletRobotics.cpp
index ec656be60..9fac0e4aa 100644
--- a/examples/RobotSimulator/HelloBulletRobotics.cpp
+++ b/examples/RobotSimulator/HelloBulletRobotics.cpp
@@ -18,7 +18,8 @@ int main(int argc, char* argv[])
 	}
 
 	//remove all existing objects (if any)
-	sim->resetSimulation();
+	// sim->resetSimulation();
+	sim->resetSimulation(RESET_USE_REDUCED_DEFORMABLE_WORLD);
 	sim->setGravity(btVector3(0, 0, -9.8));
 	sim->setNumSolverIterations(100);
 	b3RobotSimulatorSetPhysicsEngineParameters args;
@@ -27,10 +28,11 @@ int main(int argc, char* argv[])
 	int planeUid = sim->loadURDF("plane.urdf");
 	printf("planeUid = %d\n", planeUid);
 
-	int r2d2Uid = sim->loadURDF("r2d2.urdf");
-	printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid));
+	int r2d2Uid = sim->loadURDF("reduced_bottle_coarse/reduced_bottle_coarse.urdf");
+	// int r2d2Uid = sim->loadURDF("r2d2.urdf");
+	// printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid));
 
-	btVector3 basePosition = btVector3(0, 0, 0.5);
+	btVector3 basePosition = btVector3(0, 0.5, 0.2);
 	btQuaternion baseOrientation = btQuaternion(0, 0, 0, 1);
 
 	sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);
@@ -40,7 +42,7 @@ int main(int argc, char* argv[])
 		btVector3 basePos;
 		btQuaternion baseOrn;
 		sim->getBasePositionAndOrientation(r2d2Uid, basePos, baseOrn);
-		printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0], basePos[1], basePos[2]);
+		// printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0], basePos[1], basePos[2]);
 
 		sim->stepSimulation();
 	}
diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp
index 3e2dcd581..37963caaf 100644
--- a/examples/RobotSimulator/RobotSimulatorMain.cpp
+++ b/examples/RobotSimulator/RobotSimulatorMain.cpp
@@ -55,12 +55,24 @@ int main(int argc, char* argv[])
 	sim->loadURDF("plane.urdf");
 
 	{
-		int deformableUID = sim->loadURDF("reduced_cube/reduced_cube.urdf");
-		// int deformableUID = sim->loadURDF("reduced_cube/deform_cube.urdf");
-		// int deformableUID = sim->loadURDF("reduced_torus/reduced_torus.urdf");
-		// int deformableUID = sim->loadURDF("torus_deform.urdf");
+		int cubeID = sim->loadURDF("cube.urdf");
 		btVector3 basePosition = btVector3(0, 0, 2);
 		btQuaternion baseOrientation = btQuaternion(0, 0, 0, 1);
+		sim->resetBasePositionAndOrientation(cubeID, basePosition, baseOrientation);
+	}
+
+	{
+		// int deformableUID = sim->loadURDF("reduced_torus/reduced_torus.urdf");
+		int deformableUID = sim->loadURDF("reduced_cube/reduced_cube.urdf");
+		// int deformableUID = sim->loadURDF("reduced_bottle/reduced_bottle.urdf");
+		// int deformableUID = sim->loadURDF("reduced_bottle_coarse/reduced_bottle_coarse.urdf");
+		// int deformableUID = sim->loadURDF("reduced_bottle/deform_bottle.urdf");
+		// int deformableUID = sim->loadURDF("torus_deform.urdf");
+		btVector3 basePosition = btVector3(0, 3, 3);
+		// btVector3 basePosition = btVector3(0, 0, 0.2);
+		// btVector3 basePosition = btVector3(0, 0.5, 0.2);
+		// btQuaternion baseOrientation = btQuaternion(0, 0, 0, 1);
+		btQuaternion baseOrientation = btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0);
 		sim->resetBasePositionAndOrientation(deformableUID, basePosition, baseOrientation);
 	}
 
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index d9748c553..33db531d9 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -9216,6 +9216,10 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 			psb->setGravityFactor(deformable.m_gravFactor);
 			psb->setCacheBarycenter(deformable.m_cache_barycenter);
 			psb->initializeFaceTree();
+
+			deformWorld->setImplicit(true);
+			deformWorld->setLineSearch(false);
+			deformWorld->setUseProjection(true);
 		}
 #endif  //SKIP_DEFORMABLE_BODY
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -9646,7 +9650,6 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 			// collion between deformable and deformable and self-collision
 			// rsb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
 			rsb->setCollisionFlags(0);
-			rsb->setTotalMass(reduced_deformable.m_mass);
 			rsb->setSelfCollision(useSelfCollision);
 			rsb->initializeFaceTree();
 		}
@@ -9681,6 +9684,11 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 			deformWorld->addSoftBody(rsb);
 			deformWorld->getSolverInfo().m_deformable_erp = reduced_deformable.m_erp;
 			deformWorld->getSolverInfo().m_deformable_cfm = reduced_deformable.m_cfm;
+			deformWorld->getSolverInfo().m_friction = reduced_deformable.m_friction;
+			deformWorld->getSolverInfo().m_splitImpulse = false;
+			deformWorld->setImplicit(false);
+			deformWorld->setLineSearch(false);
+			deformWorld->setUseProjection(false);
 		}
 		else
 #endif  //SKIP_DEFORMABLE_BODY