Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #773 from erwincoumans/master

Browse Source 
VR: support teleporting using buttong, allow multiple controllers to be used, fast wireframe rendering,
This commit is contained in:
erwincoumans
2016-09-09 15:22:12 -07:00
committed by GitHub
parent 00babd507f c5d775a635
commit 30bd1d7362
47 changed files with 1060 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
build3/premake4.lua
View File

@@ -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

4
build_visual_studio.bat
View File

@@ -2,9 +2,9 @@
rem premake4 --with-pe vs2010
rem premake4 --bullet2demos vs2010
cd build3
premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --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
premake4 --enable_pybullet --python_include_dir="c:/python34/include" --python_lib_dir="c:/python34/libs" --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_openvr --enable_pybullet --targetdir="../bin" vs2010
rem premake4 --targetdir="../server2bin" vs2010
rem cd vs2010

1
data/cube_small.urdf
View File

@@ -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"/>

18
data/kiva_shelf/model.sdf
View File

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

BIN
data/multibody.bullet
View File

Binary file not shown.

126
data/pr2_gripper.urdf Normal file
View File

@@ -0,0 +1,126 @@
<?xml version="1.0"?>
<robot name="physics">
<link name="gripper_pole">
<visual>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
<material name="Gray">
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_gripper_joint" type="revolute">
<axis xyz="0 0 -1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
<parent link="gripper_pole"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
</robot>

29
data/samurai.urdf Normal file
View File

@@ -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>

2
examples/CMakeLists.txt
View File

@@ -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)

1
examples/CommonInterfaces/CommonCameraInterface.h
View File

@@ -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;

3
examples/CommonInterfaces/CommonRenderInterface.h
View File

@@ -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;

23
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -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

42
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

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

2
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -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

4
examples/ExampleBrowser/premake4.lua
View File

@@ -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",

33
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

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

3
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -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;
}

11
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -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;

15
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -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]);

6
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@@ -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);

30
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -3,8 +3,10 @@
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Common/b3Transform.h"
struct SimpleCameraInternalData
B3_ATTRIBUTE_ALIGNED16(struct) SimpleCameraInternalData
{
SimpleCameraInternalData()
:m_cameraTargetPosition(b3MakeVector3(0,0,0)),
@@ -19,8 +21,15 @@ struct SimpleCameraInternalData
m_frustumZFar(1000),
m_enableVR(false)
{
b3Transform tr;
tr.setIdentity();
tr.getOpenGLMatrix(m_offsetTransformVR);
}
b3Vector3 m_cameraTargetPosition;
B3_DECLARE_ALIGNED_ALLOCATOR();
B3_ATTRIBUTE_ALIGNED16(float) m_offsetTransformVR[16];
b3Vector3 m_cameraTargetPosition;
float m_cameraDistance;
b3Vector3 m_cameraUp;
b3Vector3 m_cameraForward;
@@ -37,7 +46,7 @@ struct SimpleCameraInternalData
bool m_enableVR;
float m_viewMatrixVR[16];
float m_projectionMatrixVR[16];
};
@@ -244,13 +253,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
{

2
examples/OpenGLWindow/SimpleCamera.h
View File

@@ -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;

38
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -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]);

27
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -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;

32
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -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);
};

15
examples/RoboticsLearning/IKTrajectoryHelper.cpp → examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -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;
}
}

2
examples/RoboticsLearning/IKTrajectoryHelper.h → examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -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);

73
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;
}

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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);

162
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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,9 @@ struct PhysicsServerCommandProcessorInternalData
bool m_allowRealTimeSimulation;
bool m_hasGround;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -389,6 +392,7 @@ struct PhysicsServerCommandProcessorInternalData
btScalar m_numSimulationSubSteps;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
@@ -428,6 +432,8 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData()
:m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(false),
m_commandLogger(0),
m_logPlayback(0),
@@ -552,10 +558,14 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.005;
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -1061,6 +1071,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 +1881,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 +1979,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 +2460,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 +2521,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 +2545,7 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
}
}
btVector3 gLastPickPos(0,0,0);
bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
@@ -2498,6 +2559,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 +2578,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 +2697,108 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
btVector3 gVRGripperPos(0,0,0);
btQuaternion gVRGripperOrn(0,0,0,1);
bool gVRGripperClosed = false;
int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (m_data->m_allowRealTimeSimulation)
if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper)
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.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, &gBufferServerToClient[0], gBufferServerToClient.size());
if (m_data->m_gripperRigidbodyFixed == 0)
{
int bodyId = 0;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.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_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI);
m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
}
}
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime);
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
for (int i = 0; i < 2; i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
if (motor)
{
motor->setErp(0.005);
if (gVRGripperClosed)
{
motor->setPositionTarget(0.2, 1);
}
else
{
motor->setPositionTarget(SIMD_HALF_PI, 1);
}
motor->setVelocityTarget(0, 0.1);
btScalar maxImp = 550.*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
}
}
}
}
}
int maxSteps = 3;
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime);
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps)
{
gNumSteps = numSteps;
gDtInSec = dtInSec;
gSubStep = m_data->m_physicsDeltaTime;
}
}
}

