Add grasp box example and fixjoint boxes example.

tmp

gripper is working?

fix joint example

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,84 @@ 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);
+			b3RobotSimulatorLoadUrdfFileArgs args;
-			args.m_startPosition.setValue(0, i * 0.05, 1);
+			b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
-			cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args);
 
-			b3JointInfo jointInfo;
+			for (int i = 0; i < numCubes; i++)
-
-			jointInfo.m_parentFrame[1] = -0.025;
-			jointInfo.m_childFrame[1] = 0.025;
-			jointInfo.m_jointType = eFixedType;
-			// jointInfo.m_jointType = ePoint2PointType;
-			// jointInfo.m_jointType =	ePrismaticType;
-
-			if (i > 0)
 			{
-				m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo);
+				args.m_forceOverrideFixedBase = (i == 0);
-			}
+				args.m_startPosition.setValue(0, i * 0.05, 1);
+				cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args);
 
-			m_robotSim.loadURDF("plane.urdf");
+				b3RobotJointInfo 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.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 * 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,12 +157,12 @@ public:
 
 	virtual void resetCamera()
 	{
-		// float dist = 1;
+		float dist = 1.2;
-		// float pitch = -20;
+		float pitch = -20;
-		// float yaw = -30;
+		float yaw = 90;
-		// float targetPos[3] = {0, 0.2, 0.5};
+		float targetPos[3] = {.5, 0.6, 0.5};
 
-		// m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
 	}
 };
 

examples/BulletRobotics/GraspBox.cpp

@@ -0,0 +1,186 @@
+
+#include "GraspBox.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"
+
+static btScalar x = 0.401f;
+static btScalar y = 0.025f;
+static btScalar z = 0.44f;
+
+class GraspBox : public CommonExampleInterface
+{
+	GUIHelperInterface* m_guiHelper;
+	b3RobotSimulatorClientAPI m_robotSim;
+	int m_options;
+
+	int m_armIndex;
+	int boxId;
+
+public:
+	GraspBox(GUIHelperInterface* helper, int options)
+		: m_guiHelper(helper),
+		  m_options(options)
+	{
+	}
+
+	virtual ~GraspBox()
+	{
+	}
+
+	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);
+
+		double cubePos[3] = {0.41, 0, 0.5};
+		double cubeHalfLength = 0.025;
+
+		{
+			SliderParams slider("position x", &x);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 1;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+		}
+
+		{
+			SliderParams slider("position y", &y);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 1;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+		}
+		{
+			SliderParams slider("position z", &z);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 1;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+		}
+
+		{
+			b3RobotSimulatorLoadFileResults results;
+			m_robotSim.loadSDF("kuka_iiwa/kuka_with_gripper.sdf", results);
+			if (results.m_uniqueObjectIds.size() == 1)
+			{
+				m_armIndex = results.m_uniqueObjectIds[0];
+				int numJoints = m_robotSim.getNumJoints(m_armIndex);
+				b3Printf("numJoints = %d", numJoints);
+
+				for (int i = 0; i < numJoints; i++)
+				{
+					b3RobotJointInfo jointInfo;
+					m_robotSim.getJointInfo(m_armIndex, i, &jointInfo);
+					b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName);
+				}
+
+				//adjust arm position
+				m_robotSim.resetJointState(m_armIndex, 3, -SIMD_HALF_PI);
+				m_robotSim.resetJointState(m_armIndex, 5, SIMD_HALF_PI);
+
+				// adjust gripper position
+				m_robotSim.resetJointState(m_armIndex, 8, -0.12);
+				m_robotSim.resetJointState(m_armIndex, 11, 0.12);
+
+				m_robotSim.resetJointState(m_armIndex, 10, -0.12);
+				m_robotSim.resetJointState(m_armIndex, 13, 0.12);
+
+				// {
+				//   b3RobotJointInfo jointInfo;
+				//   m_robotSim.getJointInfo(m_armIndex, 8, &jointInfo);
+				//   jointInfo.m_jointType = eGearType;
+				//   m_robotSim.createConstraint(m_armIndex, , cubeIds[i - 1], -1, &jointInfo);
+				// }
+				// for(int i = 0; i<14; i++){
+				//   b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+				//   controlArgs.m_maxTorqueValue = 0.0;
+				//   m_robotSim.setJointMotorControl(m_armIndex, i, controlArgs);
+				// }
+			}
+		}
+
+		{
+			b3RobotSimulatorLoadUrdfFileArgs args;
+			args.m_startPosition.setValue(x, y, z);
+			boxId = m_robotSim.loadURDF("cube_small.urdf", args);
+		}
+		m_robotSim.loadURDF("plane.urdf");
+		m_robotSim.setGravity(btVector3(0, 0, -10));
+	}
+
+	virtual void exitPhysics()
+	{
+		m_robotSim.disconnect();
+	}
+
+	virtual void stepSimulation(float deltaTime)
+	{
+		btScalar sGripperVerticalVelocity = 0.f;
+		btScalar sGripperClosingTargetVelocity = -0.7f;
+
+		int fingerJointIndices[4] = {8, 10, 11, 13};
+		double fingerTargetVelocities[4] = {0.4, 0.4, -0.4, -0.4};
+
+		double maxTorqueValues[4] = {20.0, 20.0, 20.0, 20.0};
+		for (int i = 0; i < 4; i++)
+		{
+			b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+			controlArgs.m_targetVelocity = fingerTargetVelocities[i];
+			controlArgs.m_maxTorqueValue = maxTorqueValues[i];
+			controlArgs.m_kd = 1.;
+			m_robotSim.setJointMotorControl(m_armIndex, fingerJointIndices[i], controlArgs);
+		}
+
+		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 = 0.8;
+		float pitch = 0;
+		float yaw = 0;
+		float targetPos[3] = {.4, 0.6, 0.45};
+
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options)
+{
+	return new GraspBox(options.m_guiHelper, options.m_option);
+}

