Merge pull request #2145 from fuchuyuan/addExample

Add example

10 files changed, 473 insertions, 24 deletions

diff --git a/data/test_joints_MB.urdf b/data/test_joints_MB.urdf
new file mode 100644
index 000000000..b575d5bf5
--- /dev/null
+++ b/data/test_joints_MB.urdf
@@ -0,0 +1,87 @@
+<?xml version="1.0"?>
+<robot name="physics">
+<link name="DOF_ArmRotate">
+<visual>
+<geometry>
+<box size="0.42 0.11 0.22"/>
+</geometry>
+<origin rpy="0 0 0" xyz="0.21 0 0"/>
+<material name="blue">
+<color rgba="0 0 0.8 1"/>
+</material>
+</visual>
+<collision>
+<geometry>
+<box size="0.42 0.11 0.22"/>
+</geometry>
+<origin rpy="0 0 0" xyz="0.21 0 0"/>
+</collision>
+<inertial>
+<mass value="0"/>
+<origin rpy="0 0 0" xyz="0.21 0 0 "/>
+<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+</inertial>
+</link>
+
+<link name="DOF_Snodo1">
+<visual>
+<geometry>
+<box size="0.68 0.23 0.64"/>
+</geometry>
+<origin rpy="0 0 0" xyz="0.34 0 0.32"/>
+<material name="white">
+<color rgba="1 1 1 1"/>
+</material>
+</visual>
+<collision>
+<geometry>
+<box size="0.68 0.23 0.64"/>
+</geometry>
+<origin rpy="0 0 0" xyz="0.34 0 0.32"/>
+</collision>
+<inertial>
+<mass value="2"/>
+<origin rpy="0 0 0" xyz="0.34 0 0.32"/>
+<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+</inertial>
+</link>
+
+<joint name="Rotate_White_Joint" type="revolute">
+<axis xyz="0 1 0"/>
+<limit effort="1000.0" lower="-0.785" upper="0.785" velocity="0.5"/>
+<origin rpy="0 0 0" xyz="0.42 0.0 0.0"/>
+<parent link="DOF_ArmRotate"/>
+<child link="DOF_Snodo1"/>
+</joint>
+
+<link name="DOF_Snodo2">
+<visual>
+<geometry>
+<box size="0.42 0.12 0.20"/>
+</geometry>
+<origin rpy="0 0 0" xyz="0.21 0 0.10"/>
+<material name="red">
+<color rgba="1 0 0 1"/>
+</material>
+</visual>
+<collision>
+<geometry>
+<box size="0.42 0.12 0.20"/>
+</geometry>
+<origin rpy="0 0 0" xyz="0.21 0 0.10"/>
+</collision>
+<inertial>
+<mass value="1"/>
+<origin rpy="0 0 0" xyz="0.21 0 0.10"/>
+<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+</inertial>
+</link>
+
+<joint name="Rotate_Red_Joint" type="revolute">
+<axis xyz="0 1 0"/>
+<limit effort="1000.0" lower="-0.785" upper="0.785" velocity="0.5"/>
+<origin rpy="0 0 0" xyz="0.68 0.0 0.64"/>
+<parent link="DOF_Snodo1"/>
+<child link="DOF_Snodo2"/>
+</joint>
+</robot>
diff --git a/examples/BulletRobotics/BoxStack.cpp b/examples/BulletRobotics/BoxStack.cpp
new file mode 100644
index 000000000..df4056465
--- /dev/null
+++ b/examples/BulletRobotics/BoxStack.cpp
@@ -0,0 +1,119 @@
+
+#include "BoxStack.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include <string>
+
+#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
+#include "../Utils/b3Clock.h"
+
+class BoxStackExample : public CommonExampleInterface
+{
+	CommonGraphicsApp* m_app;
+	GUIHelperInterface* m_guiHelper;
+	b3RobotSimulatorClientAPI m_robotSim;
+	int m_options;
+
+public:
+	BoxStackExample(GUIHelperInterface* helper, int options)
+		: m_app(helper->getAppInterface()),
+		  m_guiHelper(helper),
+		  m_options(options)
+	{
+		m_app->setUpAxis(2);
+	}
+
+	virtual ~BoxStackExample()
+	{
+	}
+
+	virtual void physicsDebugDraw(int debugDrawMode)
+	{
+		m_robotSim.debugDraw(debugDrawMode);
+	}
+	virtual void initPhysics()
+	{
+		int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
+		m_robotSim.setGuiHelper(m_guiHelper);
+		bool connected = m_robotSim.connect(mode);
+
+		b3Printf("robotSim connected = %d", connected);
+
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0);
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0);
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0);
+
+		b3RobotSimulatorLoadUrdfFileArgs args;
+		b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
+		int massRatio = 4;
+		int mass = 1;
+		for (int i = 0; i < 8; i++)
+		{
+			args.m_startPosition.setValue(0, 0, i * .06);
+			int boxIdx = m_robotSim.loadURDF("cube_small.urdf", args);
+			dynamicsArgs.m_mass = mass;
+			m_robotSim.changeDynamics(boxIdx, -1, dynamicsArgs);
+			mass *= massRatio;
+		}
+
+		m_robotSim.loadURDF("plane.urdf");
+		m_robotSim.setGravity(btVector3(0, 0, -10));
+	}
+
+	virtual void exitPhysics()
+	{
+		m_robotSim.disconnect();
+	}
+	virtual void stepSimulation(float deltaTime)
+	{
+		m_robotSim.stepSimulation();
+	}
+	virtual void renderScene()
+	{
+		m_robotSim.renderScene();
+	}
+
+	virtual bool mouseMoveCallback(float x, float y)
+	{
+		return m_robotSim.mouseMoveCallback(x, y);
+	}
+	virtual bool mouseButtonCallback(int button, int state, float x, float y)
+	{
+		return m_robotSim.mouseButtonCallback(button, state, x, y);
+	}
+	virtual bool keyboardCallback(int key, int state)
+	{
+		return false;
+	}
+
+	virtual void resetCamera()
+	{
+		float dist = 1.5;
+		float pitch = -10;
+		float yaw = 18;
+		float targetPos[3] = {-0.2, 0.8, 0.3};
+
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+
+		if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
+		{
+			m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+			m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+			m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+			m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]);
+		}
+	}
+};
+
+class CommonExampleInterface* BoxStackExampleCreateFunc(struct CommonExampleOptions& options)
+{
+	return new BoxStackExample(options.m_guiHelper, options.m_option);
+}
diff --git a/examples/BulletRobotics/BoxStack.h b/examples/BulletRobotics/BoxStack.h
new file mode 100644
index 000000000..289b4b034
--- /dev/null
+++ b/examples/BulletRobotics/BoxStack.h
@@ -0,0 +1,21 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2016 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BOX_STACK_EXAMPLE_H
+#define BOX_STACK_EXAMPLE_H
+
+class CommonExampleInterface* BoxStackExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //BOX_STACK_EXAMPLE_H
diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp
index fa29d4f93..a5a89e329 100644
--- a/examples/BulletRobotics/FixJointBoxes.cpp
+++ b/examples/BulletRobotics/FixJointBoxes.cpp
@@ -14,16 +14,26 @@
 #include <vector>
 #include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
 
