Create project file for BussIK inverse kinematics library (premake, cmake)

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>
+          <ixx>0</ixx>
-          <ixy>-0.003456</ixy>
+          <ixy>0.0</ixy>
-          <ixz>0.001474</ixz>
+          <ixz>0.0</ixz>
-          <izz>13.075</izz>
+          <izz>0.0</izz>
-          <iyz>-0.014439</iyz>
+          <iyz>0.0</iyz>
-          <iyy>47</iyy>
+          <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)
 {
-
+	if (m_data->m_vrMode)
-	m_data->m_glApp->m_renderer->renderScene();
+	{
+		//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));
+							if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
-							//cylZShape->initializePolyhedralFeatures();
+							{
-							//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+								btTriangleMesh* meshInterface = new btTriangleMesh();
-							//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+								for (int i=0;i<glmesh->m_numIndices/3;i++)
-							cylZShape->setMargin(0.001);
+								{
-							shape = cylZShape;
+									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]);

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -41,7 +41,7 @@ class GLInstancingRenderer : public CommonRenderInterface
 	int m_upAxis;
     bool m_enableBlend;
     
-	void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
+	
 
 	
 	
@@ -52,6 +52,7 @@ public:
 	virtual void init();
 
 	virtual void renderScene();
+	virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
 
 	void InitShaders();
 	void CleanupShaders();
@@ -73,6 +74,7 @@ public:
 
 	void writeTransforms();
 
+	
 	virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex);
 	virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)
     {
@@ -90,6 +92,8 @@ public:
 
     }
 
+	virtual void readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation);
+
 	virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
 
 	virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);

examples/OpenGLWindow/SimpleCamera.cpp

@@ -3,6 +3,8 @@
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Quaternion.h"
 #include "Bullet3Common/b3Matrix3x3.h"
+#include "Bullet3Common/b3Transform.h"
+
 
 struct SimpleCameraInternalData
 {
@@ -19,6 +21,9 @@ struct SimpleCameraInternalData
 		m_frustumZFar(1000),
 		m_enableVR(false)
 	{
+		b3Transform tr;
+		tr.setIdentity();
+		tr.getOpenGLMatrix(m_offsetTransformVR);
 	}
 	b3Vector3 m_cameraTargetPosition;
 	float m_cameraDistance;
@@ -37,6 +42,7 @@ struct SimpleCameraInternalData
 	bool m_enableVR;
 	float m_viewMatrixVR[16];
 	float m_projectionMatrixVR[16];
+	float m_offsetTransformVR[16];
 
 };
 
@@ -244,13 +250,26 @@ void SimpleCamera::getCameraProjectionMatrix(float projectionMatrix[16]) const
 		b3CreateFrustum(-m_data->m_aspect * m_data->m_frustumZNear, m_data->m_aspect * m_data->m_frustumZNear, -m_data->m_frustumZNear,m_data->m_frustumZNear, m_data->m_frustumZNear, m_data->m_frustumZFar,projectionMatrix);
 	}
 }
+void	SimpleCamera::setVRCameraOffsetTransform(const float offset[16])
+{
+	for (int i=0;i<16;i++)
+	{
+		m_data->m_offsetTransformVR[i]   = offset[i];
+	}
+}
 void SimpleCamera::getCameraViewMatrix(float viewMatrix[16]) const
 {
 	if (m_data->m_enableVR)
 	{
 		for (int i=0;i<16;i++)
 		{
-			viewMatrix[i] = m_data->m_viewMatrixVR[i];
+			b3Transform tr;
+			tr.setFromOpenGLMatrix(m_data->m_viewMatrixVR);
+			b3Transform shift=b3Transform::getIdentity();
+			shift.setFromOpenGLMatrix(m_data->m_offsetTransformVR);
+			tr = tr*shift;
+			tr.getOpenGLMatrix(viewMatrix);
+			//viewMatrix[i] = m_data->m_viewMatrixVR[i];
 		}
 	} else
 	{

examples/OpenGLWindow/SimpleCamera.h

@@ -15,7 +15,9 @@ struct SimpleCamera : public CommonCameraInterface
 	virtual void getCameraViewMatrix(float m[16]) const;
 	
 	virtual void	setVRCamera(const float viewMat[16], const float projectionMatrix[16]);
+	virtual void	setVRCameraOffsetTransform(const float offset[16]);
 	virtual void disableVRCamera();
+
 	virtual bool isVRCamera() const;
 
 	virtual void getCameraTargetPosition(float pos[3]) const;

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -1,6 +1,6 @@
 
 #include "KukaGraspExample.h"
-#include "IKTrajectoryHelper.h"
+#include "../SharedMemory/IKTrajectoryHelper.h"
 
 #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
 #include "Bullet3Common/b3Quaternion.h"
@@ -138,8 +138,11 @@ public:
     }
     virtual void	stepSimulation(float deltaTime)
 	{
-        m_time+=deltaTime;
+		float dt = deltaTime;
-        m_targetPos.setValue(0.5, 0, 0.5+0.4*b3Sin(3 * m_time));
+		btClamp(dt,0.0001f,0.01f);
+
+        m_time+=dt;
+        m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
 
         
         
@@ -158,11 +161,36 @@ public:
                     q_current[i] = jointStates.m_actualStateQ[i+7];
                 }
             }
+			 b3Vector3DoubleData dataOut;
+			 m_targetPos.serializeDouble(dataOut);
+
+
             // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
