add GripperGraspExample, separate from R2D2GraspExample

examples/ExampleBrowser/CMakeLists.txt

@@ -210,6 +210,8 @@ SET(BulletExampleBrowser_SRCS
 	../RenderingExamples/TimeSeriesCanvas.h
 	../RenderingExamples/TimeSeriesFontData.cpp
 	../RenderingExamples/TimeSeriesFontData.h
+	../RoboticsLearning/GripperGraspExample.cpp
+	../RoboticsLearning/GripperGraspExample.h
 	../RoboticsLearning/b3RobotSimAPI.cpp
 	../RoboticsLearning/b3RobotSimAPI.h
 	../RoboticsLearning/R2D2GraspExample.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -46,6 +46,7 @@
 #include "../MultiThreading/MultiThreadingExample.h"
 #include "../InverseDynamics/InverseDynamicsExample.h"
 #include "../RoboticsLearning/R2D2GraspExample.h"
+#include "../RoboticsLearning/GripperGraspExample.h"
 #include "../InverseKinematics/InverseKinematicsExample.h"
 
 #ifdef ENABLE_LUA
@@ -260,7 +261,7 @@ static ExampleEntry gDefaultExamples[]=
 
 	ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
 	ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
-    ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_GRASP_CONTACT),
+    ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc),
 	
 
 

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -0,0 +1,231 @@
+
+#include "GripperGraspExample.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.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 <string>
+
+#include "b3RobotSimAPI.h"
+#include "../Utils/b3Clock.h"
+
+static btScalar sGripperVerticalVelocity = -0.2f;
+static btScalar sGripperClosingTargetVelocity = 0.5f;
+
+class GripperGraspExample : public CommonExampleInterface
+{
+    CommonGraphicsApp* m_app;
+	GUIHelperInterface* m_guiHelper;
+	b3RobotSimAPI m_robotSim;
+	int m_options;
+	int m_r2d2Index;
+    int m_gripperIndex;
+    
+    float m_x;
+    float m_y;
+    float m_z;
+	b3AlignedObjectArray<int> m_movingInstances;
+	enum
+	{
+		numCubesX = 20,
+		numCubesY = 20
+	};
+public:
+    
+    GripperGraspExample(GUIHelperInterface* helper, int options)
+    :m_app(helper->getAppInterface()),
+	m_guiHelper(helper),
+	m_options(options),
+	m_r2d2Index(-1),
+    m_gripperIndex(-1),
+    m_x(0),
+    m_y(0),
+	m_z(0)
+    {
+		m_app->setUpAxis(2);
+    }
+    virtual ~GripperGraspExample()
+    {
+        m_app->m_renderer->enableBlend(false);
+    }
+
+    
+    virtual void physicsDebugDraw(int debugDrawMode)
+    {
+        
+    }
+    virtual void    initPhysics()
+    {
+	bool connected = m_robotSim.connect(m_guiHelper);
+	b3Printf("robotSim connected = %d",connected);
+	
+        {
+            
+            {
+                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);
+            }
+            
+            
+            {
+                b3RobotSimLoadFileArgs args("");
+                args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf";
+                args.m_fileType = B3_SDF_FILE;
+                b3RobotSimLoadFileResults results;
+                
+                if (m_robotSim.loadFile(args, results) && 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);
+                    }
+                    
+                    /*
+                    int fingerJointIndices[2]={1,3};
+                    double fingerTargetPositions[2]={-0.04,0.04};
+                    for (int i=0;i<2;i++)
+                    {
+                        b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
+                        controlArgs.m_targetPosition = fingerTargetPositions[i];
+                        controlArgs.m_kp = 5.0;
+                        controlArgs.m_kd = 3.0;
+                        controlArgs.m_maxTorqueValue = 1.0;
+                        m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
+                    }
+                    */
+                    int fingerJointIndices[3]={0,1,3};
+                    double fingerTargetVelocities[3]={-0.2,.5,-.5};
+                    double maxTorqueValues[3]={40.0,50.0,50.0};
+                    for (int i=0;i<3;i++)
+                    {
+                        b3JointMotorArgs 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);
+                    }
+                }
+            }
+            {
+                b3RobotSimLoadFileArgs args("");
+                b3RobotSimLoadFileResults results;
+                args.m_fileName = "cube_small.urdf";
+                args.m_startPosition.setValue(0,0,.107);
+                args.m_startOrientation.setEulerZYX(0,0,0);
+                m_robotSim.loadFile(args,results);
+            }
+            if (1)
+            {
+                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;
+                b3RobotSimLoadFileResults results;
+                m_robotSim.loadFile(args,results);
+                
+            }
+            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+            
+        }
+
+	
+		
+    }
+    virtual void    exitPhysics()
+    {
+		m_robotSim.disconnect();
+    }
+    virtual void	stepSimulation(float deltaTime)
+	{
+        if ((m_gripperIndex>=0))
+        {
+            int fingerJointIndices[3]={0,1,3};
+            double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
+                ,-sGripperClosingTargetVelocity
+            };
+            double maxTorqueValues[3]={40.0,50.0,50.0};
+            for (int i=0;i<3;i++)
+            {
+                b3JointMotorArgs 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);
+            }
+        }
+	m_robotSim.stepSimulation();
+    }
+    virtual void	renderScene()
+    {
+		m_robotSim.renderScene();
+
+		//m_app->m_renderer->renderScene();
+    }
+
+	
+    virtual void	physicsDebugDraw()
+    {
+      	
+    }
+    virtual bool	mouseMoveCallback(float x,float y)
+    {
+		return false;   
+    }
+    virtual bool	mouseButtonCallback(int button, int state, float x, float y)
+    {
+        return false;   
+    }
+    virtual bool	keyboardCallback(int key, int state)
+    {
+        return false;   
+    }
+    
+	virtual void resetCamera()
+	{
+		float dist = 1.5;
+        float pitch = 12;
+        float yaw = -10;
+		float targetPos[3]={-0.2,0.8,0.3};
+		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*    GripperGraspExampleCreateFunc(struct CommonExampleOptions& options)
+{
+	return new GripperGraspExample(options.m_guiHelper, options.m_option);
+}
+
+
+

examples/RoboticsLearning/GripperGraspExample.h

@@ -0,0 +1,23 @@
+/*
+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 GRIPPER_GRASP_EXAMPLE_H
+#define GRIPPER_GRASP_EXAMPLE_H
+
+
+class CommonExampleInterface*    GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //GRIPPER_GRASP_EXAMPLE_H

examples/RoboticsLearning/R2D2GraspExample.cpp

@@ -9,15 +9,11 @@
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
 #include "../SharedMemory/PhysicsServerSharedMemory.h"
 #include "../SharedMemory/PhysicsClientC_API.h"
-#include "../CommonInterfaces/CommonParameterInterface.h"
 #include <string>
 
 #include "b3RobotSimAPI.h"
 #include "../Utils/b3Clock.h"
 
-static btScalar sGripperVerticalVelocity = -0.2f;
-static btScalar sGripperClosingTargetVelocity = 0.5f;
-
 ///quick demo showing the right-handed coordinate system and positive rotations around each axis
 class R2D2GraspExample : public CommonExampleInterface
 {
@@ -26,8 +22,7 @@ class R2D2GraspExample : public CommonExampleInterface
 	b3RobotSimAPI m_robotSim;
 	int m_options;
 	int m_r2d2Index;
-    int m_gripperIndex;
+	
-    
     float m_x;
     float m_y;
     float m_z;
@@ -44,7 +39,6 @@ public:
 	m_guiHelper(helper),
 	m_options(options),
 	m_r2d2Index(-1),
-    m_gripperIndex(-1),
     m_x(0),
     m_y(0),
 	m_z(0)
@@ -67,9 +61,36 @@ public:
 		b3Printf("robotSim connected = %d",connected);
 		
 		
-		if (m_options == eROBOTIC_LEARN_GRASP)
+		if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
 		{
-            
+			{
+				b3RobotSimLoadFileArgs args("");
+				args.m_fileName = "r2d2.urdf";
+				args.m_startPosition.setValue(0,0,.5);
+				b3RobotSimLoadFileResults results;
+				if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
+				{
+					int m_r2d2Index = results.m_uniqueObjectIds[0];
+					int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
+					b3Printf("numJoints = %d",numJoints);
+
+					for (int i=0;i<numJoints;i++)
+					{
+						b3JointInfo jointInfo;
+						m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
+						b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+					}
+					int wheelJointIndices[4]={2,3,6,7};
+					int wheelTargetVelocities[4]={-10,-10,-10,-10};
+					for (int i=0;i<4;i++)
+					{
+						b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+						controlArgs.m_targetVelocity = wheelTargetVelocities[i];
+						controlArgs.m_maxTorqueValue = 1e30;
+						m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
+					}
+				}
+			}
 			{
 				b3RobotSimLoadFileArgs args("");
 				args.m_fileName = "kiva_shelf/model.sdf";
@@ -88,154 +109,38 @@ public:
 				args.m_forceOverrideFixedBase = true;
 				b3RobotSimLoadFileResults results;
 				m_robotSim.loadFile(args,results);
-				m_robotSim.setGravity(b3MakeVector3(0,0,0));
+				m_robotSim.setGravity(b3MakeVector3(0,0,-10));
 			}
-            {
+	
-                b3RobotSimLoadFileArgs args("");
-                args.m_fileName = "r2d2.urdf";
-                args.m_startPosition.setValue(0,0,.5);
-                b3RobotSimLoadFileResults results;
-                if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
-                {
-                    int m_r2d2Index = results.m_uniqueObjectIds[0];
-                    int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
-                    b3Printf("numJoints = %d",numJoints);
-                    
-                    for (int i=0;i<numJoints;i++)
-                    {
-                        b3JointInfo jointInfo;
-                        m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
-                        b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
-                    }
-                    int wheelJointIndices[4]={2,3,6,7};
-                    int wheelTargetVelocities[4]={-10,-10,-10,-10};
-                    for (int i=0;i<4;i++)
-                    {
-                        b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
-                        controlArgs.m_targetVelocity = wheelTargetVelocities[i];
-                        controlArgs.m_maxTorqueValue = 1e30;
-                        m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
-                    }
-                }
-            }
 		}
 
-        if (m_options == eROBOTIC_LEARN_COMPLIANT_CONTACT)
+		if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
-        {
+		{
-            b3RobotSimLoadFileArgs args("");
+			b3RobotSimLoadFileArgs args("");
-            b3RobotSimLoadFileResults results;
+			b3RobotSimLoadFileResults results;
-            {
+			{
-                args.m_fileName = "cube.urdf";
+				args.m_fileName = "cube.urdf";
-                args.m_startPosition.setValue(0,0,2.5);
+				args.m_startPosition.setValue(0,0,2.5);
-                args.m_startOrientation.setEulerZYX(0,0.2,0);
+				args.m_startOrientation.setEulerZYX(0,0.2,0);
-                m_robotSim.loadFile(args,results);
+				m_robotSim.loadFile(args,results);
-            }
+			}
-            {
+			{
-                args.m_fileName = "cube_no_friction.urdf";
+				args.m_fileName = "cube_no_friction.urdf";
-                args.m_startPosition.setValue(0,2,2.5);
+				args.m_startPosition.setValue(0,2,2.5);
-                args.m_startOrientation.setEulerZYX(0,0.2,0);
+				args.m_startOrientation.setEulerZYX(0,0.2,0);
-                m_robotSim.loadFile(args,results);
+				m_robotSim.loadFile(args,results);
-            }
+			}
-            {
+			{
-                b3RobotSimLoadFileArgs args("");
+				b3RobotSimLoadFileArgs args("");
-                args.m_fileName = "plane.urdf";
+				args.m_fileName = "plane.urdf";
-                args.m_startPosition.setValue(0,0,0);
+				args.m_startPosition.setValue(0,0,0);
-                args.m_startOrientation.setEulerZYX(0,0.2,0);
+				args.m_startOrientation.setEulerZYX(0,0.2,0);
-                args.m_forceOverrideFixedBase = true;
+				args.m_forceOverrideFixedBase = true;
-                b3RobotSimLoadFileResults results;
+				b3RobotSimLoadFileResults results;
-                m_robotSim.loadFile(args,results);
+				m_robotSim.loadFile(args,results);
-                m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+				m_robotSim.setGravity(b3MakeVector3(0,0,-10));
-            }
+			}
-        }
+		}
-        
-        if (m_options == eROBOTIC_LEARN_GRASP_CONTACT)
-        {
-            
-            {
-                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);
-            }
-            
-            
-            {
-                b3RobotSimLoadFileArgs args("");
-                args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf";
-                args.m_fileType = B3_SDF_FILE;
-                b3RobotSimLoadFileResults results;
-                
-                if (m_robotSim.loadFile(args, results) && 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);
-                    }
-                    
-                    /*
-                    int fingerJointIndices[2]={1,3};
-                    double fingerTargetPositions[2]={-0.04,0.04};
-                    for (int i=0;i<2;i++)
-                    {
-                        b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
-                        controlArgs.m_targetPosition = fingerTargetPositions[i];
-                        controlArgs.m_kp = 5.0;
-                        controlArgs.m_kd = 3.0;
-                        controlArgs.m_maxTorqueValue = 1.0;
-                        m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
-                    }
-                    */
-                    int fingerJointIndices[3]={0,1,3};
-                    double fingerTargetVelocities[3]={-0.2,.5,-.5};
-                    double maxTorqueValues[3]={40.0,50.0,50.0};
-                    for (int i=0;i<3;i++)
-                    {
-                        b3JointMotorArgs 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);
-                    }
-                }
-            }
-            {
-                b3RobotSimLoadFileArgs args("");
-                b3RobotSimLoadFileResults results;
-                args.m_fileName = "cube_small.urdf";
-                args.m_startPosition.setValue(0,0,.107);
-                args.m_startOrientation.setEulerZYX(0,0,0);
-                m_robotSim.loadFile(args,results);
-            }
-            if (1)
-            {
-                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;
-                b3RobotSimLoadFileResults results;
-                m_robotSim.loadFile(args,results);
-                
-            }
-            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
-            
-        }
-
 	
 		
     }