+static btScalar numSolverIterations = 1000;
+static btScalar solverId = 0;
+
 class FixJointBoxes : public CommonExampleInterface
 {
 	GUIHelperInterface* m_guiHelper;
 	b3RobotSimulatorClientAPI m_robotSim;
 	int m_options;
+	b3RobotSimulatorSetPhysicsEngineParameters physicsArgs;
+	int solver;
+
+	const size_t numCubes = 30;
+	std::vector<int> cubeIds;
 
 public:
 	FixJointBoxes(GUIHelperInterface* helper, int options)
 		: m_guiHelper(helper),
-		  m_options(options)
+		  m_options(options),
+		  cubeIds(numCubes, 0),
+		  solver(solverId)
 	{
 	}
 
@@ -47,39 +57,85 @@ public:
 		m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0);
 		m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0);
 
-		b3RobotSimulatorLoadUrdfFileArgs args;
-
-		size_t numCubes = 10;
-		std::vector<int> cubeIds(numCubes, 0);
-		for (int i = 0; i < numCubes; i++)
 		{
-			args.m_forceOverrideFixedBase = (i == 0);
-			args.m_startPosition.setValue(0, i * 0.05, 1);
-			cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args);
+			b3RobotSimulatorLoadUrdfFileArgs args;
+			b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
 
-			b3RobotJointInfo jointInfo;
+			for (int i = 0; i < numCubes; i++)
+			{
+				args.m_forceOverrideFixedBase = (i == 0);
+				args.m_startPosition.setValue(0, i * 0.05, 1);
+				cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args);
 
-			jointInfo.m_parentFrame[1] = -0.025;
-			jointInfo.m_childFrame[1] = 0.025;
-			jointInfo.m_jointType = eFixedType;
-			// jointInfo.m_jointType = ePoint2PointType;
-			// jointInfo.m_jointType =	ePrismaticType;
+				b3RobotJointInfo jointInfo;
 
