add gripper with deformable cloth demo

examples/ExampleBrowser/ExampleEntries.cpp

@@ -303,6 +303,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Rolling friction", "Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_ROLLING_FRICTION),
 		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, "Grasp Deformable Cloth", "Grasp experiment with deformable cloth", GripperGraspExampleCreateFunc, eGRASP_DEFORMABLE_CLOTH),
 		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),
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 		ExampleEntry(1, "Grasp Soft Body", "Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -373,6 +373,110 @@ public:
 				m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
 			}
 
+			if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
+			{
+				m_robotSim.resetSimulation(RESET_USE_DEFORMABLE_WORLD);
+			{
+				SliderParams slider("Vertical velocity", &sGripperVerticalVelocity);
+				slider.m_minVal = -2;
+				slider.m_maxVal = 2;
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+			}
+
+			{
+				SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
+				slider.m_minVal = -1;
+				slider.m_maxVal = 1;
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+			}
+
+			{
+				b3RobotSimulatorLoadFileResults results;
+				m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
+
+				if (results.m_uniqueObjectIds.size() == 1)
+				{
+					m_gripperIndex = results.m_uniqueObjectIds[0];
+					int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
+					b3Printf("numJoints = %d", numJoints);
+
+					for (int i = 0; i < numJoints; i++)
+					{
+						b3JointInfo jointInfo;
+						m_robotSim.getJointInfo(m_gripperIndex, i, &jointInfo);
+						b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName);
+					}
+
+					for (int i = 0; i < 8; i++)
+					{
+						b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+						controlArgs.m_maxTorqueValue = 0.0;
+						m_robotSim.setJointMotorControl(m_gripperIndex, i, controlArgs);
+					}
+				}
+			}
+			{
+				b3RobotSimulatorLoadUrdfFileArgs args;
+				args.m_startPosition.setValue(0, 0, -0.2);
+				args.m_startOrientation.setEulerZYX(0, 0, 0);
+				m_robotSim.loadURDF("plane.urdf", args);
+			}
+			m_robotSim.setGravity(btVector3(0, 0, -10));
+
+			m_robotSim.setGravity(btVector3(0, 0, -10));
+			b3RobotSimulatorLoadDeformableBodyArgs args(1, .01, 0.02);
+			args.m_springElasticStiffness = 1;
+			args.m_springDampingStiffness = .001;
+			args.m_springBendingStiffness = .2;
+			args.m_frictionCoeff = 2;
+			args.m_useSelfCollision = false;
+			args.m_useBendingSprings = true;
+			args.m_startPosition.setValue(0, 0, 0);
+			args.m_startOrientation.setValue(0, 0, 1, 1);
+			m_robotSim.loadDeformableBody("flat_napkin.obj", args);
+
+			b3JointInfo revoluteJoint1;
+			revoluteJoint1.m_parentFrame[0] = -0.055;
+			revoluteJoint1.m_parentFrame[1] = 0;
+			revoluteJoint1.m_parentFrame[2] = 0.02;
+			revoluteJoint1.m_parentFrame[3] = 0;
+			revoluteJoint1.m_parentFrame[4] = 0;
+			revoluteJoint1.m_parentFrame[5] = 0;
+			revoluteJoint1.m_parentFrame[6] = 1.0;
+			revoluteJoint1.m_childFrame[0] = 0;
+			revoluteJoint1.m_childFrame[1] = 0;
+			revoluteJoint1.m_childFrame[2] = 0;
+			revoluteJoint1.m_childFrame[3] = 0;
+			revoluteJoint1.m_childFrame[4] = 0;
+			revoluteJoint1.m_childFrame[5] = 0;
+			revoluteJoint1.m_childFrame[6] = 1.0;
+			revoluteJoint1.m_jointAxis[0] = 1.0;
+			revoluteJoint1.m_jointAxis[1] = 0.0;
+			revoluteJoint1.m_jointAxis[2] = 0.0;
+			revoluteJoint1.m_jointType = ePoint2PointType;
+			b3JointInfo revoluteJoint2;
+			revoluteJoint2.m_parentFrame[0] = 0.055;
+			revoluteJoint2.m_parentFrame[1] = 0;
+			revoluteJoint2.m_parentFrame[2] = 0.02;
+			revoluteJoint2.m_parentFrame[3] = 0;
+			revoluteJoint2.m_parentFrame[4] = 0;
+			revoluteJoint2.m_parentFrame[5] = 0;
+			revoluteJoint2.m_parentFrame[6] = 1.0;
+			revoluteJoint2.m_childFrame[0] = 0;
+			revoluteJoint2.m_childFrame[1] = 0;
+			revoluteJoint2.m_childFrame[2] = 0;
+			revoluteJoint2.m_childFrame[3] = 0;
+			revoluteJoint2.m_childFrame[4] = 0;
+			revoluteJoint2.m_childFrame[5] = 0;
+			revoluteJoint2.m_childFrame[6] = 1.0;
+			revoluteJoint2.m_jointAxis[0] = 1.0;
+			revoluteJoint2.m_jointAxis[1] = 0.0;
+			revoluteJoint2.m_jointAxis[2] = 0.0;
+			revoluteJoint2.m_jointType = ePoint2PointType;
+			m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
+			m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
+        }
+
 		if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
 		{
 			{
@@ -463,6 +567,21 @@ public:
 			}
 		}
         
