Merge pull request #1500 from YunfeiBai/master

Load softbody from obj file.

CMakeLists.txt

@@ -26,7 +26,7 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG")
 OPTION(USE_DOUBLE_PRECISION "Use double precision"	OFF)
 OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
 OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
-OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)
+OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON)
 
 OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
 IF (BULLET2_USE_THREAD_LOCKS)
@@ -217,8 +217,8 @@ ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION)
 SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION")
 ENDIF (USE_DOUBLE_PRECISION)
 
-IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+IF (NOT USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
-ADD_DEFINITIONS(-DUSE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+ADD_DEFINITIONS(-DSKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
 ENDIF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
 
 IF(USE_GRAPHICAL_BENCHMARK)

examples/ExampleBrowser/ExampleEntries.cpp

@@ -279,10 +279,10 @@ static ExampleEntry gDefaultExamples[]=
     ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
     ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
 	ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
-#ifdef  USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#ifndef  SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 	ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
 	ExampleEntry(1,"Softbody Multibody Coupling","Two way coupling between soft body and multibody experiment", GripperGraspExampleCreateFunc, eSOFTBODY_MULTIBODY_COUPLING),
-#endif //USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 
 #ifdef ENABLE_LUA
 	ExampleEntry(1,"Lua Script", "Create the dynamics world, collision shapes and rigid bodies using Lua scripting",

examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp

@@ -1143,7 +1143,7 @@ void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string&  profileN
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
 }
 
-void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double collisionMargin)
+void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
 {
 	if (!isConnected())
 	{
@@ -1151,9 +1151,9 @@ void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double coll
 		return;
 	}
 
-    b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClientHandle);
+    b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-    b3LoadBunnySetScale(command, scale);
+    b3LoadSoftBodySetScale(command, scale);
-    b3LoadBunnySetMass(command, mass);
+    b3LoadSoftBodySetMass(command, mass);
-    b3LoadBunnySetCollisionMargin(command, collisionMargin);
+    b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
     b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 }

examples/RobotSimulator/b3RobotSimulatorClientAPI.h

@@ -229,7 +229,7 @@ public:
 
 	//////////////// INTERNAL
 
-	void loadBunny(double scale, double mass, double collisionMargin);
+	void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin);
 
 	//setGuiHelper is only used when embedded in existing example browser
 	void setGuiHelper(struct GUIHelperInterface* guiHelper);

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -26,6 +26,11 @@ class GripperGraspExample : public CommonExampleInterface
 	b3RobotSimulatorClientAPI m_robotSim;
 	int m_options;
     int m_gripperIndex;
+	double m_time;
+	b3Vector3 m_targetPos;
+	b3Vector3 m_worldPos;
+	b3Vector4 m_targetOri;
+	b3Vector4 m_worldOri;
 	
  	b3AlignedObjectArray<int> m_movingInstances;
 	enum
@@ -331,7 +336,7 @@ public:
                 
             }
             m_robotSim.setGravity(b3MakeVector3(0,0,-10));
-            m_robotSim.loadBunny(0.1,0.1,0.02);
+            m_robotSim.loadSoftBody("bunny.obj",0.1,0.1,0.02);
             
             b3JointInfo revoluteJoint1;
             revoluteJoint1.m_parentFrame[0] = -0.055;
@@ -379,11 +384,11 @@ public:
         {
             {
                 b3RobotSimulatorLoadUrdfFileArgs args;
-                args.m_startPosition.setValue(0,1.0,2.0);
+				args.m_startPosition.setValue(-0.5,0,0.1);
-                args.m_startOrientation.setEulerZYX(0,0,1.57);
+				args.m_startOrientation.setEulerZYX(0,0,0);
-                args.m_forceOverrideFixedBase = false;
+				args.m_forceOverrideFixedBase = true;
                 args.m_useMultiBody = true;
-                int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args);
+                int kukaId = m_robotSim.loadURDF("kuka_iiwa/model.urdf", args);
                 
                 int numJoints = m_robotSim.getNumJoints(kukaId);
                 b3Printf("numJoints = %d",numJoints);
@@ -394,7 +399,7 @@ public:
                     m_robotSim.getJointInfo(kukaId,i,&jointInfo);
                     b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
                     b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
-                    controlArgs.m_maxTorqueValue = 0.0;
+                    controlArgs.m_maxTorqueValue = 500.0;
                     m_robotSim.setJointMotorControl(kukaId,i,controlArgs);
                 }
             }