+/*
+			b3RobotSimInverseKinematicArgs ikargs;
+			b3RobotSimInverseKinematicsResults ikresults;
+			
+         
+			ikargs.m_bodyUniqueId = m_kukaIndex;
+			ikargs.m_currentJointPositions = q_current;
+			ikargs.m_numPositions = 7;
+			ikargs.m_endEffectorTargetPosition[0] = dataOut.m_floats[0];
+			ikargs.m_endEffectorTargetPosition[1] = dataOut.m_floats[1];
+			ikargs.m_endEffectorTargetPosition[2] = dataOut.m_floats[2];
+			
+
+			ikargs.m_endEffectorTargetOrientation[0] = 0;
+			ikargs.m_endEffectorTargetOrientation[1] = 0;
+			ikargs.m_endEffectorTargetOrientation[2] = 0;
+			ikargs.m_endEffectorTargetOrientation[3] = 1;
+
+			if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
+			{
+			}
+*/
             double q_new[7];
             int ikMethod=IK2_SDLS;
-            b3Vector3DoubleData dataOut;
+           
-            m_targetPos.serializeDouble(dataOut);
             m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod);
             printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
                    q_new[3],q_new[4],q_new[5],q_new[6]);

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -469,6 +469,33 @@ void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps)
     b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
 }
 
+/*
+b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+	const double* jointPositionsQ, double targetPosition[3]);
+
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+	int* bodyUniqueId,
+	int* dofCount,
+	double* jointPositions);
+*/
+bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
+{
+
+	b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId,
+		args.m_currentJointPositions,args.m_endEffectorTargetPosition);
+
+    b3SharedMemoryStatusHandle statusHandle;
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+
+	bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+		&results.m_bodyUniqueId,
+		&results.m_numPositions,
+		results.m_calculatedJointPositions);
+
+	return result;
+}
+
+
 b3RobotSimAPI::~b3RobotSimAPI()
 {
 	delete m_data;

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -50,6 +50,7 @@ struct b3RobotSimLoadFileArgs
 	}
 };
 
+
 struct b3RobotSimLoadFileResults
 {
 	b3AlignedObjectArray<int> m_uniqueObjectIds;
@@ -81,6 +82,35 @@ struct b3JointMotorArgs
 	}
 };
 
+enum b3InverseKinematicsFlags
+{
+	B3_HAS_IK_TARGET_ORIENTATION=1,
+};
+
+struct b3RobotSimInverseKinematicArgs
+{
+	int m_bodyUniqueId;
+	double* m_currentJointPositions;
+	int m_numPositions;
+	double m_endEffectorTargetPosition[3];
+	double m_endEffectorTargetOrientation[3];
+	int m_flags;
+
+	b3RobotSimInverseKinematicArgs()
+		:m_bodyUniqueId(-1),
+		m_currentJointPositions(0),
+		m_numPositions(0),
+		m_flags(0)
+	{
+	}
+};
+
+struct b3RobotSimInverseKinematicsResults
+{
+	int m_bodyUniqueId;
+	double* m_calculatedJointPositions;
+	int m_numPositions;
+};
 
 class b3RobotSimAPI
 {
@@ -113,6 +143,8 @@ public:
     
     void setNumSimulationSubSteps(int numSubSteps);
 
+	bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results);
+
 	void renderScene();
 	void debugDraw(int debugDrawMode);
 };

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -4,11 +4,11 @@
 #include "BussIK/Jacobian.h"
 #include "BussIK/VectorRn.h"
 #include "Bullet3Common/b3AlignedObjectArray.h"
+#include "BulletDynamics/Featherstone/btMultiBody.h"
 
 
 #define RADIAN(X)	((X)*RadiansToDegrees)
 
-
 //use BussIK and Reflexxes to convert from Cartesian endeffector future target to
 //joint space positions at each real-time (simulation) step
 struct IKTrajectoryHelperInternalData
@@ -36,6 +36,17 @@ IKTrajectoryHelper::~IKTrajectoryHelper()
     delete m_data;
 }
 
+bool IKTrajectoryHelper::createFromMultiBody(class btMultiBody* mb)
+{
+	//todo: implement proper conversion. For now, we only 'detect' a likely KUKA iiwa and hardcode its creation
+	if (mb->getNumLinks()==7)
+	{
+		createKukaIIWA();
+		return true;
+	}
+
+	return false;
+}
 
 void IKTrajectoryHelper::createKukaIIWA()
 {
@@ -142,4 +153,4 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
         q_new[i] = m_data->m_ikNodes[i]->GetTheta();
     }
     return true;
-}
+}

examples/SharedMemory/IKTrajectoryHelper.h

@@ -22,6 +22,8 @@ public:
     ///todo: replace createKukaIIWA with a generic way of create an IK tree
     void createKukaIIWA();
     
+	bool createFromMultiBody(class btMultiBody* mb);
+
     bool computeIK(const double endEffectorTargetPosition[3],
                    const double* q_old, int numQ,
                    double* q_new, int ikMethod);

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -163,6 +163,19 @@ int	b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int
     return 0;
 }
 
+
+int	b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+    command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP;
+    command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
+    return 0;
+}
+
+
+int	b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
+
 b3SharedMemoryCommandHandle	b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1251,4 +1264,62 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl
 
 	
 	return true;
