diff --git a/build3/premake4.lua b/build3/premake4.lua index 3f92bba58..f0de611fd 100644 --- a/build3/premake4.lua +++ b/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 diff --git a/data/cube_small.urdf b/data/cube_small.urdf index f6e271ee3..a0e3cc609 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -5,6 +5,7 @@ + diff --git a/data/kiva_shelf/model.sdf b/data/kiva_shelf/model.sdf index 3e506a9b0..138f88d6d 100644 --- a/data/kiva_shelf/model.sdf +++ b/data/kiva_shelf/model.sdf @@ -2,18 +2,18 @@ 1 - 0 2 0 0 0 0 + 0 1 0 0 0 0 0.0 .0 1.2045 0 0 0 - 76.26 + 0 - 47 - -0.003456 - 0.001474 - 13.075 - -0.014439 - 47 + 0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 @@ -30,7 +30,7 @@ - + 0 0 0 1.5707 0 0 diff --git a/data/multibody.bullet b/data/multibody.bullet index 50ed04b49..19dacc389 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/data/samurai.urdf b/data/samurai.urdf new file mode 100644 index 000000000..f15d47fb6 --- /dev/null +++ b/data/samurai.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8a974c55b..47728c244 100644 --- a/examples/CMakeLists.txt +++ b/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) diff --git a/examples/CommonInterfaces/CommonCameraInterface.h b/examples/CommonInterfaces/CommonCameraInterface.h index 9c1740788..1e9fa2f25 100644 --- a/examples/CommonInterfaces/CommonCameraInterface.h +++ b/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; diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index 0ac09975f..888607dd9 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/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; diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 717d57f28..32f4c18f6 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/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 diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 8001b11e3..ecd1ed45d 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/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 m_rgbaPixelBuffer1; btAlignedObjectArray 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; im_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) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 2fac0313e..eaa72a7be 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/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 diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 2010c744e..5bd7eec42 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/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", diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 992840850..85186069f 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/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;im_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); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 3fee80465..20e6a920f 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/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; } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 25f7e3e38..0ed995f33 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/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; diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 5107ed758..1a8ee2063 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/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(srcIndexm_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]); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index 135f44133..9fae49db5 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/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); diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index 43729b236..a78f4de25 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/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 { diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h index 5a057baf0..7221f01a6 100644 --- a/examples/OpenGLWindow/SimpleCamera.h +++ b/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; diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 52cf68160..5eb89d916 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/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; - m_targetPos.setValue(0.5, 0, 0.5+0.4*b3Sin(3 * m_time)); + float dt = deltaTime; + 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]); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index e7daae828..957e27d5c 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/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; diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 142d406cf..16c379609 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -50,6 +50,7 @@ struct b3RobotSimLoadFileArgs } }; + struct b3RobotSimLoadFileResults { b3AlignedObjectArray 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); }; diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp similarity index 93% rename from examples/RoboticsLearning/IKTrajectoryHelper.cpp rename to examples/SharedMemory/IKTrajectoryHelper.cpp index 2e97653f6..4954f6b10 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp +++ b/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; -} \ No newline at end of file +} diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h similarity index 92% rename from examples/RoboticsLearning/IKTrajectoryHelper.h rename to examples/SharedMemory/IKTrajectoryHelper.h index 5b436057f..6bbd53815 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d57113e3e..e4f027695 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; -} \ No newline at end of file +} + + +///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; +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4d6522576..60a647e9a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 03357e59f..cb5a3b422 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 m_multiBodyJointFeedbacks; btHashMap m_inverseDynamicsBodies; + btHashMap 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 bufferServerToClient; + bufferServerToClient.resize(32768); + + if (!m_data->m_hasGround) { m_data->m_hasGround = true; int bodyId = 0; - btAlignedObjectArray 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); + } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 73901c659..ad5fa0fd3 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/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_isPicking(false), - m_isDragging(false), - m_isReleasing(false) + :m_physicsServerPtr(0) { + for (int i=0;i m_positions; - btVector3 m_pos; - btQuaternion m_orn; - bool m_isPicking; - bool m_isDragging; - bool m_isReleasing; + btVector3 m_vrControllerPos[MAX_VR_CONTROLLERS]; + btQuaternion m_vrControllerOrn[MAX_VR_CONTROLLERS]; + bool m_isVrControllerPicking[MAX_VR_CONTROLLERS]; + bool m_isVrControllerDragging[MAX_VR_CONTROLLERS]; + 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? - - - double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.; + 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; - 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) + for (int c=0;cm_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) - { - args->m_isDragging = false; - args->m_isReleasing = false; - args->m_physicsServerPtr->removePickingConstraint(); - //printf("Release pick\n"); + btVector3 from = args->m_vrControllerPos[c]; + btMatrix3x3 mat(args->m_vrControllerOrn[c]); + + btScalar pickDistance = 1000.; + btVector3 toX = from+mat.getColumn(0); + 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); - args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds); + if (deltaTimeInSeconds>clampedDeltaTime) + { + 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;igetAppInterface()->m_renderer->drawLine(from,toX,color,width); - color=btVector4(0,1,0,1); - m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); - color=btVector4(0,0,1,1); - m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); + btVector4 color; + color=btVector4(1,0,0,1); + m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width); + color=btVector4(0,1,0,1); + m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); + color=btVector4(0,0,1,1); + 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); - m_args[0].m_isReleasing = (state==0); - 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]); + //printf("controllerId %d, button=%d\n",controllerId, button); + + if (controllerId<0 || controllerId>MAX_VR_CONTROLLERS) + 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) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b718d40c7..2039cd40a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 28038a3ad..de76b2a98 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 }; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index ba761dd82..0dd45af08 100644 --- a/examples/SharedMemory/premake4.lua +++ b/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" } diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index e85b8af0e..a9183b43c 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/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 ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0) + if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller ) { 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); - sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn); + // printf("Device RELEASED: %d, button %d\n", unDevice,button); + + //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 ); diff --git a/examples/ThirdPartyLibs/BussIK/CMakeLists.txt b/examples/ThirdPartyLibs/BussIK/CMakeLists.txt new file mode 100644 index 000000000..96dc16025 --- /dev/null +++ b/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}) diff --git a/examples/ThirdPartyLibs/BussIK/Misc.cpp b/examples/ThirdPartyLibs/BussIK/Misc.cpp index 6aa696d55..330997909 100644 --- a/examples/ThirdPartyLibs/BussIK/Misc.cpp +++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp @@ -23,7 +23,7 @@ subject to the following restrictions: #include #include "LinearR3.h" -#include "../OpenGLWindow/OpenGLInclude.h" + /**************************************************************** Axes diff --git a/examples/ThirdPartyLibs/BussIK/premake4.lua b/examples/ThirdPartyLibs/BussIK/premake4.lua new file mode 100644 index 000000000..74a0630a0 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/premake4.lua @@ -0,0 +1,11 @@ + project "BussIK" + + kind "StaticLib" + + includedirs { + "." + } + files { + "*.cpp", + "*.h", + } diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index a5afa5d66..f49c97b25 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/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}) diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index cacb7218e..86b4eedc5 100644 --- a/examples/pybullet/premake4.lua +++ b/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", diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 592d3d4e3..f9fda2967 100644 --- a/examples/pybullet/pybullet.c +++ b/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" }, diff --git a/src/Bullet3Common/b3Scalar.h b/src/Bullet3Common/b3Scalar.h index 60b7f1cfd..16a9ac7a7 100644 --- a/src/Bullet3Common/b3Scalar.h +++ b/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 diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 8d9aa77eb..847e5c6c7 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/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, diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 011fa1392..df907e128 100644 --- a/src/LinearMath/btScalar.h +++ b/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 diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index b7bc9a255..496ca5076 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/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 diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 466ad76a4..1974ba471 100644 --- a/test/SharedMemory/premake4.lua +++ b/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",