Create project file for BussIK inverse kinematics library (premake, c…

…make)

URDF/SDF: add a flag to force concave mesh collisiofor static objects. <collision concave="yes" name="pod_collision">
VR: support teleporting using buttong, allow multiple controllers to be used, fast wireframe rendering,
Turn off warnings about deprecated C routine in btScalar.h/b3Scalar.h
Add a dummy return to stop a warning
Expose defaultContactERP in shared memory api/pybullet.
First start to expose IK in shared memory api/pybullet (not working yet)

build3/premake4.lua

@@ -249,6 +249,7 @@ end
 		include "../examples/InverseDynamics"
 		include "../examples/ExtendedTutorials"
 		include "../examples/SharedMemory"
+		include "../examples/ThirdPartyLibs/BussIK"
 		include "../examples/MultiThreading"
 
 		if _OPTIONS["lua"] then

data/cube_small.urdf

@@ -5,6 +5,7 @@
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.01"/>
       <inertia_scaling value="3.0"/>
+
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/kiva_shelf/model.sdf

@@ -2,18 +2,18 @@
 <sdf version="1.4">
   <model name="Amazon Pod">
   	<static>1</static>
-  	<pose>0 2 0 0 0 0</pose>
+  	<pose>0 1 0 0 0 0</pose>
     <link name="pod_link">
       <inertial>
         <pose>0.0 .0 1.2045 0 0 0</pose>
-        <mass>76.26</mass>
+        <mass>0</mass>
         <inertia>
-          <ixx>47</ixx>
-          <ixy>-0.003456</ixy>
-          <ixz>0.001474</ixz>
-          <izz>13.075</izz>
-          <iyz>-0.014439</iyz>
-          <iyy>47</iyy>
+          <ixx>0</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <izz>0.0</izz>
+          <iyz>0.0</iyz>
+          <iyy>0.0</iyy>
         </inertia>
       </inertial>
 
@@ -30,7 +30,7 @@
       </visual>
 
 
-      <collision name="pod_collision">
+      <collision concave="yes" name="pod_collision">
         <pose>0 0 0 1.5707 0 0</pose>
         <geometry>
           <mesh>

data/samurai.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+  <link concave="yes" name="baseLink">
+    <contact>
+      <rolling_friction value="0.3"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.5707963 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="samurai_monastry.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision concave="yes">
+      <origin rpy="1.5707963 0 0" xyz="0 0 0"/>
+     <geometry>
+				<mesh filename="samurai_monastry.obj" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

examples/CMakeLists.txt

@@ -1,6 +1,6 @@
 SUBDIRS( HelloWorld BasicDemo )
 IF(BUILD_BULLET3)
-	SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen OpenGLWindow )
+	SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK OpenGLWindow )
 ENDIF()
 IF(BUILD_PYBULLET)
 	SUBDIRS(pybullet)

examples/CommonInterfaces/CommonCameraInterface.h

@@ -9,6 +9,7 @@ struct CommonCameraInterface
 	virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0;
 	virtual void disableVRCamera()=0;
 	virtual bool isVRCamera() const =0;
+	virtual void	setVRCameraOffsetTransform(const float offset[16])=0;
 
 	virtual void getCameraTargetPosition(float pos[3]) const = 0;
 	virtual void getCameraPosition(float pos[3]) const = 0;

examples/CommonInterfaces/CommonRenderInterface.h

@@ -27,8 +27,9 @@ struct CommonRenderInterface
 	virtual CommonCameraInterface* getActiveCamera()=0;
 	virtual void setActiveCamera(CommonCameraInterface* cam)=0;
 
-	virtual void renderScene()=0;
 
+	virtual void renderScene()=0;
+	virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){};
 	virtual int getScreenWidth() = 0;
 	virtual int getScreenHeight() = 0;
 

examples/ExampleBrowser/CMakeLists.txt