-}
+}
+
+
+///compute the joint positions to move the end effector to a desired target using inverse kinematics
+b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+	const double* jointPositionsQ, const double targetPosition[3])
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
+	command->m_updateFlags = 0;
+	command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
+	int numJoints = cl->getNumJoints(bodyIndex);
+	for (int i = 0; i < numJoints;i++)
+	{
+		command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
+	}
+
+	command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
+	command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
+	command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
+
+	return (b3SharedMemoryCommandHandle)command;
+
+}
+
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+	int* bodyUniqueId,
+	int* dofCount,
+	double* jointPositions)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
+	if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
+		return false;
+
+
+	if (dofCount)
+	{
+		*dofCount = status->m_inverseKinematicsResultArgs.m_dofCount;
+	}
+	if (bodyUniqueId)
+	{
+		*bodyUniqueId = status->m_inverseKinematicsResultArgs.m_bodyUniqueId;
+	}
+	if (jointPositions)
+	{
+		for (int i = 0; i < status->m_inverseKinematicsResultArgs.m_dofCount; i++)
+		{
+			jointPositions[i] = status->m_inverseKinematicsResultArgs.m_jointPositions[i];
+		}
+	}
+
+	return false;
+}

examples/SharedMemory/PhysicsClientC_API.h

@@ -89,6 +89,7 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
 b3SharedMemoryCommandHandle	b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
 int	b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
 int	b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
+int	b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
 int	b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
 int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
 
@@ -115,6 +116,17 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl
 	int* dofCount,
 	double* jointForces);
 
+///compute the joint positions to move the end effector to a desired target using inverse kinematics
+b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+	const double* jointPositionsQ, const double targetPosition[3]);
+
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+	int* bodyUniqueId,
+	int* dofCount,
+	double* jointPositions);
+
+
+
 
 b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -15,7 +15,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
 #include "LinearMath/btHashMap.h"
 #include "BulletInverseDynamics/MultiBodyTree.hpp"
-
+#include "IKTrajectoryHelper.h"
 #include "btBulletDynamicsCommon.h"
 
 #include "LinearMath/btTransform.h"
@@ -381,6 +381,8 @@ struct PhysicsServerCommandProcessorInternalData
 	bool m_allowRealTimeSimulation;
 	bool m_hasGround;
 
+	btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
+
 	CommandLogger* m_commandLogger;
 	CommandLogPlayback* m_logPlayback;
 
@@ -389,6 +391,7 @@ struct PhysicsServerCommandProcessorInternalData
     btScalar m_numSimulationSubSteps;
 	btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
 	btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
+	btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
 
 
 
@@ -428,7 +431,8 @@ struct PhysicsServerCommandProcessorInternalData
 
 	PhysicsServerCommandProcessorInternalData()
 		:m_hasGround(false),
-		m_allowRealTimeSimulation(false),
+		m_gripperRigidbodyFixed(0),
+		m_allowRealTimeSimulation(true),
 		m_commandLogger(0),
 		m_logPlayback(0),
 		m_physicsDeltaTime(1./240.),
@@ -556,6 +560,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
 
 	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
+	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.005;
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -1061,6 +1066,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 			//no timestamp yet
             int timeStamp = 0;
+			
+			//catch uninitialized cases
+			serverStatusOut.m_type = CMD_INVALID_STATUS;
 
             //consume the command
 			switch (clientCmd.m_type)
@@ -1868,6 +1876,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                         m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
                     }
 
+					if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
+                    {
+                        m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
+                    }
+
 					SharedMemoryStatus& serverCmd =serverStatusOut;
 					serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
 					hasStatus = true;
@@ -1961,6 +1974,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
 					hasStatus = true;
 					m_data->m_hasGround = false;
+					m_data->m_gripperRigidbodyFixed = 0;
                     break;
                 }
 				case CMD_CREATE_RIGID_BODY:
@@ -2441,9 +2455,50 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                             }
                         }
                     }
+					SharedMemoryStatus& serverCmd =serverStatusOut;
+                    serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
                     hasStatus = true;
                     break;
                 }
+				case CMD_CALCULATE_INVERSE_KINEMATICS:
+					{
+						SharedMemoryStatus& serverCmd = serverStatusOut;
+						serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
+
+						InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
+						if (bodyHandle && bodyHandle->m_multiBody)
+						{
+							IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
+							IKTrajectoryHelper* ikHelperPtr = 0;
+							
+
+							if (ikHelperPtrPtr)
+							{
+								ikHelperPtr = *ikHelperPtrPtr;
+							}
+							else
+							{
+								IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
+								if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody))
+								{
+									m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, ikHelperPtr);
+									ikHelperPtr = tmpHelper;
+								} else
+								{
+									delete tmpHelper;
+								}
+							}
+
+							if (ikHelperPtr)
+							{
+
+								serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
+							}
+						}
+						hasStatus = true;
+						break;
+					}
+			
                 default:
                 {
                     b3Error("Unknown command encountered");
@@ -2461,13 +2516,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 	return hasStatus;
 }
 
+static int skip=1;
 
 void PhysicsServerCommandProcessor::renderScene()
 {
 	if (m_data->m_guiHelper)
 	{
 		m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
-
 		m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
 	}
 
@@ -2485,6 +2540,7 @@ void    PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
 	}
 }
 
