add set starting position and orientation to softbody

examples/ExampleBrowser/CMakeLists.txt

@@ -94,6 +94,9 @@ LINK_LIBRARIES(
         BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
 )
 
+
+add_definitions(-DINCLUDE_CLOTH_DEMOS)
+
 IF (WIN32)
 	INCLUDE_DIRECTORIES(
 		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/glad
@@ -346,6 +349,8 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/InvertedPendulumPDControl.cpp
   ../MultiBody/InvertedPendulumPDControl.h
  ../MultiBody/MultiBodyConstraintFeedback.cpp
+  ../SoftDemo/SoftDemo.cpp
+  ../SoftDemo/SoftDemo.h
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -326,7 +326,10 @@ public:
 				m_robotSim.loadURDF("plane.urdf", args);
 			}
 			m_robotSim.setGravity(btVector3(0, 0, -10));
-			m_robotSim.loadSoftBody("bunny.obj", 0.1, 0.1, 0.02);
+                        b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
+                        args.m_startPosition.setValue(0, 0, 5);
+                        args.m_startOrientation.setValue(1, 0, 0, 1);
+			m_robotSim.loadSoftBody("bunny.obj", args);
 
 			b3JointInfo revoluteJoint1;
 			revoluteJoint1.m_parentFrame[0] = -0.055;
@@ -402,7 +405,8 @@ public:
 				m_robotSim.loadURDF("plane.urdf", args);
 			}
 			m_robotSim.setGravity(btVector3(0, 0, -10));
-			m_robotSim.loadSoftBody("bunny.obj", 0.3, 10.0, 0.1);
+                        b3RobotSimulatorLoadSoftBodyArgs args(0.3, 10, 0.1);
+			m_robotSim.loadSoftBody("bunny.obj", args);
 		}
 	}
 	virtual void exitPhysics()

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -315,7 +315,7 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle c
 }
 
 
-B3_SHARED_API int b3LoadSoftbodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
+B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
 	b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
@@ -326,7 +326,7 @@ B3_SHARED_API int b3LoadSoftbodySetStartPosition(b3SharedMemoryCommandHandle com
 	return 0;
 }
 
-B3_SHARED_API int b3LoadSoftbodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
+B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
 	b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);

examples/SharedMemory/PhysicsClientC_API.h

@@ -614,8 +614,8 @@ extern "C"
 	B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
 	B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
 	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 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 b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
 
 
 	B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -7376,6 +7376,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 				psb->generateBendingConstraints(2, pm);
 				psb->m_cfg.piterations = 20;
 				psb->m_cfg.kDF = 0.5;
+                                //turn on softbody vs softbody collision
+                                psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
 				psb->randomizeConstraints();
 				psb->rotate(initialOrn);
 				psb->translate(initialPos);

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -1134,7 +1134,7 @@ void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string&
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
 }
 
-void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
+void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, const struct b3RobotSimulatorLoadSoftBodyArgs& args)
 {
 	if (!isConnected())
 	{
@@ -1143,9 +1143,11 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
 	}
 
 	b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-	b3LoadSoftBodySetScale(command, scale);
+        b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
-	b3LoadSoftBodySetMass(command, mass);
+	b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
-	b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
+	b3LoadSoftBodySetScale(command, args.m_scale);
+	b3LoadSoftBodySetMass(command, args.m_mass);
+	b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 }
 

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -52,6 +52,47 @@ struct b3RobotSimulatorLoadSdfFileArgs
 	}
 };
 
+struct b3RobotSimulatorLoadSoftBodyArgs
+{
+	btVector3 m_startPosition;
+	btQuaternion m_startOrientation;
+        double m_scale;
+        double m_mass;
+        double m_collisionMargin;
+
+        b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double& scale, const double& mass, const double& collisionMargin )
+		: m_startPosition(startPos),
+		  m_startOrientation(startOrn),
+		  m_scale(scale),
+		  m_mass(mass),
+                  m_collisionMargin(collisionMargin)
+	{
+	}
+
+
+        b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
+	{
+          b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
+	}
+
+
+	b3RobotSimulatorLoadSoftBodyArgs()
+	{
+          b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
+	}
+
+
+        b3RobotSimulatorLoadSoftBodyArgs(double scale, double mass, double collisionMargin)
+		: m_startPosition(btVector3(0, 0, 0)),
+		  m_startOrientation(btQuaternion(0, 0, 0, 1)),
+		  m_scale(scale),
+		  m_mass(mass),
+                  m_collisionMargin(collisionMargin)
+	{
+	}
+};
+
+
 struct b3RobotSimulatorLoadFileResults
 {
 	btAlignedObjectArray<int> m_uniqueObjectIds;
@@ -647,7 +688,7 @@ public:
 
 	int getConstraintUniqueId(int serialIndex);
 
-	void loadSoftBody(const std::string &fileName, double scale, double mass, double collisionMargin);
+        void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs& args);
 
 	virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
 	virtual struct GUIHelperInterface *getGuiHelper();

examples/pybullet/pybullet.c

@@ -1942,7 +1942,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
 	int physicsClientId = 0;
 	int flags = 0;
 
-	static char* kwlist[] = {"fileName", "scale", "mass", "collisionMargin", "physicsClientId", NULL};
+	static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", NULL};
 
 	int bodyUniqueId = -1;
 	const char* fileName = "";
@@ -1952,10 +1952,36 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
 
 	b3PhysicsClientHandle sm = 0;
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &fileName, &scale, &mass, &collisionMargin, &physicsClientId))
+	double startPos[3] = {0.0, 0.0, 0.0};
+	double startOrn[4] = {0.0, 0.0, 0.0, 1.0};
+
+
+	PyObject* basePosObj = 0;
+	PyObject* baseOrnObj = 0;
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId))
 	{
 		return NULL;
 	}
+	else
+	{
+		if (basePosObj)
+		{
+			if (!pybullet_internalSetVectord(basePosObj, startPos))
+			{
+				PyErr_SetString(SpamError, "Cannot convert basePosition.");
+				return NULL;
+			}
+		}
+		if (baseOrnObj)
+		{
+			if (!pybullet_internalSetVector4d(baseOrnObj, startOrn))
+			{
+				PyErr_SetString(SpamError, "Cannot convert baseOrientation.");
+				return NULL;
+			}
+		}
+	}
 
 	sm = getPhysicsClient(physicsClientId);
 	if (sm == 0)