@@ -407,7 +412,7 @@ public:
                 m_robotSim.loadURDF("plane.urdf", args);
             }
             m_robotSim.setGravity(b3MakeVector3(0,0,-10));
-            m_robotSim.loadBunny(0.5,10.0,0.1);
+            m_robotSim.loadSoftBody("bunny.obj",0.3,10.0,0.1);
         }
     }
     virtual void    exitPhysics()
@@ -468,6 +473,120 @@ public:
             }
         }
 		
+		if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0)
+		{
+			float dt = deltaTime;
+			btClamp(dt,0.0001f,0.01f);
+			
+			m_time+=dt;
+			m_targetPos.setValue(0, 0, 0.5+0.2*b3Cos( m_time));
+			m_targetOri.setValue(0, 1.0, 0, 0);
+			
+			
+			int numJoints = m_robotSim.getNumJoints(0);
+			
+			if (numJoints==7)
+			{
+				double q_current[7]={0,0,0,0,0,0,0};
+				
+				b3JointStates2 jointStates;
+				
+				if (m_robotSim.getJointStates(0,jointStates))
+				{
+					//skip the base positions (7 values)
+					b3Assert(7+numJoints == jointStates.m_numDegreeOfFreedomQ);
+					for (int i=0;i<numJoints;i++)
+					{
+						q_current[i] = jointStates.m_actualStateQ[i+7];
+					}
+				}
+				// compute body position and orientation
+				b3LinkState linkState;
+				m_robotSim.getLinkState(0, 6, &linkState);
+				
+				m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
+				m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);
+				
+				b3Vector3DoubleData targetPosDataOut;
+				m_targetPos.serializeDouble(targetPosDataOut);
+				b3Vector3DoubleData worldPosDataOut;
+				m_worldPos.serializeDouble(worldPosDataOut);
+				b3Vector3DoubleData targetOriDataOut;
+				m_targetOri.serializeDouble(targetOriDataOut);
+				b3Vector3DoubleData worldOriDataOut;
+				m_worldOri.serializeDouble(worldOriDataOut);
+				
+				
+				b3RobotSimulatorInverseKinematicArgs ikargs;
+				b3RobotSimulatorInverseKinematicsResults ikresults;
+				
+				ikargs.m_bodyUniqueId = 0;
+				//			ikargs.m_currentJointPositions = q_current;
+				//			ikargs.m_numPositions = 7;
+				ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0];
+				ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
+				ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
+				
+				ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION/* + B3_HAS_NULL_SPACE_VELOCITY*/;
+				
+				ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
+				ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
+				ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
+				ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
+				ikargs.m_endEffectorLinkIndex = 6;
+				
+				// Settings based on default KUKA arm setting
+				ikargs.m_lowerLimits.resize(numJoints);
+				ikargs.m_upperLimits.resize(numJoints);
+				ikargs.m_jointRanges.resize(numJoints);
+				ikargs.m_restPoses.resize(numJoints);
+				ikargs.m_lowerLimits[0] = -2.32;
+				ikargs.m_lowerLimits[1] = -1.6;
+				ikargs.m_lowerLimits[2] = -2.32;
+				ikargs.m_lowerLimits[3] = -1.6;
+				ikargs.m_lowerLimits[4] = -2.32;
+				ikargs.m_lowerLimits[5] = -1.6;
+				ikargs.m_lowerLimits[6] = -2.4;
+				ikargs.m_upperLimits[0] = 2.32;
+				ikargs.m_upperLimits[1] = 1.6;
+				ikargs.m_upperLimits[2] = 2.32;
+				ikargs.m_upperLimits[3] = 1.6;
+				ikargs.m_upperLimits[4] = 2.32;
+				ikargs.m_upperLimits[5] = 1.6;
+				ikargs.m_upperLimits[6] = 2.4;
+				ikargs.m_jointRanges[0] = 5.8;
+				ikargs.m_jointRanges[1] = 4;
+				ikargs.m_jointRanges[2] = 5.8;
+				ikargs.m_jointRanges[3] = 4;
+				ikargs.m_jointRanges[4] = 5.8;
+				ikargs.m_jointRanges[5] = 4;
+				ikargs.m_jointRanges[6] = 6;
+				ikargs.m_restPoses[0] = 0;
+				ikargs.m_restPoses[1] = 0;
+				ikargs.m_restPoses[2] = 0;
+				ikargs.m_restPoses[3] = SIMD_HALF_PI;
+				ikargs.m_restPoses[4] = 0;
+				ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
+				ikargs.m_restPoses[6] = 0;
+				ikargs.m_numDegreeOfFreedom = numJoints;
+				
+				if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
+				{
+					//copy the IK result to the desired state of the motor/actuator
+					for (int i=0;i<numJoints;i++)
+					{
+						b3RobotSimulatorJointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
+						t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
+						t.m_maxTorqueValue = 100.0;
+						t.m_kp= 1.0;
+						t.m_targetVelocity = 0;
+						t.m_kd = 1.0;
+						m_robotSim.setJointMotorControl(0,i,t);
+					}
+				}
+			}
+		}
+		
 		m_robotSim.stepSimulation();
     }
     virtual void	renderScene()

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -256,44 +256,57 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
 	}
 }
 