+btVector3 gLastPickPos(0,0,0);
 
 bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
 {
@@ -2498,6 +2554,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
 	{
 
 		btVector3 pickPos = rayCallback.m_hitPointWorld;
+		gLastPickPos = pickPos;
 		btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
 		if (body)
 		{
@@ -2516,6 +2573,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
 				//very weak constraint for picking
 				p2p->m_setting.m_tau = 0.001f;
 			}
+			
 		} else
 		{
 			btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
@@ -2634,23 +2692,69 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
 	m_data->m_logPlayback = pb;
 }
 
+btVector3 gVRGripperPos(0,0,0);
+btQuaternion gVRGripperOrn(0,0,0,1);
+
 void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 {
-	if (m_data->m_allowRealTimeSimulation)
+	if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper)
 	{
+		btAlignedObjectArray<char> bufferServerToClient;
+		bufferServerToClient.resize(32768);
+
+
 		if (!m_data->m_hasGround)
 		{
 			m_data->m_hasGround = true;
 
 			int bodyId = 0;
-			btAlignedObjectArray<char> bufferServerToClient;
+			
-			bufferServerToClient.resize(32768);
-
 
 			loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
 		}
 
-		m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime);
+		if (0)//m_data->m_gripperRigidbodyFixed==0)
+		{
+			int bodyId = 0;
+
+			loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
+
+			InteralBodyData* parentBody = m_data->getHandle(bodyId);
+			if (parentBody->m_multiBody)
+			{
+				parentBody->m_multiBody->setHasSelfCollision(1);
+				btVector3 pivotInParent(0,0,0);
+				btMatrix3x3 frameInParent;
+				frameInParent.setRotation(btQuaternion(0,0,0,1));
+				
+				btVector3 pivotInChild(0,0,0);
+				btMatrix3x3 frameInChild;
+				frameInChild.setIdentity();
+
+				m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,-1,0,pivotInParent,pivotInChild,frameInParent,frameInChild);
+				m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
+				btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+				world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
+			}
+		}
+
+		if (m_data->m_gripperRigidbodyFixed)
+		{
+			m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
+			m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
+		}
+
+		int maxSteps = 3;
+
+		int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime);
+		int droppedSteps = numSteps > maxSteps ? numSteps - maxSteps : 0;
+		static int skipReport = 0;
+		skipReport++;
+		if (skipReport>100)
+		{
+			skipReport = 0;
+			//printf("numSteps: %d (dropped %d), %f (internal step = %f)\n", numSteps, droppedSteps , dtInSec, m_data->m_physicsDeltaTime);
+		}
 	}
 }
 

examples/SharedMemory/PhysicsServerExample.cpp

@@ -13,8 +13,11 @@
 #include "../Utils/b3Clock.h"
 #include "../MultiThreading/b3ThreadSupportInterface.h"
 
+#define MAX_VR_CONTROLLERS 4
 
-
+extern btVector3 gLastPickPos;
+btVector3 gVRTeleportPos(0,0,0);
+bool gDebugRenderToggle  = false;
 void	MotionThreadFunc(void* userPtr,void* lsMemory);
 void*	MotionlsMemoryFunc();
 #define MAX_MOTION_NUM_THREADS 1
@@ -82,22 +85,27 @@ b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
 struct	MotionArgs
 {
 	MotionArgs()
-		:m_physicsServerPtr(0),
+		:m_physicsServerPtr(0)
-		m_isPicking(false),
-		m_isDragging(false),
-		m_isReleasing(false)
 	{
+		for (int i=0;i<MAX_VR_CONTROLLERS;i++)
+		{
+			m_isVrControllerPicking[i] = false;
+			m_isVrControllerDragging[i] = false;
+			m_isVrControllerReleasing[i] = false;
+			m_isVrControllerTeleporting[i] = false;
+		}
 	}
 	b3CriticalSection* m_cs;
 	
 	PhysicsServerSharedMemory*	m_physicsServerPtr;
 	b3AlignedObjectArray<b3Vector3> m_positions;
 
-	btVector3 m_pos;
+	btVector3 m_vrControllerPos[MAX_VR_CONTROLLERS];
-	btQuaternion m_orn;
+	btQuaternion m_vrControllerOrn[MAX_VR_CONTROLLERS];
-	bool m_isPicking;
+	bool m_isVrControllerPicking[MAX_VR_CONTROLLERS];
-	bool m_isDragging;
+	bool m_isVrControllerDragging[MAX_VR_CONTROLLERS];
-	bool m_isReleasing;
+	bool m_isVrControllerReleasing[MAX_VR_CONTROLLERS];
+	bool m_isVrControllerTeleporting[MAX_VR_CONTROLLERS];
 	
 };
 
@@ -108,6 +116,7 @@ struct MotionThreadLocalStorage
 
 int skip = 0;
 int skip1 = 0;