296
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -13,8 +13,14 @@
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
#define MAX_VR_CONTROLLERS 8
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos(0,0,0);
extern bool gVRGripperClosed;
bool gDebugRenderToggle = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
#define MAX_MOTION_NUM_THREADS 1
@@ -82,22 +88,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<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;
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 +119,7 @@ struct MotionThreadLocalStorage
int skip = 0;
int skip1 = 0;
float clampedDeltaTime = 0.2;
void MotionThreadFunc(void* userPtr,void* lsMemory)
{
@@ -127,12 +139,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 +161,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;c<MAX_VR_CONTROLLERS;c++)
{
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)
{
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 +347,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,61 +877,101 @@ 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};
extern int gDroppedSimulationSteps;
extern int gNumSteps;
extern double gDtInSec;
extern double gSubStep;
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;
btMatrix3x3 mat(m_args[0].m_orn);
if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
{
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 toY = m_args[0].m_pos+mat.getColumn(1);
btVector3 toZ = m_args[0].m_pos+mat.getColumn(2);
btVector3 toX = from+mat.getColumn(0);
btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = from+mat.getColumn(2);
int width = 2;
int width = 2;
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);
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)
static int frameCount=0;
frameCount++;
char bla[1024];
static btScalar prevTime = m_clock.getTimeSeconds();
btScalar curTime = m_clock.getTimeSeconds();
static btScalar deltaTime = 0.f;
static int count = 10;
if (count-- < 0)
{
count = 10;
deltaTime = curTime - prevTime;
}
if (deltaTime == 0)
deltaTime = 1000;
frameCount++;
static char line0[1024];
static char line1[1024];
static btScalar worseFps = 1000000;
int numFrames = 200;
static int count = 0;
count++;
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
btScalar fps = 1. / (curTime - prevTime);
prevTime = curTime;
if (fps < worseFps)
{
worseFps = fps;
}
if (count > numFrames)
{
count = 0;
sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2);
sprintf(line1, "Physics Steps = %d, Dropped = %d, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, gDtInSec, gSubStep);
gDroppedSimulationSteps = 0;
worseFps = 1000000;
}
}
prevTime = curTime;
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,17 +980,20 @@ 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;
btVector3 side = viewTrInv.getBasis().getColumn(0);
btVector3 up = viewTrInv.getBasis().getColumn(1);
up+=0.35*side;
m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
up+=0.6*side;
m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
//btVector3 fwd = viewTrInv.getBasis().getColumn(2);
sprintf(bla,"VR line 2 sub-title text test, frame %d", frameCount/2);
upMag = -0.7;
m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
}
//m_args[0].m_cs->unlock();
@@ -952,7 +1027,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 +1100,72 @@ 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 (controllerId == 3 && (button == 33))
{
gVRGripperClosed =state;
}
else
{
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]);
if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
return;
}
if (controllerId == 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);
}
else
{
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]);
}
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

20
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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;
};
};

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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
};

10
examples/SharedMemory/premake4.lua
View File

@@ -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"
}

76
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -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;
@@ -621,6 +625,9 @@ void CMainApplication::Shutdown()
}
}
sExample->exitPhysics();
delete sExample;
delete m_app;
m_app=0;
@@ -668,6 +675,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,7 +697,19 @@ 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 );
///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
//add a special debug drawer that deals with this
//gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints
//btIDebugDraw::DBG_DrawConstraintLimits+
//btIDebugDraw::DBG_DrawConstraints
;
}
sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
}
else
{
@@ -699,16 +719,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);
}
}
}
}
@@ -725,13 +759,11 @@ bool CMainApplication::HandleInput()
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CMainApplication::RunMainLoop()
{
bool bQuit = false;
//SDL_StartTextInput();
//SDL_ShowCursor( SDL_DISABLE );
while ( !bQuit && !m_app->m_window->requestedExit())
{
bQuit = HandleInput();
@@ -739,7 +771,6 @@ void CMainApplication::RunMainLoop()
RenderFrame();
}
//SDL_StopTextInput();
}
@@ -1603,11 +1634,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 +1684,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 );

10
examples/ThirdPartyLibs/BussIK/CMakeLists.txt Normal file
View File

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

2
examples/ThirdPartyLibs/BussIK/Misc.cpp
View File

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

11
examples/ThirdPartyLibs/BussIK/premake4.lua Normal file
View File

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

4
examples/pybullet/CMakeLists.txt
View File

@@ -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})

4
examples/pybullet/premake4.lua
View File

@@ -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",

49
examples/pybullet/pybullet.c
View File

@@ -490,6 +490,7 @@ static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) {
double timeStep = 0.001;
int ret;
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
@@ -507,6 +508,37 @@ static PyObject* 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
// Orientation is returned in quaternions
static int pybullet_internalGetBasePositionAndOrientation(
@@ -1683,9 +1715,20 @@ static PyMethodDef SpamMethods[] = {
"Set the amount of time to proceed at each call to stepSimulation. (unit "
"is seconds, typically range is 0.01 or 0.001)"},
{"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"},
{"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. "
"This value depends on the time step."},
{ "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" },
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Create a multibody by loading a URDF file."},

2
src/Bullet3Common/b3Scalar.h
View File

@@ -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

5
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -26,7 +26,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0)
m_kp(0),
m_erp(1)
{
m_maxAppliedImpulse = maxMotorImpulse;
@@ -123,7 +124,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;

9
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -29,6 +29,7 @@ protected:
btScalar m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
btScalar m_erp;
public:
@@ -57,6 +58,14 @@ public:
m_kp = kp;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{

1
src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
View File

@@ -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,

2
src/LinearMath/btScalar.h
View File

@@ -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

4
test/SharedMemory/CMakeLists.txt
View File

@@ -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

12
test/SharedMemory/premake4.lua
View File

@@ -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",