-B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
+B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
     b3Assert(cl);
     b3Assert(cl->canSubmitCommand());
 	
-    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	if (cl->canSubmitCommand())
-    b3Assert(command);
+	{
-    command->m_type = CMD_LOAD_BUNNY;
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
-    command->m_updateFlags = 0;
+		b3Assert(command);
+		command->m_type = CMD_LOAD_SOFT_BODY;
+		int len = strlen(fileName);
+		if (len < MAX_FILENAME_LENGTH)
+		{
+			strcpy(command->m_loadSoftBodyArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_loadSoftBodyArguments.m_fileName[0] = 0;
+		}
+		command->m_updateFlags = LOAD_SOFT_BODY_FILE_NAME;
 		
-    return (b3SharedMemoryCommandHandle) command;
+		return (b3SharedMemoryCommandHandle) command;
+	}
+	return 0;
 }
 
-B3_SHARED_API int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
+B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
-    b3Assert(command->m_type == CMD_LOAD_BUNNY);
+    b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
-    command->m_loadBunnyArguments.m_scale = scale;
+    command->m_loadSoftBodyArguments.m_scale = scale;
-    command->m_updateFlags |= LOAD_BUNNY_UPDATE_SCALE;
+    command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_SCALE;
     return 0;
 }
 
-B3_SHARED_API int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
+B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
-    b3Assert(command->m_type == CMD_LOAD_BUNNY);
+    b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
-    command->m_loadBunnyArguments.m_mass = mass;
+    command->m_loadSoftBodyArguments.m_mass = mass;
-    command->m_updateFlags |= LOAD_BUNNY_UPDATE_MASS;
+    command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_MASS;
     return 0;
 }
 
-B3_SHARED_API int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
+B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
-    b3Assert(command->m_type == CMD_LOAD_BUNNY);
+    b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
-    command->m_loadBunnyArguments.m_collisionMargin = collisionMargin;
+    command->m_loadSoftBodyArguments.m_collisionMargin = collisionMargin;
-    command->m_updateFlags |= LOAD_BUNNY_UPDATE_COLLISION_MARGIN;
+    command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN;
     return 0;
 }
 

examples/SharedMemory/PhysicsClientC_API.h

@@ -517,10 +517,10 @@ B3_SHARED_API	void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandl
 B3_SHARED_API	void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[/*3*/], int flag);
 
 ///experiments of robots interacting with non-rigid objects (such as btSoftBody)