+float clampedDeltaTime  = 0.2;
 
 void	MotionThreadFunc(void* userPtr,void* lsMemory)
 {
@@ -127,12 +136,11 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 		args->m_cs->unlock();
 
 
+		double deltaTimeInSeconds = 0;
+
 		do
 		{
-//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
+			deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.;
-			
-
-			double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
 
 			if (deltaTimeInSeconds<(1./5000.))
 			{
@@ -150,40 +158,58 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 				//process special controller commands, such as
 				//VR controller button press/release and controller motion
 
-				btVector3 from = args->m_pos;
+				for (int c=0;c<MAX_VR_CONTROLLERS;c++)
-				btMatrix3x3 mat(args->m_orn);
-				btScalar pickDistance = 100.;
-				btVector3 toX = args->m_pos+mat.getColumn(0);
-				btVector3 toY = args->m_pos+mat.getColumn(1);
-				btVector3 toZ = args->m_pos+mat.getColumn(2)*pickDistance;
-
-
-				if (args->m_isPicking)
 				{
-					args->m_isPicking  = false;
-					args->m_isDragging = true;
-					args->m_physicsServerPtr->pickBody(from,-toZ);
-					//printf("PICK!\n");
-				}
-
-				 if (args->m_isDragging)
-				 {
-					 args->m_physicsServerPtr->movePickedBody(from,-toZ);
-					// printf(".");
-				 }
 				
-				if (args->m_isReleasing)
+					btVector3 from = args->m_vrControllerPos[c];
-				{
+					btMatrix3x3 mat(args->m_vrControllerOrn[c]);
-					args->m_isDragging = false;
+				
-					args->m_isReleasing = false;
+					btScalar pickDistance = 1000.;
-					args->m_physicsServerPtr->removePickingConstraint();
+					btVector3 toX = from+mat.getColumn(0);
-					//printf("Release pick\n");
+					btVector3 toY = from+mat.getColumn(1);
+					btVector3 toZ = from+mat.getColumn(2)*pickDistance;
+
+					if (args->m_isVrControllerTeleporting[c])
+					{
+						args->m_isVrControllerTeleporting[c] = false;
+						args->m_physicsServerPtr->pickBody(from,-toZ);
+						args->m_physicsServerPtr->removePickingConstraint();
+					}
+
+					if (args->m_isVrControllerPicking[c])
+					{
+						args->m_isVrControllerPicking[c]  = false;
+						args->m_isVrControllerDragging[c] = true;
+						args->m_physicsServerPtr->pickBody(from,-toZ);
+						//printf("PICK!\n");
+					}
+
+					 if (args->m_isVrControllerDragging[c])
+					 {
+						 args->m_physicsServerPtr->movePickedBody(from,-toZ);
+						// printf(".");
+					 }
+				
+					if (args->m_isVrControllerReleasing[c])
+					{
+						args->m_isVrControllerDragging[c] = false;
+						args->m_isVrControllerReleasing[c] = false;
+						args->m_physicsServerPtr->removePickingConstraint();
+						//printf("Release pick\n");
+					}
 				}
 
 				//don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
-				btClamp(deltaTimeInSeconds,0.,0.1);
+				if (deltaTimeInSeconds>clampedDeltaTime)
-				args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
+				{
+					deltaTimeInSeconds = clampedDeltaTime;
+					b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
+				}
+				
 				clock.reset();
+				args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
+				deltaTimeInSeconds = 0;
+				
 			}
 
 			args->m_physicsServerPtr->processClientCommands();
@@ -318,7 +344,10 @@ public:
 		m_childGuiHelper->render(0);
 	}
 
-	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
+	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)
+	{
+		m_childGuiHelper->createPhysicsDebugDrawer(rbWorld);
+	}
 
 	virtual int	registerTexture(const unsigned char* texels, int width, int height)
 	{
@@ -845,36 +874,52 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 	}
 }
 
+static float vrOffset[16]={1,0,0,0,
+							0,1,0,0,
+							0,0,1,0,
+							0,0,0,0};
+
 
 void PhysicsServerExample::renderScene()
 {
 	///debug rendering
 	//m_args[0].m_cs->lock();
 	
+	vrOffset[12]=-gVRTeleportPos[0];
+	vrOffset[13]=-gVRTeleportPos[1];
+	vrOffset[14]=-gVRTeleportPos[2];
+
+	this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
+		getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
+
 	m_physicsServer.renderScene();
 	
-	if (m_args[0].m_isPicking || m_args[0].m_isDragging)
+	for (int i=0;i<MAX_VR_CONTROLLERS;i++)
 	{
-		btVector3 from = m_args[0].m_pos;
+		if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
-		btMatrix3x3 mat(m_args[0].m_orn);
+		{
+			btVector3 from = m_args[0].m_vrControllerPos[i];
+			btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]);
 	
-		btVector3 toX = m_args[0].m_pos+mat.getColumn(0);
+			btVector3 toX = from+mat.getColumn(0);
-		btVector3 toY = m_args[0].m_pos+mat.getColumn(1);
+			btVector3 toY = from+mat.getColumn(1);
-		btVector3 toZ = m_args[0].m_pos+mat.getColumn(2);
+			btVector3 toZ = from+mat.getColumn(2);
 	
-		int width = 2;
+			int width = 2;
 
 	
-		btVector4 color;
+			btVector4 color;
-		color=btVector4(1,0,0,1);
+			color=btVector4(1,0,0,1);
-		m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
+			m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
-		color=btVector4(0,1,0,1);
+			color=btVector4(0,1,0,1);
-		m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
+			m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
-		color=btVector4(0,0,1,1);
+			color=btVector4(0,0,1,1);
-		m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
+			m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
 	
+		}
 	}
 
+	if (gDebugRenderToggle)
 	if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
 	{
 		//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
@@ -900,6 +945,10 @@ void PhysicsServerExample::renderScene()
 		sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2);
 		float pos[4];
 		m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
+		pos[0]+=gVRTeleportPos[0];
+		pos[1]+=gVRTeleportPos[1];
+		pos[2]+=gVRTeleportPos[2];
+
 		btTransform viewTr;
 		btScalar m[16];
 		float mf[16];
@@ -908,6 +957,9 @@ void PhysicsServerExample::renderScene()
 		{
 			m[i] = mf[i];
 		}
+		m[12]=+gVRTeleportPos[0];
+		m[13]=+gVRTeleportPos[1];
+		m[14]=+gVRTeleportPos[2];
 		viewTr.setFromOpenGLMatrix(m);
 		btTransform viewTrInv = viewTr.inverse();
 		float upMag = -.6;
