Add API to set bunny properties. Add example to show coupling between softbody and multibody.

examples/ExampleBrowser/ExampleEntries.cpp

@@ -269,6 +269,7 @@ static ExampleEntry gDefaultExamples[]=
     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),
     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),
 
 
 #ifdef ENABLE_LUA

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -408,7 +408,7 @@ public:
                 
             }
             m_robotSim.setGravity(b3MakeVector3(0,0,-10));
-            m_robotSim.loadBunny();
+            m_robotSim.loadBunny(0.1,0.1,0.02);
             
             b3JointInfo revoluteJoint1;
             revoluteJoint1.m_parentFrame[0] = -0.007;
@@ -571,6 +571,46 @@ public:
             m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint7);
             m_robotSim.createJoint(0, 3, 0, 5, &revoluteJoint8);
         }
+        
+        if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0)
+        {
+            {
+                b3RobotSimLoadFileArgs args("");
+                args.m_fileName = "kuka_iiwa/model_free_base.urdf";
+                args.m_startPosition.setValue(0,1.0,2.0);
+                args.m_startOrientation.setEulerZYX(0,0,1.57);
+                args.m_forceOverrideFixedBase = false;
+                args.m_useMultiBody = true;
+                b3RobotSimLoadFileResults results;
+                m_robotSim.loadFile(args,results);
+                
+                int kukaId = results.m_uniqueObjectIds[0];
+                int numJoints = m_robotSim.getNumJoints(kukaId);
+                b3Printf("numJoints = %d",numJoints);
+                
+                for (int i=0;i<numJoints;i++)
+                {
+                    b3JointInfo jointInfo;
+                    m_robotSim.getJointInfo(kukaId,i,&jointInfo);
+                    b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+                    b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+                    controlArgs.m_maxTorqueValue = 0.0;
+                    m_robotSim.setJointMotorControl(kukaId,i,controlArgs);
+                }
+            }
+            {
+                b3RobotSimLoadFileArgs args("");
+                args.m_fileName = "plane.urdf";
+                args.m_startPosition.setValue(0,0,0);
+                args.m_startOrientation.setEulerZYX(0,0,0);
+                args.m_forceOverrideFixedBase = true;
+                args.m_useMultiBody = true;
+                b3RobotSimLoadFileResults results;
+                m_robotSim.loadFile(args,results);
+            }
+            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+            m_robotSim.loadBunny(0.5,10.0,0.1);
+        }
     }
     virtual void    exitPhysics()
     {

examples/RoboticsLearning/GripperGraspExample.h

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

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -1000,8 +1000,11 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP
     }
 }
 
-void b3RobotSimAPI::loadBunny()
+void b3RobotSimAPI::loadBunny(double scale, double mass, double collisionMargin)
 {
     b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient);
+    b3LoadBunnySetScale(command, scale);
+    b3LoadBunnySetMass(command, mass);
+    b3LoadBunnySetCollisionMargin(command, collisionMargin);
     b3SubmitClientCommand(m_data->m_physicsClient, command);
-}
+}

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -165,7 +165,7 @@ public:
     
     void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
     
-    void loadBunny();
+    void loadBunny(double scale, double mass, double collisionMargin);
 };
 
 #endif //B3_ROBOT_SIM_API_H

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -89,6 +89,33 @@ b3SharedMemoryCommandHandle	b3LoadBunnyCommandInit(b3PhysicsClientHandle physCli
     return (b3SharedMemoryCommandHandle) command;
 }
 
+int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command->m_type == CMD_LOAD_BUNNY);
+    command->m_loadBunnyArguments.m_scale = scale;
+    command->m_updateFlags |= LOAD_BUNNY_UPDATE_SCALE;
+    return 0;
+}
+
+int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command->m_type == CMD_LOAD_BUNNY);
+    command->m_loadBunnyArguments.m_mass = mass;
+    command->m_updateFlags |= LOAD_BUNNY_UPDATE_MASS;
+    return 0;
+}
+
+int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command->m_type == CMD_LOAD_BUNNY);
+    command->m_loadBunnyArguments.m_collisionMargin = collisionMargin;
+    command->m_updateFlags |= LOAD_BUNNY_UPDATE_COLLISION_MARGIN;
+    return 0;
+}
+
 int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1580,4 +1607,4 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
 	}
 
 	return true;
-}
+}

examples/SharedMemory/PhysicsClientC_API.h

@@ -229,6 +229,9 @@ void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUni
 void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
     
 b3SharedMemoryCommandHandle	b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
+int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
+int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
+int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
 
 #ifdef __cplusplus
 }

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1629,6 +1629,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                 case CMD_LOAD_BUNNY:
                 {
 #ifdef USE_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)
+                    {
+                        scale = clientCmd.m_loadBunnyArguments.m_scale;
+                    }
+                    if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS)
+                    {
+                        mass = clientCmd.m_loadBunnyArguments.m_mass;
+                    }
+                    if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN)
+                    {
+                        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;
@@ -1648,9 +1663,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     psb->randomizeConstraints();
                     psb->rotate(btQuaternion(0.70711,0,0,0.70711));
                     psb->translate(btVector3(0,0,1.0));
-                    psb->scale(btVector3(0.1,0.1,0.1));
+                    psb->scale(btVector3(scale,scale,scale));
-                    psb->setTotalMass(0.1,true);
+                    psb->setTotalMass(mass,true);
-                    psb->getCollisionShape()->setMargin(0.02);
+                    psb->getCollisionShape()->setMargin(collisionMargin);
                     
                     m_data->m_dynamicsWorld->addSoftBody(psb);
 #endif

examples/SharedMemory/SharedMemoryCommands.h

@@ -255,6 +255,13 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64
 };
 
+enum EnumLoadBunnyUpdateFlags
+{
+    LOAD_BUNNY_UPDATE_SCALE=1,
+    LOAD_BUNNY_UPDATE_MASS=2,
+    LOAD_BUNNY_UPDATE_COLLISION_MARGIN=4
+};
+
 enum EnumSimParamInternalSimFlags
 {
 	SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS=1,
@@ -274,6 +281,13 @@ struct SendPhysicsSimulationParameters
 	double m_defaultContactERP;
 };
 
+struct LoadBunnyArgs
+{
+    double m_scale;
+    double m_mass;
+    double m_collisionMargin;
+};
+
 struct RequestActualStateArgs
 {
 	int m_bodyUniqueId;
@@ -501,6 +515,7 @@ struct SharedMemoryCommand
         struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
         struct LoadTextureArgs m_loadTextureArguments;
 		struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
+        struct LoadBunnyArgs m_loadBunnyArguments;
     };
 };
 

src/BulletSoftBody/btSoftBody.cpp

@@ -18,6 +18,8 @@ subject to the following restrictions:
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "btSoftBodyData.h"
 #include "LinearMath/btSerializer.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
 
 //