-B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
-B3_SHARED_API	int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
+B3_SHARED_API	int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
-B3_SHARED_API	int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
+B3_SHARED_API	int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
-B3_SHARED_API	int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
+B3_SHARED_API	int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
 
 
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -51,7 +51,7 @@
 #include "../TinyAudio/b3SoundEngine.h"
 #endif
 
-#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "BulletSoftBody/btSoftBodyHelpers.h"
@@ -170,6 +170,9 @@ struct InternalBodyData
 {
 	btMultiBody* m_multiBody;
 	btRigidBody* m_rigidBody;
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+	btSoftBody* m_softBody;
+#endif
 	int m_testData;
 	std::string m_bodyName;
 
@@ -192,6 +195,9 @@ struct InternalBodyData
 	{
 		m_multiBody=0;
 		m_rigidBody=0;
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+		m_softBody = 0;
+#endif
 		m_testData=0;
 		m_bodyName="";
 		m_rootLocalInertialFrame.setIdentity();
@@ -1521,7 +1527,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btMultiBodyConstraintSolver*	m_solver;
 	btDefaultCollisionConfiguration* m_collisionConfiguration;
     
-#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 	btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
     btSoftBodySolver* m_softbodySolver;
     btSoftBodyWorldInfo	m_softBodyWorldInfo;
@@ -2182,7 +2188,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 {
     ///collision configuration contains default setup for memory, collision setup
     //m_collisionConfiguration->setConvexConvexMultipointIterations();
-#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
     m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
 #else
     m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
@@ -2205,7 +2211,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
     m_data->m_solver = new btMultiBodyConstraintSolver;
     
-#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#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);
@@ -2370,6 +2376,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
             m_data->m_dynamicsWorld->removeMultiBody(mb);
             delete mb;
         }
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+		for (i=m_data->m_dynamicsWorld->getSoftBodyArray().size()-1;i>=0;i--)
+		{
+			btSoftBody* sb = m_data->m_dynamicsWorld->getSoftBodyArray()[i];
+			m_data->m_dynamicsWorld->removeSoftBody(sb);
+			delete sb;
+		}
+#endif
 	}
 
     for (int i=0;i<constraints.size();i++)
@@ -5741,26 +5755,35 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe
 	return hasStatus;
 }
 
-bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
 	bool hasStatus = true;
-#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
     double scale = 0.1;
     double mass = 0.1;
     double collisionMargin = 0.02;
-    if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_SCALE)
+	const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
+	if (m_data->m_verboseOutput)
+	{
+		b3Printf("Processed CMD_LOAD_SOFT_BODY:%s", loadSoftBodyArgs.m_fileName);
+	}
+	btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) !=0);
+	btAssert(loadSoftBodyArgs.m_fileName);
+
+    if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
     {
-        scale = clientCmd.m_loadBunnyArguments.m_scale;
+        scale = clientCmd.m_loadSoftBodyArguments.m_scale;
     }
-    if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS)
+    if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS)
     {
-        mass = clientCmd.m_loadBunnyArguments.m_mass;
+        mass = clientCmd.m_loadSoftBodyArguments.m_mass;
     }
-    if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN)
+    if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
     {
-        collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin;
+        collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
     }
+	
     m_data->m_softBodyWorldInfo.air_density		=	(btScalar)1.2;
     m_data->m_softBodyWorldInfo.water_density	=	0;
     m_data->m_softBodyWorldInfo.water_offset	=	0;
@@ -5769,22 +5792,58 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
     m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase;
     m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize();
 	