@@ -952,7 +1004,7 @@ btVector3	PhysicsServerExample::getRayTo(int x,int y)
 	btVector3 camPos,camTarget;
 	renderer->getActiveCamera()->getCameraPosition(camPos);
 	renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
-
+	
 	btVector3	rayFrom = camPos;
 	btVector3 rayForward = (camTarget-camPos);
 	rayForward.normalize();
@@ -1025,17 +1077,54 @@ class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOpt
 
 }
 
+
+
 void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
 {
-	m_args[0].m_isPicking = (state!=0);
+	//printf("controllerId %d, button=%d\n",controllerId, button);
-	m_args[0].m_isReleasing = (state==0);
+	
-	m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]);
+	if (controllerId<0 || controllerId>MAX_VR_CONTROLLERS)
-	m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]);
+		return;
+
+	if (button==1 && state==0)
+	{
+		gVRTeleportPos = gLastPickPos;
+	}
+	if (button==32 && state==0)
+	{
+		gDebugRenderToggle = !gDebugRenderToggle;
+	}
+	
+	if (button==1)
+	{
+		m_args[0].m_isVrControllerTeleporting[controllerId] = true;
+	}
+
+	if ((button==33))
+	{
+		m_args[0].m_isVrControllerPicking[controllerId] = (state!=0);
+		m_args[0].m_isVrControllerReleasing[controllerId] = (state==0);
+	}
+	if ((button==33)||(button==1))
+	{
+		m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
+		m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]);
+	}
 }
 
+extern btVector3 gVRGripperPos;
+extern btQuaternion gVRGripperOrn;
+
 void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4])
 {
-	m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]);
+
-	m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]);
+	m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
+	m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]);
+	
+	gVRGripperPos.setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
+	btQuaternion orgOrn(orn[0],orn[1],orn[2],orn[3]);
+	gVRGripperOrn = orgOrn*btQuaternion(btVector3(0,0,1),SIMD_HALF_PI)*btQuaternion(btVector3(0,1,0),SIMD_HALF_PI);
+
+
 }
 B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

examples/SharedMemory/SharedMemoryCommands.h

@@ -223,6 +223,7 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4,	
 	SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
 	SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
+	SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32
 };
 
 ///Controlling a robot involves sending the desired state to its joint motor controllers.
@@ -234,6 +235,7 @@ struct SendPhysicsSimulationParameters
 	int m_numSimulationSubSteps;
 	int m_numSolverIterations;
 	bool m_allowRealTimeSimulation;
+	double m_defaultContactERP;
 };
 
 struct RequestActualStateArgs
@@ -372,6 +374,22 @@ struct CalculateInverseDynamicsResultArgs
 	double m_jointForces[MAX_DEGREE_OF_FREEDOM];
 };
 
+struct CalculateInverseKinematicsArgs
+{
+	int m_bodyUniqueId;
+	double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+	double m_targetPosition[3];
+	double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
+};
+
+struct CalculateInverseKinematicsResultArgs
+{
+	int m_bodyUniqueId;
+	int m_dofCount;
+	double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
+};
+
+
 struct CreateJointArgs
 {
     int m_parentBodyIndex;
@@ -413,6 +431,7 @@ struct SharedMemoryCommand
 		struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
         struct CreateJointArgs m_createJointArguments;
         struct RequestContactDataArgs m_requestContactPointArguments;
+		struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
     };
 };
 
@@ -445,6 +464,7 @@ struct SharedMemoryStatus
 		struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
 		struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
 		struct SendContactDataArgs m_sendContactPointArgs;
+		struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -69,6 +69,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
 		CMD_CONTACT_POINT_INFORMATION_COMPLETED,
 		CMD_CONTACT_POINT_INFORMATION_FAILED,
+		CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
+		CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
         CMD_MAX_SERVER_COMMANDS
 };
 

examples/SharedMemory/premake4.lua

@@ -10,13 +10,15 @@ end
 includedirs {".","../../src", "../ThirdPartyLibs",}
 
 links {
-	"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics",	"BulletDynamics","BulletCollision", "LinearMath"
+	"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics",	"BulletDynamics","BulletCollision", "LinearMath", "BussIK"
 }
 
 language "C++"
 
 myfiles = 
 {
+	"IKTrajectoryHelper.cpp",
+	"IKTrajectoryHelper.h",
 	"PhysicsClient.cpp",
 	"PhysicsClientSharedMemory.cpp",
 	"PhysicsClientExample.cpp",
@@ -134,10 +136,10 @@ else
 end
 defines {"B3_USE_STANDALONE_EXAMPLE"}
 
-includedirs {"../../src"}
+includedirs {"../../src", "../ThirdPartyLibs"}
 
 links {
-        "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+        "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common","BussIK"
 }
 	initOpenGL()
   initGlew()
@@ -211,7 +213,7 @@ if os.is("Windows") then
 		}
 						
 	links {
-		"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common",	"BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
+		"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common",	"BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api","BussIK"
 	}
 	
 	

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -13,7 +13,9 @@
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
 
+#include "LinearMath/btIDebugDraw.h"
 int gSharedMemoryKey = -1;
+int  gDebugDrawFlags = 0;
 
 //how can you try typing on a keyboard, without seeing it?
 //it is pretty funny, to see the desktop in VR!
@@ -32,7 +34,7 @@ int gSharedMemoryKey = -1;
 CommonExampleInterface*    sExample;
 
 int sPrevPacketNum=0;