+		if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
+		{
+			int fingerJointIndices[2] = {0, 1};
+			double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
+			double maxTorqueValues[2] = {250.0, 50.0};
+			for (int i = 0; i < 2; i++)
+			{
+				b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+				controlArgs.m_targetVelocity = fingerTargetVelocities[i];
+				controlArgs.m_maxTorqueValue = maxTorqueValues[i];
+				controlArgs.m_kd = 1.;
+				m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
+			}
+		}
+
 		if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
 		{
 			float dt = deltaTime;

examples/RoboticsLearning/GripperGraspExample.h

@@ -23,6 +23,7 @@ enum GripperGraspExampleOptions
 	eONE_MOTOR_GRASP = 4,
 	eGRASP_SOFT_BODY = 8,
 	eSOFTBODY_MULTIBODY_COUPLING = 16,
+	eGRASP_DEFORMABLE_CLOTH = 32,
 };
 
 class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -89,6 +89,19 @@ void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
 		m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
 }
 
+void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryStatusHandle statusHandle;
+	b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle);
+	b3InitResetSimulationSetFlags(command, flag);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+}
+
 bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
 {
 	if (!isConnected())
@@ -1151,6 +1164,29 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 }
 
+void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
+	b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
+	b3LoadSoftBodySetScale(command, args.m_scale);
+	b3LoadSoftBodySetMass(command, args.m_mass);
+	b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
+	b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
+	b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
+	b3LoadSoftBodyUseSelfCollision(command, args.m_useSelfCollision);
+	b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
+	b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
+	b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+}
+
 void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
 {
 	mouseEventsData->m_numMouseEvents = 0;

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -89,6 +89,55 @@ struct b3RobotSimulatorLoadSoftBodyArgs
 	}
 };
 
+
+struct b3RobotSimulatorLoadDeformableBodyArgs
+{
+	btVector3 m_startPosition;
+	btQuaternion m_startOrientation;
+	double m_scale;
+	double m_mass;
+	double m_collisionMargin;
+	double m_springElasticStiffness;
+	double m_springDampingStiffness;
+	double m_springBendingStiffness;
+	double m_NeoHookeanMu;
+	double m_NeoHookeanLambda;
+	double m_NeoHookeanDamping;
+	bool m_useSelfCollision;
+	bool m_useFaceContact;
+	bool m_useBendingSprings;
+	double m_frictionCoeff;
+
+	b3RobotSimulatorLoadDeformableBodyArgs(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)
+	{
+	}
+
+	b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
+	{
+		b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
+	}
+
+	b3RobotSimulatorLoadDeformableBodyArgs()
+	{
+		b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
+	}
+
+	b3RobotSimulatorLoadDeformableBodyArgs(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;
@@ -535,6 +584,8 @@ public:
 
 	void resetSimulation();
     
+	void resetSimulation(int flag);
+
 	btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw);
 	btVector3 getEulerFromQuaternion(const btQuaternion &quat);
 
@@ -686,6 +737,8 @@ public:
 
 	void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);
     
+	void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args);
+
 	virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
 	virtual struct GUIHelperInterface *getGuiHelper();
 

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -22,8 +22,32 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
     {
         ///this is a special step to resolve penetrations (just for contacts)
         solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+//        
+//        int maxMotorIterations = 150;
+//        for (int iteration = 0; iteration < maxMotorIterations; ++iteration)
+//        {
+//            btScalar motorResidual = 0;
+//            for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
+//            {
+//                int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
+//                
+//                btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
+//                
+//                btScalar residual = resolveSingleConstraintRowGeneric(constraint);
+//                motorResidual = btMax(motorResidual, residual * residual);
+//                if (constraint.m_multiBodyA)
+//                    constraint.m_multiBodyA->setPosUpdated(false);
+//                if (constraint.m_multiBodyB)
+//                    constraint.m_multiBodyB->setPosUpdated(false);
+//            }
+//            if (motorResidual < infoGlobal.m_leastSquaresResidualThreshold)
+//            {
+//                break;
+//            }
+//        }
         
         int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+        maxIterations = 500;
         for (int iteration = 0; iteration < maxIterations; iteration++)
         {
             // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body