-			if (i > 0)
-			{
-				m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo);
+				jointInfo.m_parentFrame[1] = -0.025;
+				jointInfo.m_childFrame[1] = 0.025;
+
+				if (i > 0)
+				{
+					m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo);
+                    m_robotSim.setCollisionFilterGroupMask(cubeIds[i], -1, 0, 0);
+				}
+
+				m_robotSim.loadURDF("plane.urdf");
 			}
+		}
 
-			m_robotSim.loadURDF("plane.urdf");
+		{
+			SliderParams slider("Direct solver", &solverId);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 1;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 		}
+		{
+			SliderParams slider("numSolverIterations", &numSolverIterations);
+			slider.m_minVal = 50;
+			slider.m_maxVal = 1e4;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+		}
+
+		physicsArgs.m_defaultGlobalCFM = 1e-6;
+		m_robotSim.setPhysicsEngineParameter(physicsArgs);
+
+		m_robotSim.setGravity(btVector3(0, 0, -10));
+		m_robotSim.setNumSolverIterations((int)numSolverIterations);
 	}
 
 	virtual void exitPhysics()
 	{
 		m_robotSim.disconnect();
 	}
+
+	void resetCubePosition()
+	{
+		for (int i = 0; i < numCubes; i++)
+		{
+                  btVector3 pos (0, i * (btScalar)0.05, 1);
+                  btQuaternion quar (0, 0, 0, 1);
+			m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar);
+		}
+	}
 	virtual void stepSimulation(float deltaTime)
 	{
+		int newSolver = (int)(solverId + 0.5);
+		if (newSolver != solver)
+		{
+			printf("Switching solver, new %d, old %d\n", newSolver, solver);
+			solver = newSolver;
+			resetCubePosition();
+			if (solver)
+			{
+				physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG;
+			}
+			else
+			{
+				physicsArgs.m_constraintSolverType = eConstraintSolverLCP_SI;
+			}
+
+			m_robotSim.setPhysicsEngineParameter(physicsArgs);
+		}
+		m_robotSim.setNumSolverIterations((int)numSolverIterations);
 		m_robotSim.stepSimulation();
 	}
 	virtual void renderScene()
@@ -102,11 +158,11 @@ public:
 
 	virtual void resetCamera()
 	{
-		 float dist = 1;
-		 float pitch = -20;
-		 float yaw = -30;
-		 float targetPos[3] = {0, 0.2, 0.5};
-		 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+		float dist = 1;
+		float pitch = -20;
+		float yaw = -30;
+		float targetPos[3] = {0, 0.2, 0.5};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
 	}
 };
 