-GUIHelperInterface* sGuiPtr = 0;
+OpenGLGuiHelper* sGuiPtr = 0;
 
 
 static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 };
@@ -384,6 +386,8 @@ bool CMainApplication::BInit()
 
 	
 	sGuiPtr = new OpenGLGuiHelper(m_app,false);
+	sGuiPtr->setVRMode(true);
+
 	//sGuiPtr = new DummyGUIHelper;
 
     
@@ -467,7 +471,7 @@ bool CMainApplication::BInit()
  	m_fScaleSpacing = 4.0f;
  
  	m_fNearClip = 0.1f;
- 	m_fFarClip = 30.0f;
+ 	m_fFarClip = 3000.0f;
  
  	m_iTexture = 0;
  	m_uiVertcount = 0;
@@ -668,6 +672,7 @@ bool CMainApplication::HandleInput()
 		{
 			//we need to have the 'move' events, so no early out here
 			//if (sPrevStates[unDevice].unPacketNum != state.unPacketNum)
+			if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
 			{
 				sPrevStates[unDevice].unPacketNum = state.unPacketNum;
 
@@ -689,6 +694,14 @@ bool CMainApplication::HandleInput()
 						if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0)
 						{
 //							printf("Device PRESSED: %d, button %d\n", unDevice, button);
+							if (button==2)
+							{
+								glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+								gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints
+									//btIDebugDraw::DBG_DrawConstraintLimits+
+									//btIDebugDraw::DBG_DrawConstraints
+									;
+							}
 							sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
 						}
 						else
@@ -699,16 +712,30 @@ bool CMainApplication::HandleInput()
 					}
 					else
 					{
-						//not pressed now, but pressed before -> raise a button up event
+						if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
-						if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0)
 						{
 							b3Transform tr;
 							getControllerTransform(unDevice, tr);
 							float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] };
 							b3Quaternion born = tr.getRotation();
 							float orn[4] = { born[0], born[1], born[2], born[3] };
-//							printf("Device RELEASED: %d, button %d\n", unDevice,button);
+	//							printf("Device RELEASED: %d, button %d\n", unDevice,button);
-							sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn);
+					
+							//not pressed now, but pressed before -> raise a button up event
+							if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0)
+							{
+								if (button==2)
+								{
+									gDebugDrawFlags = 0;
+									glPolygonMode( GL_FRONT_AND_BACK, GL_FILL);
+								}
+							
+								sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn);
+							} else
+							{
+
+								sExample->vrControllerMoveCallback(unDevice, pos, orn);
+							}
 						}
 					}
 				}
@@ -1603,11 +1630,19 @@ void CMainApplication::RenderStereoTargets()
 	
 	m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId);
 
-	sExample->renderScene();
+	if (gDebugDrawFlags)
+	{
+		sExample->physicsDebugDraw(gDebugDrawFlags);
+	} 
+	
+	{
+		sExample->renderScene();
+	}
+
 	//m_app->m_instancingRenderer->renderScene();
 	DrawGridData gridUp;
 	gridUp.upAxis = m_app->getUpAxis();
-	m_app->drawGrid(gridUp);
+//	m_app->drawGrid(gridUp);
 
 	
  	glBindFramebuffer( GL_FRAMEBUFFER, 0 );
@@ -1645,8 +1680,17 @@ void CMainApplication::RenderStereoTargets()
 	
 	m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId);
 	//m_app->m_renderer->renderScene();
-	sExample->renderScene();
+	
-	m_app->drawGrid(gridUp);
+	if (gDebugDrawFlags)
+	{
+		sExample->physicsDebugDraw(gDebugDrawFlags);
+	} 
+
+	{
+		sExample->renderScene();
+	}
+
+	//m_app->drawGrid(gridUp);
 	
  	glBindFramebuffer( GL_FRAMEBUFFER, 0 );
  	

examples/ThirdPartyLibs/BussIK/CMakeLists.txt

@@ -0,0 +1,10 @@
+
+INCLUDE_DIRECTORIES(
+	.
+)
+
+
+FILE(GLOB BussIK_SRCS "*.cpp" )
+FILE(GLOB BussIK_HDRS "*.h" )
+
+ADD_LIBRARY(BussIK ${BussIK_SRCS} ${BussIK_HDRS})

examples/ThirdPartyLibs/BussIK/Misc.cpp

@@ -23,7 +23,7 @@ subject to the following restrictions:
 #include <math.h>
 #include "LinearR3.h"
 