@@ -245,22 +150,6 @@ public:
     }
     virtual void	stepSimulation(float deltaTime)
 	{
-        if ((m_options == eROBOTIC_LEARN_GRASP_CONTACT) && (m_gripperIndex>=0))
-        {
-            int fingerJointIndices[3]={0,1,3};
-            double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
-                ,-sGripperClosingTargetVelocity
-            };
-            double maxTorqueValues[3]={40.0,50.0,50.0};
-            for (int i=0;i<3;i++)
-            {
-                b3JointMotorArgs 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);
-            }
-        }
 		m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
@@ -290,9 +179,9 @@ public:
     
 	virtual void resetCamera()
 	{
-		float dist = 1.5;
+		float dist = 3;
-        float pitch = 12;
+		float pitch = -75;
-        float yaw = -10;
+		float yaw = 30;
 		float targetPos[3]={-0.2,0.8,0.3};
 		if (m_app->m_renderer  && m_app->m_renderer->getActiveCamera())
 		{

examples/RoboticsLearning/R2D2GraspExample.h

@@ -20,7 +20,6 @@ enum RobotLearningExampleOptions
 {
 	eROBOTIC_LEARN_GRASP=1,
 	eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
-    eROBOTIC_LEARN_GRASP_CONTACT=3,
 };
 
 class CommonExampleInterface*    R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);