diff --git a/examples/BulletRobotics/JointLimit.cpp b/examples/BulletRobotics/JointLimit.cpp
new file mode 100644
index 000000000..6a4d91eef
--- /dev/null
+++ b/examples/BulletRobotics/JointLimit.cpp
@@ -0,0 +1,109 @@
+
+#include "JointLimit.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <string>
+
+#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
+#include "../Utils/b3Clock.h"
+
+class JointLimit : public CommonExampleInterface
+{
+	GUIHelperInterface* m_guiHelper;
+	b3RobotSimulatorClientAPI m_robotSim;
+	int m_options;
+
+public:
+	JointLimit(GUIHelperInterface* helper, int options)
+		: m_guiHelper(helper),
+		  m_options(options)
+	{
+	}
+
+	virtual ~JointLimit()
+	{
+	}
+
+	virtual void physicsDebugDraw(int debugDrawMode)
+	{
+		m_robotSim.debugDraw(debugDrawMode);
+	}
+	virtual void initPhysics()
+	{
+		int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
+		m_robotSim.setGuiHelper(m_guiHelper);
+		bool connected = m_robotSim.connect(mode);
+
+		b3Printf("robotSim connected = %d", connected);
+
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0);
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0);
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0);
+
+		b3RobotSimulatorSetPhysicsEngineParameters physicsArgs;
+		physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG;
+
+		physicsArgs.m_defaultGlobalCFM = 1e-6;
+
+		m_robotSim.setNumSolverIterations(10);
+
+		b3RobotSimulatorLoadUrdfFileArgs loadArgs;
+		int humanoid = m_robotSim.loadURDF("test_joints_MB.urdf", loadArgs);
+
+		b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
+		dynamicsArgs.m_linearDamping = 0;
+		dynamicsArgs.m_angularDamping = 0;
+		m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs);
+
+		m_robotSim.setGravity(btVector3(0, 0, -10));
+	}
+
+	virtual void exitPhysics()
+	{
+		m_robotSim.disconnect();
+	}
+	virtual void stepSimulation(float deltaTime)
+	{
+		m_robotSim.stepSimulation();
+	}
+	virtual void renderScene()
+	{
+		m_robotSim.renderScene();
+	}
+
+	virtual bool mouseMoveCallback(float x, float y)
+	{
+		return m_robotSim.mouseMoveCallback(x, y);
+	}
+	virtual bool mouseButtonCallback(int button, int state, float x, float y)
+	{
+		return m_robotSim.mouseButtonCallback(button, state, x, y);
+	}
+	virtual bool keyboardCallback(int key, int state)
+	{
+		return false;
+	}
+
+	virtual void resetCamera()
+	{
+		float dist = 3;
+		float pitch = -10;
+		float yaw = 18;
+		float targetPos[3] = {0.6, 0.8, 0.3};
+
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+class CommonExampleInterface* JointLimitCreateFunc(struct CommonExampleOptions& options)
+{
+	return new JointLimit(options.m_guiHelper, options.m_option);
+}
diff --git a/examples/BulletRobotics/JointLimit.h b/examples/BulletRobotics/JointLimit.h
new file mode 100644
index 000000000..c8a125e53
--- /dev/null
+++ b/examples/BulletRobotics/JointLimit.h
@@ -0,0 +1,21 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2016 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef JOINT_LIMIT_EXAMPLE_H
+#define JOINT_LIMIT_EXAMPLE_H
+
+class CommonExampleInterface* JointLimitCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //JOINT_LIMIT_EXAMPLE_H
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 99dc6fd1d..0b76fdead 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -146,6 +146,11 @@ SET(ExtendedTutorialsSources
 SET(BulletExampleBrowser_SRCS
         ../BulletRobotics/FixJointBoxes.cpp
 
+        ../BulletRobotics/BoxStack.cpp
+        ../BulletRobotics/JointLimit.cpp
+        # ../BulletRobotics/GraspBox.cpp
+        ../BulletRobotics/FixJointBoxes.cpp
+
 	../TinyRenderer/geometry.cpp
 	../TinyRenderer/model.cpp
 	../TinyRenderer/tgaimage.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 059795646..15080e4dc 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -3,6 +3,10 @@
 #include "../BlockSolver/BlockSolverExample.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "EmptyExample.h"
+#include "../BulletRobotics/BoxStack.h"
+#include "../BulletRobotics/FixJointBoxes.h"
+#include "../BulletRobotics/JointLimit.h"
+// #include "../BulletRobotics/GraspBox.h"
 #include "../BulletRobotics/FixJointBoxes.h"
 #include "../RenderingExamples/RenderInstancingDemo.h"
 #include "../RenderingExamples/CoordinateSystemDemo.h"
@@ -132,6 +136,9 @@ static ExampleEntry gDefaultExamples[] =
 
 
 		ExampleEntry(0, "Bullet Robotics"),
+		ExampleEntry(1, "Box Stack", "Create a stack of boxes of large mass ratio.", BoxStackExampleCreateFunc),
+		ExampleEntry(1, "Joint Limit", "Create three objects joint together", JointLimitCreateFunc),
+		// ExampleEntry(1, "Grasp Box", "A robot arm of large mass tries to grasp a box of small mass", GraspBoxCreateFunc),
 		ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc),
 
 		ExampleEntry(0, "MultiBody"),
diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
index 284430847..9efed7f7e 100644
--- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
+++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
@@ -2417,3 +2417,25 @@ void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::stri
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 	}
 }
+
+void b3RobotSimulatorClientAPI_NoDirect::setCollisionFilterGroupMask(int bodyUniqueIdA, int linkIndexA, int collisionFilterGroup, int collisionFilterMask)
+{
+    int physicsClientId = 0;
+    b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+    if (sm == 0)
+    {
+        b3Warning("Not connected");
+        return;
+    }
+    
+    b3SharedMemoryCommandHandle commandHandle;
+    b3SharedMemoryStatusHandle statusHandle;
+    int statusType;
+    
+    commandHandle = b3CollisionFilterCommandInit(sm);
+    b3SetCollisionFilterGroupMask(commandHandle, bodyUniqueIdA, linkIndexA, collisionFilterGroup, collisionFilterMask);
+    
+    statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+    statusType = b3GetStatusType(statusHandle);
+}
+
diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
index cca09d974..297c7e203 100644
--- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
+++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
@@ -664,6 +664,8 @@ public:
 		return SHARED_MEMORY_MAGIC_NUMBER;
 	}
 	void setAdditionalSearchPath(const std::string &path);
+    
+    void setCollisionFilterGroupMask(int bodyUniqueIdA, int linkIndexA, int collisionFilterGroup, int collisionFilterMask);
 };
 
 #endif  //B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H