Load the deformable object through obj file. Modify the demo for two way coupling between multibody and softbody.

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/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
@@ -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.loadBunny(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/PhysicsServerCommandProcessor.cpp

@@ -50,7 +50,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"
@@ -169,6 +169,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;
 
@@ -191,6 +194,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();
@@ -1519,7 +1525,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;
@@ -2166,7 +2172,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();
@@ -2189,7 +2195,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);
@@ -2354,6 +2360,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++)
@@ -5650,7 +5664,7 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
 {
 	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;
@@ -5666,6 +5680,7 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
     {
         collisionMargin = clientCmd.m_loadBunnyArguments.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;
@@ -5673,7 +5688,7 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
     m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10);
     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);
                     
     btSoftBody::Material*	pm=psb->appendMaterial();
@@ -5690,6 +5705,60 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
     psb->getCollisionShape()->setMargin(collisionMargin);
                     
     m_data->m_dynamicsWorld->addSoftBody(psb);
+	*/
+	
+	{
+		char relativeFileName[1024];
+		char pathPrefix[1024];
+		pathPrefix[0] = 0;
+		if (b3ResourcePath::findResourcePath("bunny.obj", 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_loadBunnyResultArguments.m_objectUniqueId = bodyUniqueId;
+			}
+		}
+	}
+	
 	serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
 #endif
 	return hasStatus;
@@ -8758,7 +8827,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/SharedMemoryCommands.h

@@ -451,6 +451,11 @@ struct LoadBunnyArgs
     double m_collisionMargin;
 };
 
+struct b3LoadBunnyResultArgs
+{
+	int m_objectUniqueId;
+};
+
 struct RequestActualStateArgs
 {
 	int m_bodyUniqueId;
@@ -1068,6 +1073,7 @@ struct SharedMemoryStatus
 		struct b3CustomCommandResultArgs m_customCommandResultArgs;
 		struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
 		struct b3StateSerializationArguments m_saveStateResultArgs;
+		struct b3LoadBunnyResultArgs m_loadBunnyResultArguments;
 	};
 };