@@ -47,20 +47,20 @@ IF (BUILD_SHARED_LIBS)
 		IF (WIN32)
                 TARGET_LINK_LIBRARIES(
                         BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
-                        BulletInverseDynamics LinearMath OpenGLWindow gwen
+                        BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
                         ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
                 )
 		ELSE(WIN32)
         IF(APPLE)
                  TARGET_LINK_LIBRARIES(
                                 BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
-                                BulletInverseDynamics LinearMath OpenGLWindow gwen
+                                BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
                                 ${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
                  )
   			ELSE(APPLE)
                  TARGET_LINK_LIBRARIES(
                                 BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
-                                BulletInverseDynamics LinearMath OpenGLWindow gwen
+                                BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
                                 pthread dl
                         )
         ENDIF(APPLE)
@@ -81,7 +81,7 @@ INCLUDE_DIRECTORIES(
 
 
 LINK_LIBRARIES(
-        BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen
+        BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
 )
 
 IF (WIN32)
@@ -129,6 +129,8 @@ SET(BulletExampleBrowser_SRCS
 	../TinyRenderer/TinyRenderer.cpp
 	../SharedMemory/TinyRendererVisualShapeConverter.cpp
 	../SharedMemory/TinyRendererVisualShapeConverter.h
+	../SharedMemory/IKTrajectoryHelper.cpp
+	../SharedMemory/IKTrajectoryHelper.h
 	../RenderingExamples/TinyRendererSetup.cpp
 	../SharedMemory/PhysicsServer.cpp
 	../SharedMemory/PhysicsClientSharedMemory.cpp
@@ -218,8 +220,6 @@ SET(BulletExampleBrowser_SRCS
 	../RoboticsLearning/R2D2GraspExample.h
 	../RoboticsLearning/KukaGraspExample.cpp
 	../RoboticsLearning/KukaGraspExample.h
-	../RoboticsLearning/IKTrajectoryHelper.cpp
-	../RoboticsLearning/IKTrajectoryHelper.h
 	../RenderingExamples/CoordinateSystemDemo.cpp
 	../RenderingExamples/CoordinateSystemDemo.h
 	../RenderingExamples/RaytracerSetup.cpp
@@ -307,17 +307,6 @@ SET(BulletExampleBrowser_SRCS
 	../ThirdPartyLibs/stb_image/stb_image.cpp
 	../ThirdPartyLibs/stb_image/stb_image.h
 
-	../ThirdPartyLibs/BussIK/Jacobian.cpp
-	../ThirdPartyLibs/BussIK/Tree.cpp
-	../ThirdPartyLibs/BussIK/Node.cpp
-	../ThirdPartyLibs/BussIK/LinearR2.cpp
-	../ThirdPartyLibs/BussIK/LinearR3.cpp
-	../ThirdPartyLibs/BussIK/LinearR4.cpp
-	../ThirdPartyLibs/BussIK/MatrixRmn.cpp
-	../ThirdPartyLibs/BussIK/VectorRn.cpp
-	../ThirdPartyLibs/BussIK/Misc.cpp
-
-
 	../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
 	../ThirdPartyLibs/tinyxml/tinystr.cpp
 	../ThirdPartyLibs/tinyxml/tinyxml.cpp

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -146,11 +146,26 @@ struct OpenGLGuiHelperInternalData
 	struct CommonGraphicsApp* m_glApp;
 	class MyDebugDrawer* m_debugDraw;
 	GL_ShapeDrawer* m_gl2ShapeDrawer;
-
+	bool m_vrMode;
+	int m_vrSkipShadowPass;
+
 	btAlignedObjectArray<unsigned char> m_rgbaPixelBuffer1;
 	btAlignedObjectArray<float> m_depthBuffer1;
+
+	OpenGLGuiHelperInternalData()
+		:m_vrMode(false),
+		m_vrSkipShadowPass(0)
+	{
+	}
+
 };
 
+void OpenGLGuiHelper::setVRMode(bool vrMode)
+{
+	m_data->m_vrMode = vrMode;
+	m_data->m_vrSkipShadowPass = 0;
+}
+
 
 
 OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2)
@@ -269,6 +284,10 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
 }
 void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
 {
+	//in VR mode, we skip the synchronization for the second eye
+	if (m_data->m_vrMode && m_data->m_vrSkipShadowPass==1)
+		return;
+
 	int numCollisionObjects = rbWorld->getNumCollisionObjects();
 	for (int i = 0; i<numCollisionObjects; i++)
 	{
@@ -288,8 +307,25 @@ void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWor
 
 void OpenGLGuiHelper::render(const btDiscreteDynamicsWorld* rbWorld)
 {
-
-	m_data->m_glApp->m_renderer->renderScene();
+	if (m_data->m_vrMode)
+	{
+		//in VR, we skip the shadow generation for the second eye
+
+		if (m_data->m_vrSkipShadowPass>=1)
+		{
+			m_data->m_glApp->m_renderer->renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
+			m_data->m_vrSkipShadowPass=0;
+
+		} else
+		{
+			m_data->m_glApp->m_renderer->renderScene();	
+			m_data->m_vrSkipShadowPass++;
+		}
+	} else
+	{
+		m_data->m_glApp->m_renderer->renderScene();	
+	}
+
 	//backwards compatible OpenGL2 rendering
 
 	if (m_data->m_gl2ShapeDrawer && rbWorld)

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -56,6 +56,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
     virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size);
 
 	void renderInternalGl2(int  pass, const btDiscreteDynamicsWorld* dynamicsWorld);
+
+	void setVRMode(bool vrMode);
 };
 
 #endif //OPENGL_GUI_HELPER_H

examples/ExampleBrowser/premake4.lua

@@ -10,7 +10,7 @@ project "App_BulletExampleBrowser"
             initOpenCL("clew")
         end
 
-        links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+        links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
         initOpenGL()
         initGlew()
 
@@ -55,6 +55,8 @@ project "App_BulletExampleBrowser"
 		"../TinyRenderer/our_gl.cpp",
 		"../TinyRenderer/TinyRenderer.cpp",
 		"../RenderingExamples/TinyRendererSetup.cpp",
+		"../SharedMemory/IKTrajectoryHelper.cpp",
+		"../SharedMemory/IKTrajectoryHelper.h",
 		"../SharedMemory/PhysicsClientC_API.cpp",
 		"../SharedMemory/PhysicsClientC_API.h",
 		"../SharedMemory/PhysicsServerExample.cpp",

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -577,13 +577,32 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
                                            glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1],
                                            glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2]));
 							}
