Merge pull request #2499 from xhan0619/fix-render-mesh

Fix render mesh

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -338,6 +338,24 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle
 	return 0;
 }
 
+B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+    b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
+    int len = strlen(filename);
+    if (len < MAX_FILENAME_LENGTH)
+    {
+        strcpy(command->m_loadSoftBodyArguments.m_simFileName, filename);
+    }
+    else
+    {
+        command->m_loadSoftBodyArguments.m_simFileName[0] = 0;
+    }
+    command->m_updateFlags |= LOAD_SOFT_BODY_SIM_MESH;
+    return 0;
+
+}
+
 B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -375,7 +375,6 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 	B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
-
 	///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
 	///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
@@ -634,6 +633,8 @@ extern "C"
 	B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
 	B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
 	B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
+    B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename);
+	B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
     B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
 	B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping);
 	B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1709,7 +1709,6 @@ struct PhysicsServerCommandProcessorInternalData
 		  m_dispatcher(0),
 		  m_solver(0),
 		  m_collisionConfiguration(0),
-		  
 #ifndef SKIP_DEFORMABLE_BODY
 		  m_deformablebodySolver(0),
 #endif
@@ -2665,9 +2664,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
 
         if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD)))
 	{
-#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
         m_data->m_solver = new btMultiBodyConstraintSolver;
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
         m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
+#else
+        m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
 #endif
 	}
 
@@ -8083,27 +8084,21 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 		b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
 	}
 
-        
 	const std::string& error_message_prefix = "";
-	std::string out_found_filename;
+	std::string out_found_filename, out_found_sim_filename;
-	std::string out_found_sim_filename;
 	int out_type(0), out_sim_type(0);
 
-        bool render_mesh_is_sim_mesh = true;
-
 	bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
-        if (out_type == UrdfGeometry::FILE_OBJ)
-        {
-		    foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
-            render_mesh_is_sim_mesh = !foundFile;
-        }
 
-        if (render_mesh_is_sim_mesh)
+        if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
+        {
+            bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
+        }
+        else
         {
           out_sim_type = out_type;
           out_found_sim_filename = out_found_filename;
         }
-        
         if (out_sim_type == UrdfGeometry::FILE_OBJ)
         {
 	    std::vector<tinyobj::shape_t> shapes;
@@ -8192,6 +8187,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 					deformWorld->addForce(psb, springForce);
 					m_data->m_lf.push_back(springForce);
 				}
+                
 			}
 #endif
 		}
@@ -8202,13 +8198,11 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 		  	btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
 			if (deformWorld)
 			{
-            if (!render_mesh_is_sim_mesh)
+            			// load render mesh
+            			if (out_found_sim_filename != out_found_filename)
             			{
             			    // load render mesh
-
-
 	    			    {
-					
 	    			    	tinyobj::attrib_t attribute;
 	    			    	std::vector<tinyobj::shape_t> shapes;
 	    			    	
@@ -8244,7 +8238,6 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 	    			    	}
 	    			    	
 	    			    }
-
             			    btSoftBodyHelpers::interpolateBarycentricWeights(psb);
             			}
             			else

examples/SharedMemory/SharedMemoryCommands.h

@@ -505,6 +505,7 @@ enum EnumLoadSoftBodyUpdateFlags
         LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
         LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13,
     LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
+    LOAD_SOFT_BODY_SIM_MESH = 1<<15,
 };
 
 enum EnumSimParamInternalSimFlags
@@ -535,6 +536,7 @@ struct LoadSoftBodyArgs
         double m_NeoHookeanLambda;
         double m_NeoHookeanDamping;
     int m_useFaceContact;
+    char m_simFileName[MAX_FILENAME_LENGTH];
 };
 
 struct b3LoadSoftBodyResultArgs

examples/pybullet/examples/deformable_anchor.py

@@ -2,8 +2,8 @@ import pybullet as p
 from time import sleep
 
 physicsClient = p.connect(p.GUI)
-
 p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
 gravZ=-10
 p.setGravity(0, 0, gravZ)
 

examples/pybullet/examples/test_inertia.py

@@ -0,0 +1,28 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+
+useDeformable = True
+if useDeformable:
+	p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+gravZ=-10
+p.setGravity(0, 0, gravZ)
+
+planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
+planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+
+boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
+
+clothId = p.loadSoftBody("bunny.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1, useMassSpring=1, springElasticStiffness=100, springDampingStiffness=.001, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
+
+p.setTimeStep(0.0005)
+
+p.setRealTimeSimulation(1)
+
+
+while p.isConnected():
+  p.setGravity(0,0,gravZ)
+  sleep(1./240.)
+