-    btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny,                                                       &gIndicesBunny[0][0],                                                         BUNNY_NUM_TRIANGLES);
+	{
+		char relativeFileName[1024];
+		char pathPrefix[1024];
+		pathPrefix[0] = 0;
+		if (b3ResourcePath::findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
+		{
+			b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+		}
+		const std::string& error_message_prefix="";
+		std::string out_found_filename;
+		int out_type;
+		bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
+		std::vector<tinyobj::shape_t> shapes;
+		std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
+		if (shapes.size()>0)
+		{
+			const tinyobj::shape_t& shape = shapes[0];
+			btAlignedObjectArray<btScalar> vertices;
+			btAlignedObjectArray<int> indices;
+			for (int i=0;i<shape.mesh.positions.size();i++)
+			{
+				vertices.push_back(shape.mesh.positions[i]);
+			}
+			for (int i=0;i<shape.mesh.indices.size();i++)
+			{
+				indices.push_back(shape.mesh.indices[i]);
+			}
+			int numTris = indices.size()/3;
+			if (numTris>0)
+			{
+				btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,&vertices[0],&indices[0],numTris);
+				btSoftBody::Material*	pm=psb->appendMaterial();
+				pm->m_kLST				=	0.5;
+				pm->m_flags				-=	btSoftBody::fMaterial::DebugDraw;
+				psb->generateBendingConstraints(2,pm);
+				psb->m_cfg.piterations	=	20;
+				psb->m_cfg.kDF			=	0.5;
+				psb->randomizeConstraints();
+				psb->rotate(btQuaternion(0.70711,0,0,0.70711));
+				psb->translate(btVector3(-0.05,0,1.0));
+				psb->scale(btVector3(scale,scale,scale));
+				psb->setTotalMass(mass,true);
+				psb->getCollisionShape()->setMargin(collisionMargin);
+				m_data->m_dynamicsWorld->addSoftBody(psb);
+				int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
+				InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+				bodyHandle->m_softBody = psb;
+				serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
+			}
+		}
+	}
 	
-    btSoftBody::Material*	pm=psb->appendMaterial();
-    pm->m_kLST				=	1.0;
-    pm->m_flags				-=	btSoftBody::fMaterial::DebugDraw;
-    psb->generateBendingConstraints(2,pm);
-    psb->m_cfg.piterations	=	50;
-    psb->m_cfg.kDF			=	0.5;
-    psb->randomizeConstraints();
-    psb->rotate(btQuaternion(0.70711,0,0,0.70711));
-    psb->translate(btVector3(0,0,1.0));
-    psb->scale(btVector3(scale,scale,scale));
-    psb->setTotalMass(mass,true);
-    psb->getCollisionShape()->setMargin(collisionMargin);
-                    
-    m_data->m_dynamicsWorld->addSoftBody(psb);
 	serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
 #endif
 	return hasStatus;
@@ -8831,9 +8890,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processLoadURDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
-	case CMD_LOAD_BUNNY:
+	case CMD_LOAD_SOFT_BODY:
 		{
-			hasStatus = processLoadBunnyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
+			hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
 	case CMD_CREATE_SENSOR:
@@ -9079,7 +9138,7 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
 
 		m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
 	}
-#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
     for (  int i=0;i<m_data->m_dynamicsWorld->getSoftBodyArray().size();i++)
     {
         btSoftBody*	psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -46,7 +46,7 @@ protected:
 	bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
-	bool processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);

examples/SharedMemory/SharedMemoryCommands.h

@@ -439,11 +439,12 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
 };
 
-enum EnumLoadBunnyUpdateFlags
+enum EnumLoadSoftBodyUpdateFlags
 {
-    LOAD_BUNNY_UPDATE_SCALE=1,
+	LOAD_SOFT_BODY_FILE_NAME=1,
-    LOAD_BUNNY_UPDATE_MASS=2,
+    LOAD_SOFT_BODY_UPDATE_SCALE=2,
-    LOAD_BUNNY_UPDATE_COLLISION_MARGIN=4
+    LOAD_SOFT_BODY_UPDATE_MASS=4,
+    LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN=8
 };
 
 enum EnumSimParamInternalSimFlags
@@ -455,13 +456,19 @@ enum EnumSimParamInternalSimFlags
 ///Controlling a robot involves sending the desired state to its joint motor controllers.
 ///The control mode determines the state variables used for motor control.
 
-struct LoadBunnyArgs
+struct LoadSoftBodyArgs
 {
+	char m_fileName[MAX_FILENAME_LENGTH];
     double m_scale;
     double m_mass;
     double m_collisionMargin;
 };
 