-							//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
-							btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
-							//cylZShape->initializePolyhedralFeatures();
-							//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
-							//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
-							cylZShape->setMargin(0.001);
-							shape = cylZShape;
+
+							if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+							{
+								btTriangleMesh* meshInterface = new btTriangleMesh();
+								for (int i=0;i<glmesh->m_numIndices/3;i++)
+								{
+									float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
+									float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
+									float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
+									meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
+																btVector3(v1[0],v1[1],v1[2]),
+															btVector3(v2[0],v2[1],v2[2]));
+								}
+
+								btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
+								shape = trimesh;
+							} else
+							{
+								//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
+								btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+								//cylZShape->initializePolyhedralFeatures();
+								//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+								//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+								cylZShape->setMargin(0.001);
+								shape = cylZShape;
+							}
 						} else
 						{
 							b3Warning("issue extracting mesh from STL file %s\n", fullPath);

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -489,6 +489,9 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
 	if (name_char)
 		collision.m_name = name_char;
 
+	const char *concave_char = config->Attribute("concave");
+	if (concave_char)
+		collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
 
 	return true;
 }

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -75,11 +75,22 @@ struct UrdfVisual
 	UrdfMaterial m_localMaterial;
 };
 
+enum UrdfCollisionFlags
+{
+	URDF_FORCE_CONCAVE_TRIMESH=1,
+};
+
+
 struct UrdfCollision
 {
 	btTransform m_linkLocalFrame;
 	UrdfGeometry m_geometry;
 	std::string m_name;
+	int m_flags;
+	UrdfCollision()
+		:m_flags(0)
+	{
+	}
 };
 
 struct UrdfJoint;

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -17,7 +17,7 @@ subject to the following restrictions:
 
 ///todo: make this configurable in the gui
 bool useShadowMap = true;// true;//false;//true;
-int shadowMapWidth= 2048;//8192;
+int shadowMapWidth= 2048;
 int shadowMapHeight= 2048;
 float shadowMapWorldSize=5;
 
@@ -329,6 +329,19 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
 }
 
 
+void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation)
+{
+	b3Assert(srcIndex<m_data->m_totalNumInstances);
+	b3Assert(srcIndex>=0);
+	position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0];
+	position[1] = m_data->m_instance_positions_ptr[srcIndex*4+1];
+	position[2] = m_data->m_instance_positions_ptr[srcIndex*4+2];
+
+	orientation[0] = m_data->m_instance_quaternion_ptr[srcIndex*4+0];
+	orientation[1] = m_data->m_instance_quaternion_ptr[srcIndex*4+1];
+	orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
+	orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
+}
 void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
 {
 	m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);