examples/BulletRobotics/GraspBox.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 GRASP_BOX_EXAMPLE_H
+#define GRASP_BOX_EXAMPLE_H
+
+class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //GRASP_BOX_EXAMPLE_H

examples/BulletRobotics/JointLimit.cpp

@@ -51,20 +51,17 @@ public:
 		b3RobotSimulatorSetPhysicsEngineParameters physicsArgs;
 		physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG;
 
-		b3Printf(m_robotSim.getSolvername());
-
 		physicsArgs.m_defaultGlobalCFM = 1e-6;
 
-                m_robotSim.setNumSolverIterations(10);
+		m_robotSim.setNumSolverIterations(10);
-
 
 		b3RobotSimulatorLoadUrdfFileArgs loadArgs;
 		int humanoid = m_robotSim.loadURDF("test_joints_MB.urdf", loadArgs);
 
-                b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
+		b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
-                dynamicsArgs.m_linearDamping = 0;
+		dynamicsArgs.m_linearDamping = 0;
-                dynamicsArgs.m_angularDamping = 0;
+		dynamicsArgs.m_angularDamping = 0;
-                m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs);
+		m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs);
 
 		m_robotSim.setGravity(btVector3(0, 0, -10));
 	}

examples/ExampleBrowser/CMakeLists.txt

@@ -148,6 +148,8 @@ SET(BulletExampleBrowser_SRCS
 
         ../BulletRobotics/BoxStack.cpp
         ../BulletRobotics/JointLimit.cpp
+        ../BulletRobotics/GraspBox.cpp
+        ../BulletRobotics/FixJointBoxes.cpp
 
 	../TinyRenderer/geometry.cpp
 	../TinyRenderer/model.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -6,6 +6,8 @@
 #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"
 #include "../RenderingExamples/RaytracerSetup.h"
@@ -132,9 +134,12 @@ static ExampleEntry gDefaultExamples[] =
 
 		ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc),
 
+
 		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"),
 		ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),