+struct b3LoadSoftBodyResultArgs
+{
+	int m_objectUniqueId;
+};
+
 struct RequestActualStateArgs
 {
 	int m_bodyUniqueId;
@@ -991,7 +998,7 @@ struct SharedMemoryCommand
 		struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
 		struct UserDebugDrawArgs m_userDebugDrawArgs;
 		struct RequestRaycastIntersections m_requestRaycastIntersections;
-        struct LoadBunnyArgs m_loadBunnyArguments;
+        struct LoadSoftBodyArgs m_loadSoftBodyArguments;
 		struct VRCameraState m_vrCameraStateArguments;
 		struct StateLoggingRequest m_stateLoggingArguments;
         struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
@@ -1080,6 +1087,7 @@ struct SharedMemoryStatus
 		struct b3CustomCommandResultArgs m_customCommandResultArgs;
 		struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
 		struct b3StateSerializationArguments m_saveStateResultArgs;
+		struct b3LoadSoftBodyResultArgs m_loadSoftBodyResultArguments;
 		struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
 	};
 };

examples/SharedMemory/SharedMemoryPublic.h

@@ -23,7 +23,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_LOAD_BULLET,
 	CMD_SAVE_BULLET,
 	CMD_LOAD_MJCF,
-    CMD_LOAD_BUNNY,
+    CMD_LOAD_SOFT_BODY,
 	CMD_SEND_BULLET_DATA_STREAM,
 	CMD_CREATE_BOX_COLLISION_SHAPE,
 	CMD_CREATE_RIGID_BODY,

examples/pybullet/examples/load_soft_body.py

@@ -0,0 +1,20 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+
+p.setGravity(0,0,-10)
+planeId = p.loadURDF("plane.urdf")
+bunnyId = p.loadSoftBody("bunny.obj")
+
+useRealTimeSimulation = 1
+
+if (useRealTimeSimulation):
+	p.setRealTimeSimulation(1)
+
+while 1:
+	if (useRealTimeSimulation):
+		p.setGravity(0,0,-10)
+		sleep(0.01) # Time in seconds.
+	else:
+		p.stepSimulation()

examples/pybullet/pybullet.c

@@ -1367,6 +1367,67 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
 	return pylist;
 }
 
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+// Load a softbody from an obj file
+static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	int flags = 0;
+	
+	static char* kwlist[] = {"fileName", "scale", "mass", "collisionMargin", "physicsClientId", NULL};
+	
+	int bodyUniqueId= -1;
+	const char* fileName = "";
+	double scale = -1;
+	double mass = -1;
+	double collisionMargin = -1;
+	
+	b3PhysicsClientHandle sm = 0;
+	
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &fileName, &scale, &mass, &collisionMargin, &physicsClientId))
+	{
+		return NULL;
+	}
+	
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	
+	if (strlen(fileName))
+	{
+		b3SharedMemoryStatusHandle statusHandle;
+		int statusType;
+		b3SharedMemoryCommandHandle command =
+		b3LoadSoftBodyCommandInit(sm, fileName);
+
+		if (scale>0)
+		{
+			b3LoadSoftBodySetScale(command,scale);
+		}
+		if (mass>0)
+		{
+			b3LoadSoftBodySetMass(command,mass);
+		}
+		if (collisionMargin>0)
+		{
+			b3LoadSoftBodySetCollisionMargin(command,collisionMargin);
+		}
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType != CMD_CLIENT_COMMAND_COMPLETED)
+		{
+			PyErr_SetString(SpamError, "Cannot load soft body.");
+			return NULL;
+		}
+		bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
+	}
+	return PyLong_FromLong(bodyUniqueId);
+}
+#endif
+
 // Reset the simulation to remove all loaded objects
 static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds)
 {
@@ -8227,7 +8288,10 @@ static PyMethodDef SpamMethods[] = {
 
 	{"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS,
 	 "Load multibodies from an SDF file."},
-
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+	{"loadSoftBody", (PyCFunction)pybullet_loadSoftBody, METH_VARARGS | METH_KEYWORDS,
+	 "Load a softbody from an obj file."},
+#endif
 	{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
 	 "Load a world from a .bullet file."},