-#include "../OpenGLWindow/OpenGLInclude.h"
+
 
 /****************************************************************
 							 Axes 

examples/ThirdPartyLibs/BussIK/premake4.lua

@@ -0,0 +1,11 @@
+	project "BussIK"
+		
+	kind "StaticLib"
+
+	includedirs {
+		"."
+	}
+	files {
+		"*.cpp",
+		"*.h",
+	}

examples/pybullet/CMakeLists.txt

@@ -8,6 +8,8 @@ INCLUDE_DIRECTORIES(
 
 SET(pybullet_SRCS
 	pybullet.c
+		../../examples/SharedMemory/IKTrajectoryHelper.cpp
+		../../examples/SharedMemory/IKTrajectoryHelper.h
 		../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
     ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
 		../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
@@ -76,6 +78,6 @@ ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS})
 SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION})
 SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION})
 
-TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen Bullet3Common ${PYTHON_LIBRARIES})
+TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common ${PYTHON_LIBRARIES})
 
 

examples/pybullet/premake4.lua

@@ -10,7 +10,7 @@ project ("pybullet")
 		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
 	hasCL = findOpenCL("clew")
 
-	links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+	links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
 	initOpenGL()
 	initGlew()
 
@@ -36,6 +36,8 @@ project ("pybullet")
 
 		files {
 			"pybullet.c",
+			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+			"../../examples/SharedMemory/IKTrajectoryHelper.h",
 			"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
     	"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
 			"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",

examples/pybullet/pybullet.c

@@ -593,6 +593,36 @@ pybullet_setTimeStep(PyObject* self, PyObject* args)
     return Py_None;
 }
 
+static PyObject *
+pybullet_setDefaultContactERP(PyObject* self, PyObject* args)
+{
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+    
+    {
+        double defaultContactERP=0.005;
+        int ret;
+        
+        b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+        b3SharedMemoryStatusHandle statusHandle;
+        
+        if (!PyArg_ParseTuple(args, "d", &defaultContactERP))
+        {
+            PyErr_SetString(SpamError, "default Contact ERP expected a single value (double).");
+            return NULL;
+        }
+        ret = b3PhysicsParamSetDefaultContactERP(command,  defaultContactERP);
+
+        statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+    }
+    
+    Py_INCREF(Py_None);
+    return Py_None;
+}
+
 
 
 // Internal function used to get the base position and orientation
@@ -1865,6 +1895,10 @@ static PyMethodDef SpamMethods[] = {
 
     {"setTimeStep",  pybullet_setTimeStep, METH_VARARGS,
         "Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"},
+
+	{"setDefaultContactERP",  pybullet_setDefaultContactERP, METH_VARARGS,
+        "Set the amount of contact penetration Error Recovery Paramater (ERP) in each time step. \
+		This is an tuning parameter to control resting contact stability. It depends on the time step. For 1/240 timestep, 0.005 is a reasonable values."},
     
 	{ "setRealTimeSimulation", pybullet_setRealTimeSimulation, METH_VARARGS,
 	"Enable or disable real time simulation (using the real time clock, RTC) in the physics server. Expects one integer argument, 0 or 1" },

src/Bullet3Common/b3Scalar.h

@@ -55,7 +55,7 @@ inline int	b3GetVersion()
 			//#define B3_HAS_ALIGNED_ALLOCATOR
 			#pragma warning(disable : 4324) // disable padding warning
 //			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
-//			#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+			#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
 //			#pragma warning(disable:4786) // Disable the "debug name too long" warning
 
 			#define B3_FORCE_INLINE __forceinline

src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -609,6 +609,7 @@ static inline int jointNumDoFs(const JointType &type) {
 	error_message("invalid joint type\n");
 	// TODO add configurable abort/crash function
 	abort();
+	return 0;
 }
 
 int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,

src/LinearMath/btScalar.h

@@ -58,7 +58,7 @@ inline int	btGetVersion()
 			//#define BT_HAS_ALIGNED_ALLOCATOR
 			#pragma warning(disable : 4324) // disable padding warning
 //			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
-//			#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+			#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
 //			#pragma warning(disable:4786) // Disable the "debug name too long" warning
 
 			#define SIMD_FORCE_INLINE __forceinline

test/SharedMemory/CMakeLists.txt

@@ -13,7 +13,7 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA
 
 
 LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest
+ BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK
 )
 
 IF (NOT WIN32)
@@ -23,6 +23,8 @@ ENDIF()
 	ADD_EXECUTABLE(Test_PhysicsClientServer
 		gtestwrap.cpp
 		../../examples/SharedMemory/PhysicsClient.cpp
+												../../examples/SharedMemory/IKTrajectoryHelper.cpp
+												../../examples/SharedMemory/IKTrajectoryHelper.h
                         ../../examples/SharedMemory/PhysicsClient.h
                         ../../examples/SharedMemory/PhysicsServer.cpp
                         ../../examples/SharedMemory/PhysicsServer.h

test/SharedMemory/premake4.lua

@@ -43,11 +43,14 @@ project ("Test_PhysicsServerLoopBack")
 			"Bullet3Common",
 			"BulletDynamics", 
 			"BulletCollision", 
+			"BussIK",
 			"LinearMath"
 		}
 			
 		files {
 			"test.c",
+			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+			"../../examples/SharedMemory/IKTrajectoryHelper.h",
 			"../../examples/SharedMemory/PhysicsClient.cpp",
 			"../../examples/SharedMemory/PhysicsClient.h",
 			"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -112,12 +115,15 @@ project ("Test_PhysicsServerLoopBack")
 			"BulletWorldImporter",
 			"Bullet3Common",
 			"BulletDynamics", 
-			"BulletCollision", 
+			"BulletCollision",
+			"BussIK",
 			"LinearMath"
 		}
 			
 		files {
 			"test.c",
+			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+			"../../examples/SharedMemory/IKTrajectoryHelper.h",
 			"../../examples/SharedMemory/PhysicsClient.cpp",
 			"../../examples/SharedMemory/PhysicsClient.h",
 			"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -187,7 +193,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
 --		}
 	hasCL = findOpenCL("clew")
 
-	links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+	links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"}
 	initOpenGL()
 	initGlew()
 
@@ -214,6 +220,8 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
 
 		files {
 			"test.c",
+			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+			"../../examples/SharedMemory/IKTrajectoryHelper.h",
 			"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
 			"../../examples/SharedMemory/InProcessMemory.cpp",
 			"../../examples/SharedMemory/PhysicsClient.cpp",