From ebe5d9bb849c382da5c2bbcc8da2ed80460d5f34 Mon Sep 17 00:00:00 2001 From: YunfeiBai Date: Fri, 30 Sep 2016 16:47:33 -0700 Subject: [PATCH 1/6] Add null space task as an option for IK in VR. --- data/teddy_vhacd.urdf | 2 +- .../PhysicsServerCommandProcessor.cpp | 49 +++++++++++++++++-- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/data/teddy_vhacd.urdf b/data/teddy_vhacd.urdf index 1868cb9ff..dd6cb2c40 100644 --- a/data/teddy_vhacd.urdf +++ b/data/teddy_vhacd.urdf @@ -2,7 +2,7 @@ - + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 43f76cf60..f0fa8359c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2699,8 +2699,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); } + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); } btVector3DoubleData endEffectorWorldPosition; @@ -3089,7 +3089,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)), btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)), btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)) + btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8)) }; @@ -3116,6 +3117,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); // Shelf area loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); @@ -3134,6 +3136,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_huskyId = bodyId; + //loadUrdf("jz/jz.urdf", btVector3(0, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); @@ -3283,7 +3286,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) } } - int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS; + int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; //IK2_VEL_DLS; btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldOrientation; @@ -3314,6 +3317,46 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) //controllerOrn.serializeDouble(targetWorldOrientation); + if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) + { + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + lower_limit[0] = -2.32; + lower_limit[1] = -1.6; + lower_limit[2] = -2.32; + lower_limit[3] = -1.6; + lower_limit[4] = -2.32; + lower_limit[5] = -1.6; + lower_limit[6] = -2.4; + upper_limit[0] = 2.32; + upper_limit[1] = 1.6; + upper_limit[2] = 2.32; + upper_limit[3] = 1.6; + upper_limit[4] = 2.32; + upper_limit[5] = 1.6; + upper_limit[6] = 2.4; + joint_range[0] = 5.8; + joint_range[1] = 4; + joint_range[2] = 5.8; + joint_range[3] = 4; + joint_range[4] = 5.8; + joint_range[5] = 4; + joint_range[6] = 6; + rest_pose[0] = 0; + rest_pose[1] = 0; + rest_pose[2] = 0; + rest_pose[3] = SIMD_HALF_PI; + rest_pose[4] = 0; + rest_pose[5] = -SIMD_HALF_PI*0.66; + rest_pose[6] = 0; + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, From 0d58ebb311467f745e64df57b38cb9560ccc5472 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 5 Oct 2016 14:24:49 -0700 Subject: [PATCH 2/6] Fix the desired angular velocity in IK to prevent flip motion. --- examples/SharedMemory/IKTrajectoryHelper.cpp | 6 ++++++ examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 1 - 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 89260d47d..13bd8a2ad 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -82,6 +82,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], btQuaternion deltaQ = endQ*startQ.inverse(); float angle = deltaQ.getAngle(); btVector3 axis = deltaQ.getAxis(); + if (angle > PI) { + angle -= 2.0*PI; + } + else if (angle < -PI) { + angle += 2.0*PI; + } float angleDot = angle; btVector3 angularVel = angleDot*axis.normalize(); for (int i = 0; i < 3; ++i) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f0fa8359c..4209631e2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3136,7 +3136,6 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_huskyId = bodyId; - //loadUrdf("jz/jz.urdf", btVector3(0, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); From d59b0f05a43503221b9605aa6ba1afac1dc3bdd1 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 5 Oct 2016 16:31:48 -0700 Subject: [PATCH 3/6] expose b3PhysicsParamSetNumSolverIterations split Visual Studio project generation batch file in regular and VR+pybullet+double precision build. disable KUKA iiwa joint limit hack --- build_visual_studio.bat | 8 ++---- build_visual_studio_vr_pybullet_double.bat | 8 ++++++ data/kuka_iiwa/model.urdf | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 9 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 ++ .../PhysicsServerCommandProcessor.cpp | 28 +++++++++++-------- 6 files changed, 39 insertions(+), 19 deletions(-) create mode 100644 build_visual_studio_vr_pybullet_double.bat diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 302194631..d8a85bc44 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -1,9 +1,5 @@ -IF NOT EXIST bin mkdir bin -IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin cd build3 -premake4 --enable_openvr --targetdir="../bin" vs2010 - -rem premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010 -pause +premake4 --targetdir="../bin" vs2010 +start vs2010 diff --git a/build_visual_studio_vr_pybullet_double.bat b/build_visual_studio_vr_pybullet_double.bat new file mode 100644 index 000000000..71d676172 --- /dev/null +++ b/build_visual_studio_vr_pybullet_double.bat @@ -0,0 +1,8 @@ + +IF NOT EXIST bin mkdir bin +IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin + +cd build3 + +premake4 --double --enable_openvr --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --targetdir="../bin" vs2010 +start vs2010 diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf index c3ef25fd6..a6e53d3ab 100644 --- a/data/kuka_iiwa/model.urdf +++ b/data/kuka_iiwa/model.urdf @@ -174,7 +174,7 @@ - + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 95babb3a6..0e75054cc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -145,6 +145,15 @@ int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandH return 0; } +int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_numSolverIterations = numSolverIterations; + command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; + return 0; +} + int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index dcd68c7d6..b44b9a452 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -101,6 +101,9 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); +int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); + + b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3a68fcc69..8628f8bee 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1490,7 +1490,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } - case CMD_SEND_DESIRED_STATE: + case CMD_SEND_DESIRED_STATE: { if (m_data->m_verboseOutput) { @@ -1670,18 +1670,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } default: - { - b3Warning("m_controlMode not implemented yet"); - break; - } + { + b3Warning("m_controlMode not implemented yet"); + break; + } - } - } + } + } - serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; - hasStatus = true; - break; - } + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; + hasStatus = true; + break; + } case CMD_REQUEST_ACTUAL_STATE: { if (m_data->m_verboseOutput) @@ -1944,6 +1944,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; + } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) { m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; @@ -3351,7 +3355,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { btScalar desiredVelocity = 0.f; btScalar desiredPosition = q_new[link]; - motor->setRhsClamp(gRhsClamp); + //motor->setRhsClamp(gRhsClamp); //printf("link %d: %f", link, q_new[link]); motor->setVelocityTarget(desiredVelocity,1.0); motor->setPositionTarget(desiredPosition,0.6); From efe86c9b415c44c80f2f9369c4d9bea5f9a3169f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 5 Oct 2016 17:22:35 -0700 Subject: [PATCH 4/6] Update README.md --- README.md | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 88d7e4997..fecbeefa4 100644 --- a/README.md +++ b/README.md @@ -2,18 +2,20 @@ [![Travis Build Status](https://api.travis-ci.org/bulletphysics/bullet3.png?branch=master)](https://travis-ci.org/bulletphysics/bullet3) [![Appveyor Build status](https://ci.appveyor.com/api/projects/status/6sly9uxajr6xsstq)](https://ci.appveyor.com/project/erwincoumans/bullet3) -# Bullet 2.x including pybullet and experimental Bullet 3 OpenCL. +# Bullet 2.x including pybullet, Virtual Reality support and +# experimental Bullet 3 OpenCL implementation. This is the official repository of Bullet 2.x, moved from http://bullet.googlecode.com It includes the optional experimental Bullet 3 GPU pipeline. -The Bullet 2 API will stay default and up-to-date while slowly moving to Bullet 3. -The steps towards Bullet 3 are in a nutshell: +The Bullet 2 API will stay default and up-to-date while slowly moving to a new API. +The steps towards a new API is in a nutshell: 1. The old Bullet2 demos are being merged into the examples/ExampleBrowser -2. A new Bullet 3 API is created -3. All Bullet2 functionality is added to Bullet 3. -4. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl +2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h +3. Python bindings in pybullet are on top of this C-API, see examples/pybullet +4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available +5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk @@ -49,6 +51,17 @@ All source code files are licensed under the permissive zlib license Click on build_visual_studio.bat and open build3/vs2010/0MySolution.sln +**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift ** + +Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln +Edit this batch file to choose where Python include/lib directories are located. +You can connect from Python pybullet to the sandbox using: + +``` +import pybullet as p +p.connect(p.SHARED_MEMORY) +``` + **Linux and Mac OSX gnu make** In a terminal type: From 4e9d25a2ef582bd0675835edf69485ba9578d2c8 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 5 Oct 2016 17:25:49 -0700 Subject: [PATCH 5/6] Update README.md --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index fecbeefa4..2e1a67be4 100644 --- a/README.md +++ b/README.md @@ -2,8 +2,7 @@ [![Travis Build Status](https://api.travis-ci.org/bulletphysics/bullet3.png?branch=master)](https://travis-ci.org/bulletphysics/bullet3) [![Appveyor Build status](https://ci.appveyor.com/api/projects/status/6sly9uxajr6xsstq)](https://ci.appveyor.com/project/erwincoumans/bullet3) -# Bullet 2.x including pybullet, Virtual Reality support and -# experimental Bullet 3 OpenCL implementation. +# Bullet 2.x including pybullet, Virtual Reality support This is the official repository of Bullet 2.x, moved from http://bullet.googlecode.com It includes the optional experimental Bullet 3 GPU pipeline. @@ -51,10 +50,11 @@ All source code files are licensed under the permissive zlib license Click on build_visual_studio.bat and open build3/vs2010/0MySolution.sln -**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift ** +**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift** Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln Edit this batch file to choose where Python include/lib directories are located. +Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build. You can connect from Python pybullet to the sandbox using: ``` From 90ae12659c03f5fb0bb6b0d6973147a52ddb85f5 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 5 Oct 2016 17:27:34 -0700 Subject: [PATCH 6/6] Update README.md --- README.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/README.md b/README.md index 2e1a67be4..b5b9737c6 100644 --- a/README.md +++ b/README.md @@ -93,9 +93,6 @@ You can just run it though a terminal/command prompt, or by clicking it. ``` [--start_demo_name="Demo Name"] Start with a selected demo -[--enable_pybullet] Build with pybullet Python bindings. See also examples/pybullet -[--enable_openvr] Build with VR support for HTC Vive and Oculus Rift using OpenVR -[--enable_experimental_opencl] Enable some experimental OpenCL examples [--mp4=moviename.mp4] Create a mp4 movie of the window, requires ffmpeg installed [--mouse_move_multiplier=0.400000] Set the mouse move sensitivity [--mouse_wheel_multiplier=0.01] Set the mouse wheel sensitivity