AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai 2016-09-11 04:11:51 -07:00
commit c952c50ad9
66 changed files with 2812 additions and 1923 deletions
Show Stats 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/InverseDynamics"
include "../examples/ExtendedTutorials" include "../examples/ExtendedTutorials"
include "../examples/SharedMemory" include "../examples/SharedMemory"
include "../examples/ThirdPartyLibs/BussIK"
include "../examples/MultiThreading" include "../examples/MultiThreading"
if _OPTIONS["lua"] then if _OPTIONS["lua"] then

4
build_visual_studio.bat
View File

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

4
data/cube_no_friction.urdf
View File

@ -4,8 +4,8 @@
<contact> <contact>
<lateral_friction value="0.0"/> <lateral_friction value="0.0"/>
<rolling_friction value="0.0"/> <rolling_friction value="0.0"/>
<contact_cfm value="0.0"/> <stiffness value="300.0"/>
<contact_erp value="1.0"/> <damping value="10.0"/>
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>

5
data/cube_small.urdf
View File

@ -3,10 +3,9 @@
<link name="baseLink"> <link name="baseLink">
<contact> <contact>
<lateral_friction value="1.0"/> <lateral_friction value="1.0"/>
<rolling_friction value="0.3"/> <rolling_friction value="0.01"/>
<inertia_scaling value="3.0"/> <inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>

32
data/cube_soft.urdf Normal file
View File

@ -0,0 +1,32 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="300"/>
<damping value="10"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

28
data/husky/husky.urdf
View File

@ -102,6 +102,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
</gazebo> </gazebo>
<!-- Husky wheel macros --> <!-- Husky wheel macros -->
<link name="front_left_wheel_link"> <link name="front_left_wheel_link">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial> <inertial>
<mass value="2.637"/> <mass value="2.637"/>
<origin xyz="0 0 0"/> <origin xyz="0 0 0"/>
@ -146,6 +153,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
</joint> </joint>
</transmission> </transmission>
<link name="front_right_wheel_link"> <link name="front_right_wheel_link">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial> <inertial>
<mass value="2.637"/> <mass value="2.637"/>
<origin xyz="0 0 0"/> <origin xyz="0 0 0"/>
@ -190,6 +204,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
</joint> </joint>
</transmission> </transmission>
<link name="rear_left_wheel_link"> <link name="rear_left_wheel_link">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial> <inertial>
<mass value="2.637"/> <mass value="2.637"/>
<origin xyz="0 0 0"/> <origin xyz="0 0 0"/>
@ -234,6 +255,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
</joint> </joint>
</transmission> </transmission>
<link name="rear_right_wheel_link"> <link name="rear_right_wheel_link">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial> <inertial>
<mass value="2.637"/> <mass value="2.637"/>
<origin xyz="0 0 0"/> <origin xyz="0 0 0"/>

18
data/kiva_shelf/model.sdf
View File

@ -2,18 +2,18 @@
<sdf version="1.4"> <sdf version="1.4">
<model name="Amazon Pod"> <model name="Amazon Pod">
<static>1</static> <static>1</static>
<pose>0 2 0 0 0 0</pose> <pose>0 1 0 0 0 0</pose>
<link name="pod_link"> <link name="pod_link">
<inertial> <inertial>
<pose>0.0 .0 1.2045 0 0 0</pose> <pose>0.0 .0 1.2045 0 0 0</pose>
<mass>76.26</mass> <mass>0</mass>
<inertia> <inertia>
<ixx>47</ixx> <ixx>0</ixx>
<ixy>-0.003456</ixy> <ixy>0.0</ixy>
<ixz>0.001474</ixz> <ixz>0.0</ixz>
<izz>13.075</izz> <izz>0.0</izz>
<iyz>-0.014439</iyz> <iyz>0.0</iyz>
<iyy>47</iyy> <iyy>0.0</iyy>
</inertia> </inertia>
</inertial> </inertial>
@ -30,7 +30,7 @@
</visual> </visual>
<collision name="pod_collision"> <collision concave="yes" name="pod_collision">
<pose>0 0 0 1.5707 0 0</pose> <pose>0 0 0 1.5707 0 0</pose>
<geometry> <geometry>
<mesh> <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 ) SUBDIRS( HelloWorld BasicDemo )
IF(BUILD_BULLET3) IF(BUILD_BULLET3)
SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen OpenGLWindow ) SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK OpenGLWindow )
ENDIF() ENDIF()
IF(BUILD_PYBULLET) IF(BUILD_PYBULLET)
SUBDIRS(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 setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0;
virtual void disableVRCamera()=0; virtual void disableVRCamera()=0;
virtual bool isVRCamera() const =0; virtual bool isVRCamera() const =0;
virtual void setVRCameraOffsetTransform(const float offset[16])=0;
virtual void getCameraTargetPosition(float pos[3]) const = 0; virtual void getCameraTargetPosition(float pos[3]) const = 0;
virtual void getCameraPosition(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 CommonCameraInterface* getActiveCamera()=0;
virtual void setActiveCamera(CommonCameraInterface* cam)=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 getScreenWidth() = 0;
virtual int getScreenHeight() = 0; virtual int getScreenHeight() = 0;

23
examples/ExampleBrowser/CMakeLists.txt
View File

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

42
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@ -146,11 +146,26 @@ struct OpenGLGuiHelperInternalData
struct CommonGraphicsApp* m_glApp; struct CommonGraphicsApp* m_glApp;
class MyDebugDrawer* m_debugDraw; class MyDebugDrawer* m_debugDraw;
GL_ShapeDrawer* m_gl2ShapeDrawer; GL_ShapeDrawer* m_gl2ShapeDrawer;
bool m_vrMode;
int m_vrSkipShadowPass;
btAlignedObjectArray<unsigned char> m_rgbaPixelBuffer1; btAlignedObjectArray<unsigned char> m_rgbaPixelBuffer1;
btAlignedObjectArray<float> m_depthBuffer1; 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) OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2)
@ -269,6 +284,10 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
} }
void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) 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(); int numCollisionObjects = rbWorld->getNumCollisionObjects();
for (int i = 0; i<numCollisionObjects; i++) for (int i = 0; i<numCollisionObjects; i++)
{ {
@ -288,8 +307,25 @@ void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWor
void OpenGLGuiHelper::render(const btDiscreteDynamicsWorld* rbWorld) void OpenGLGuiHelper::render(const btDiscreteDynamicsWorld* rbWorld)
{ {
if (m_data->m_vrMode)
m_data->m_glApp->m_renderer->renderScene(); {
//in VR, we skip the shadow generation for the second eye
if (m_data->m_vrSkipShadowPass>=1)
{
m_data->m_glApp->m_renderer->renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
m_data->m_vrSkipShadowPass=0;
} else
{
m_data->m_glApp->m_renderer->renderScene();
m_data->m_vrSkipShadowPass++;
}
} else
{
m_data->m_glApp->m_renderer->renderScene();
}
//backwards compatible OpenGL2 rendering //backwards compatible OpenGL2 rendering
if (m_data->m_gl2ShapeDrawer && rbWorld) 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); virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size);
void renderInternalGl2(int pass, const btDiscreteDynamicsWorld* dynamicsWorld); void renderInternalGl2(int pass, const btDiscreteDynamicsWorld* dynamicsWorld);
void setVRMode(bool vrMode);
}; };
#endif //OPENGL_GUI_HELPER_H #endif //OPENGL_GUI_HELPER_H

4
examples/ExampleBrowser/premake4.lua
View File

@ -10,7 +10,7 @@ project "App_BulletExampleBrowser"
initOpenCL("clew") initOpenCL("clew")
end 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() initOpenGL()
initGlew() initGlew()
@ -55,6 +55,8 @@ project "App_BulletExampleBrowser"
"../TinyRenderer/our_gl.cpp", "../TinyRenderer/our_gl.cpp",
"../TinyRenderer/TinyRenderer.cpp", "../TinyRenderer/TinyRenderer.cpp",
"../RenderingExamples/TinyRendererSetup.cpp", "../RenderingExamples/TinyRendererSetup.cpp",
"../SharedMemory/IKTrajectoryHelper.cpp",
"../SharedMemory/IKTrajectoryHelper.h",
"../SharedMemory/PhysicsClientC_API.cpp", "../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h", "../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsServerExample.cpp",

35
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[1]*collision->m_geometry.m_meshScale[1],
glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2])); glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2]));
} }
//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
//cylZShape->initializePolyhedralFeatures(); {
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); btTriangleMesh* meshInterface = new btTriangleMesh();
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); for (int i=0;i<glmesh->m_numIndices/3;i++)
cylZShape->setMargin(0.001); {
shape = cylZShape; float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
btVector3(v1[0],v1[1],v1[2]),
btVector3(v2[0],v2[1],v2[2]));
}
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
shape = trimesh;
} else
{
//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
//cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
shape = cylZShape;
}
} else } else
{ {
b3Warning("issue extracting mesh from STL file %s\n", fullPath); b3Warning("issue extracting mesh from STL file %s\n", fullPath);
@ -1001,7 +1020,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
//delete textures //delete textures
for (int i=0;i<textures.size();i++) for (int i=0;i<textures.size();i++)
{ {
delete textures[i].textureData; free( textures[i].textureData);
} }
return graphicsIndex; return graphicsIndex;
} }

5
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@ -410,7 +410,10 @@ void ConvertURDF2BulletInternal(
{ {
col->setRollingFriction(contactInfo.m_rollingFriction); col->setRollingFriction(contactInfo.m_rollingFriction);
} }
if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0)
{
col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping);
}
if (mbLinkIndex>=0) //???? double-check +/- 1 if (mbLinkIndex>=0) //???? double-check +/- 1
{ {

10
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@ -19,7 +19,8 @@ enum URDF_LinkContactFlags
URDF_CONTACT_HAS_ROLLING_FRICTION=2, URDF_CONTACT_HAS_ROLLING_FRICTION=2,
URDF_CONTACT_HAS_INERTIA_SCALING=2, URDF_CONTACT_HAS_INERTIA_SCALING=2,
URDF_CONTACT_HAS_CONTACT_CFM=4, URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_CONTACT_ERP=8 URDF_CONTACT_HAS_CONTACT_ERP=8,
URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
}; };
struct URDFLinkContactInfo struct URDFLinkContactInfo
@ -29,6 +30,9 @@ struct URDFLinkContactInfo
btScalar m_inertiaScaling; btScalar m_inertiaScaling;
btScalar m_contactCfm; btScalar m_contactCfm;
btScalar m_contactErp; btScalar m_contactErp;
btScalar m_contactStiffness;
btScalar m_contactDamping;
int m_flags; int m_flags;
URDFLinkContactInfo() URDFLinkContactInfo()
@ -36,7 +40,9 @@ struct URDFLinkContactInfo
m_rollingFriction(0), m_rollingFriction(0),
m_inertiaScaling(1), m_inertiaScaling(1),
m_contactCfm(0), m_contactCfm(0),
m_contactErp(0) m_contactErp(0),
m_contactStiffness(1e4),
m_contactDamping(1)
{ {
m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION; m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
} }

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

@ -489,6 +489,9 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
if (name_char) if (name_char)
collision.m_name = 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; return true;
} }
@ -647,26 +650,75 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
} }
} }
TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
if (rolling_xml)
{ {
if (m_parseSDF) TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
if (rolling_xml)
{ {
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText()); if (m_parseSDF)
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} else
{
if (!rolling_xml->Attribute("value"))
{ {
logger->reportError("Link/contact: rolling friction element must have value attribute"); link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
return false; link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} else
{
if (!rolling_xml->Attribute("value"))
{
logger->reportError("Link/contact: rolling friction element must have value attribute");
return false;
}
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} }
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} }
} }
{
TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness");
if (stiffness_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
} else
{
if (!stiffness_xml->Attribute("value"))
{
logger->reportError("Link/contact: stiffness element must have value attribute");
return false;
}
link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
}
}
}
{
TiXmlElement *damping_xml = ci->FirstChildElement("damping");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
}
}
}
} }
} }

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

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

21
examples/MultiBody/MultiBodySoftContact.cpp
View File

@ -36,7 +36,6 @@ public:
}; };
extern ContactAddedCallback gContactAddedCallback;
MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper) MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
@ -47,18 +46,7 @@ m_once(true)
MultiBodySoftContact::~MultiBodySoftContact() MultiBodySoftContact::~MultiBodySoftContact()
{ {
gContactAddedCallback = 0;
}
static bool btMultiBodySoftContactCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
cp.m_contactCFM = 0.3;
cp.m_contactERP = 0.2;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP;
return true;
} }
@ -66,7 +54,7 @@ static bool btMultiBodySoftContactCallback(btManifoldPoint& cp, const btCollisio
void MultiBodySoftContact::initPhysics() void MultiBodySoftContact::initPhysics()
{ {
gContactAddedCallback = btMultiBodySoftContactCallback;
int upAxis = 2; int upAxis = 2;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);
@ -109,12 +97,13 @@ void MultiBodySoftContact::initPhysics()
start.setOrigin(groundOrigin); start.setOrigin(groundOrigin);
// start.setRotation(groundOrn); // start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box); btRigidBody* body = createRigidBody(0,start,box);
//setContactStiffnessAndDamping will enable compliant rigid body contact
body->setContactStiffnessAndDamping(300,10);
btVector4 color = colors[curColor]; btVector4 color = colors[curColor];
curColor++; curColor++;
curColor&=3; curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color); m_guiHelper->createRigidBodyGraphicsObject(body,color);
int flags = body->getCollisionFlags();
body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
} }

21
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@ -17,9 +17,9 @@ subject to the following restrictions:
///todo: make this configurable in the gui ///todo: make this configurable in the gui
bool useShadowMap = true;// true;//false;//true; bool useShadowMap = true;// true;//false;//true;
int shadowMapWidth=4096;//8192; int shadowMapWidth= 2048;
int shadowMapHeight= 4096; int shadowMapHeight= 2048;
float shadowMapWorldSize=25; float shadowMapWorldSize=5;
#define MAX_POINTS_IN_BATCH 1024 #define MAX_POINTS_IN_BATCH 1024
#define MAX_LINES_IN_BATCH 1024 #define MAX_LINES_IN_BATCH 1024
@ -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) void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
{ {
m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]); m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
@ -1475,7 +1488,7 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
// m_data->m_shadowMap->disable(); // m_data->m_shadowMap->disable();
// return; // return;
glEnable(GL_CULL_FACE); glEnable(GL_CULL_FACE);
glCullFace(GL_BACK); // Cull back-facing triangles -> draw only front-facing triangles glCullFace(GL_FRONT); // Cull back-facing triangles -> draw only front-facing triangles
b3Assert(glGetError() ==GL_NO_ERROR); b3Assert(glGetError() ==GL_NO_ERROR);
} else } else

6
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@ -41,7 +41,7 @@ class GLInstancingRenderer : public CommonRenderInterface
int m_upAxis; int m_upAxis;
bool m_enableBlend; bool m_enableBlend;
void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
@ -52,6 +52,7 @@ public:
virtual void init(); virtual void init();
virtual void renderScene(); virtual void renderScene();
virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
void InitShaders(); void InitShaders();
void CleanupShaders(); void CleanupShaders();
@ -73,6 +74,7 @@ public:
void writeTransforms(); void writeTransforms();
virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex); virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex);
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* 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 writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
virtual void writeSingleInstanceColorToCPU(float* color, 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/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h" #include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Matrix3x3.h" #include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Common/b3Transform.h"
struct SimpleCameraInternalData
B3_ATTRIBUTE_ALIGNED16(struct) SimpleCameraInternalData
{ {
SimpleCameraInternalData() SimpleCameraInternalData()
:m_cameraTargetPosition(b3MakeVector3(0,0,0)), :m_cameraTargetPosition(b3MakeVector3(0,0,0)),
@ -19,8 +21,15 @@ struct SimpleCameraInternalData
m_frustumZFar(1000), m_frustumZFar(1000),
m_enableVR(false) 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; float m_cameraDistance;
b3Vector3 m_cameraUp; b3Vector3 m_cameraUp;
b3Vector3 m_cameraForward; b3Vector3 m_cameraForward;
@ -37,7 +46,7 @@ struct SimpleCameraInternalData
bool m_enableVR; bool m_enableVR;
float m_viewMatrixVR[16]; float m_viewMatrixVR[16];
float m_projectionMatrixVR[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); 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 void SimpleCamera::getCameraViewMatrix(float viewMatrix[16]) const
{ {
if (m_data->m_enableVR) if (m_data->m_enableVR)
{ {
for (int i=0;i<16;i++) 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 } 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 getCameraViewMatrix(float m[16]) const;
virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]); virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]);
virtual void setVRCameraOffsetTransform(const float offset[16]);
virtual void disableVRCamera(); virtual void disableVRCamera();
virtual bool isVRCamera() const; virtual bool isVRCamera() const;
virtual void getCameraTargetPosition(float pos[3]) const; virtual void getCameraTargetPosition(float pos[3]) const;

20
examples/RigidBody/RigidBodySoftContact.cpp
View File

@ -29,7 +29,7 @@ subject to the following restrictions:
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" #include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
extern ContactAddedCallback gContactAddedCallback;
struct RigidBodySoftContact : public CommonRigidBodyBase struct RigidBodySoftContact : public CommonRigidBodyBase
@ -40,7 +40,7 @@ struct RigidBodySoftContact : public CommonRigidBodyBase
} }
virtual ~RigidBodySoftContact() virtual ~RigidBodySoftContact()
{ {
gContactAddedCallback = 0;
} }
virtual void initPhysics(); virtual void initPhysics();
virtual void renderScene(); virtual void renderScene();
@ -55,21 +55,12 @@ struct RigidBodySoftContact : public CommonRigidBodyBase
}; };
static bool btRigidBodySoftContactCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
cp.m_contactCFM = 0.3;
cp.m_contactERP = 0.2;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP;
return true;
}
void RigidBodySoftContact::initPhysics() void RigidBodySoftContact::initPhysics()
{ {
gContactAddedCallback = btRigidBodySoftContactCallback;
m_guiHelper->setUpAxis(1); m_guiHelper->setUpAxis(1);
@ -120,8 +111,9 @@ void RigidBodySoftContact::initPhysics()
{ {
btScalar mass(0.); btScalar mass(0.);
btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
int flags = body->getCollisionFlags();
body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); body->setContactStiffnessAndDamping(300,10);
} }

2
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@ -153,7 +153,7 @@ public:
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setGravity(b3MakeVector3(0,0,-10));
//m_robotSim.setNumSimulationSubSteps(4); m_robotSim.setNumSimulationSubSteps(4);
} }
if ((m_options & eTWO_POINT_GRASP)!=0) if ((m_options & eTWO_POINT_GRASP)!=0)

31
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@ -1,6 +1,6 @@
#include "KukaGraspExample.h" #include "KukaGraspExample.h"
#include "IKTrajectoryHelper.h" #include "../SharedMemory/IKTrajectoryHelper.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h" #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h" #include "Bullet3Common/b3Quaternion.h"
@ -140,8 +140,11 @@ public:
} }
virtual void stepSimulation(float deltaTime) virtual void stepSimulation(float deltaTime)
{ {
m_time+=deltaTime; float dt = deltaTime;
m_targetPos.setValue(0.5, 0, 0.5+0.4*b3Sin(3 * m_time)); btClamp(dt,0.0001f,0.01f);
m_time+=dt;
m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
@ -174,6 +177,28 @@ public:
m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular); m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular);
// m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo); // 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]; double q_new[7];
int ikMethod=IK2_DLS; int ikMethod=IK2_DLS;
b3Vector3DoubleData dataOut; b3Vector3DoubleData dataOut;

2
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@ -117,7 +117,7 @@ public:
b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results; b3RobotSimLoadFileResults results;
{ {
args.m_fileName = "cube.urdf"; args.m_fileName = "cube_soft.urdf";
args.m_startPosition.setValue(0,0,2.5); args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results); m_robotSim.loadFile(args,results);

27
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@ -469,6 +469,33 @@ void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps)
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); 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() b3RobotSimAPI::~b3RobotSimAPI()
{ {
delete m_data; delete m_data;

32
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@ -50,6 +50,7 @@ struct b3RobotSimLoadFileArgs
} }
}; };
struct b3RobotSimLoadFileResults struct b3RobotSimLoadFileResults
{ {
b3AlignedObjectArray<int> m_uniqueObjectIds; 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 class b3RobotSimAPI
{ {
@ -113,6 +143,8 @@ public:
void setNumSimulationSubSteps(int numSubSteps); void setNumSimulationSubSteps(int numSubSteps);
bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results);
void renderScene(); void renderScene();
void debugDraw(int debugDrawMode); void debugDraw(int debugDrawMode);

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

@ -5,11 +5,11 @@
#include "BussIK/VectorRn.h" #include "BussIK/VectorRn.h"
#include "BussIK/MatrixRmn.h" #include "BussIK/MatrixRmn.h"
#include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3AlignedObjectArray.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#define RADIAN(X) ((X)*RadiansToDegrees) #define RADIAN(X) ((X)*RadiansToDegrees)
//use BussIK and Reflexxes to convert from Cartesian endeffector future target to //use BussIK and Reflexxes to convert from Cartesian endeffector future target to
//joint space positions at each real-time (simulation) step //joint space positions at each real-time (simulation) step
struct IKTrajectoryHelperInternalData struct IKTrajectoryHelperInternalData
@ -37,6 +37,17 @@ IKTrajectoryHelper::~IKTrajectoryHelper()
delete m_data; 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() void IKTrajectoryHelper::createKukaIIWA()
{ {
@ -166,4 +177,4 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
q_new[i] = m_data->m_ikNodes[i]->GetTheta(); q_new[i] = m_data->m_ikNodes[i]->GetTheta();
} }
return true; 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 ///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA(); void createKukaIIWA();
bool createFromMultiBody(class btMultiBody* mb);
bool computeIK(const double endEffectorTargetPosition[3], bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3], const double endEffectorWorldPosition[3],
const double* q_old, int numQ, const double* q_old, int numQ,

70
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -163,6 +163,19 @@ int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int
return 0; 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) b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{ {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;
@ -1303,4 +1316,61 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
} }
return true; 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;
} }

10
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -89,6 +89,7 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
@ -119,6 +120,15 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian); int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
///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); b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead

162
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -15,7 +15,7 @@
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "LinearMath/btHashMap.h" #include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp" #include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "IKTrajectoryHelper.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h" #include "LinearMath/btTransform.h"
@ -381,6 +381,9 @@ struct PhysicsServerCommandProcessorInternalData
bool m_allowRealTimeSimulation; bool m_allowRealTimeSimulation;
bool m_hasGround; bool m_hasGround;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
CommandLogger* m_commandLogger; CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback; CommandLogPlayback* m_logPlayback;
@ -389,6 +392,7 @@ struct PhysicsServerCommandProcessorInternalData
btScalar m_numSimulationSubSteps; btScalar m_numSimulationSubSteps;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks; btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies; btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
@ -428,6 +432,8 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData() PhysicsServerCommandProcessorInternalData()
:m_hasGround(false), :m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(false), m_allowRealTimeSimulation(false),
m_commandLogger(0), m_commandLogger(0),
m_logPlayback(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); 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_remoteDebugDrawer = new SharedMemoryDebugDrawer();
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.005;
} }
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@ -1061,6 +1071,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//no timestamp yet //no timestamp yet
int timeStamp = 0; int timeStamp = 0;
//catch uninitialized cases
serverStatusOut.m_type = CMD_INVALID_STATUS;
//consume the command //consume the command
switch (clientCmd.m_type) switch (clientCmd.m_type)
@ -1868,6 +1881,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; 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; SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true; hasStatus = true;
@ -1961,6 +1979,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true; hasStatus = true;
m_data->m_hasGround = false; m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
break; break;
} }
case CMD_CREATE_RIGID_BODY: case CMD_CREATE_RIGID_BODY:
@ -2516,9 +2535,50 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
} }
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true; hasStatus = true;
break; 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: default:
{ {
b3Error("Unknown command encountered"); b3Error("Unknown command encountered");
@ -2536,13 +2596,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
return hasStatus; return hasStatus;
} }
static int skip=1;
void PhysicsServerCommandProcessor::renderScene() void PhysicsServerCommandProcessor::renderScene()
{ {
if (m_data->m_guiHelper) if (m_data->m_guiHelper)
{ {
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
m_data->m_guiHelper->render(m_data->m_dynamicsWorld); m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
} }
@ -2560,6 +2620,7 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
} }
} }
btVector3 gLastPickPos(0,0,0);
bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{ {
@ -2573,6 +2634,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
{ {
btVector3 pickPos = rayCallback.m_hitPointWorld; btVector3 pickPos = rayCallback.m_hitPointWorld;
gLastPickPos = pickPos;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body) if (body)
{ {
@ -2591,6 +2653,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//very weak constraint for picking //very weak constraint for picking
p2p->m_setting.m_tau = 0.001f; p2p->m_setting.m_tau = 0.001f;
} }
} else } else
{ {
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
@ -2709,23 +2772,108 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb; 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) 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) if (!m_data->m_hasGround)
{ {
m_data->m_hasGround = true; m_data->m_hasGround = true;
int bodyId = 0; 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;
}
} }
} }

297
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -13,8 +13,14 @@
#include "../Utils/b3Clock.h" #include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.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 MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc(); void* MotionlsMemoryFunc();
#define MAX_MOTION_NUM_THREADS 1 #define MAX_MOTION_NUM_THREADS 1
@ -82,22 +88,27 @@ b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
struct MotionArgs struct MotionArgs
{ {
MotionArgs() MotionArgs()
:m_physicsServerPtr(0), :m_physicsServerPtr(0)
m_isPicking(false),
m_isDragging(false),
m_isReleasing(false)
{ {
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
m_isVrControllerPicking[i] = false;
m_isVrControllerDragging[i] = false;
m_isVrControllerReleasing[i] = false;
m_isVrControllerTeleporting[i] = false;
}
} }
b3CriticalSection* m_cs; b3CriticalSection* m_cs;
PhysicsServerSharedMemory* m_physicsServerPtr; PhysicsServerSharedMemory* m_physicsServerPtr;
b3AlignedObjectArray<b3Vector3> m_positions; b3AlignedObjectArray<b3Vector3> m_positions;
btVector3 m_pos; btVector3 m_vrControllerPos[MAX_VR_CONTROLLERS];
btQuaternion m_orn; btQuaternion m_vrControllerOrn[MAX_VR_CONTROLLERS];
bool m_isPicking; bool m_isVrControllerPicking[MAX_VR_CONTROLLERS];
bool m_isDragging; bool m_isVrControllerDragging[MAX_VR_CONTROLLERS];
bool m_isReleasing; bool m_isVrControllerReleasing[MAX_VR_CONTROLLERS];
bool m_isVrControllerTeleporting[MAX_VR_CONTROLLERS];
}; };
@ -108,6 +119,7 @@ struct MotionThreadLocalStorage
int skip = 0; int skip = 0;
int skip1 = 0; int skip1 = 0;
float clampedDeltaTime = 0.2;
void MotionThreadFunc(void* userPtr,void* lsMemory) void MotionThreadFunc(void* userPtr,void* lsMemory)
{ {
@ -127,18 +139,18 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_cs->unlock(); args->m_cs->unlock();
double deltaTimeInSeconds = 0;
do do
{ {
//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread? deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.;
double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
if (deltaTimeInSeconds<(1./5000.)) if (deltaTimeInSeconds<(1./5000.))
{ {
skip++; skip++;
skip1++; skip1++;
if (skip1>5)
{ {
b3Clock::usleep(250); b3Clock::usleep(250);
} }
@ -149,40 +161,58 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
//process special controller commands, such as //process special controller commands, such as
//VR controller button press/release and controller motion //VR controller button press/release and controller motion
btVector3 from = args->m_pos; for (int c=0;c<MAX_VR_CONTROLLERS;c++)
btMatrix3x3 mat(args->m_orn);
btScalar pickDistance = 100.;
btVector3 toX = args->m_pos+mat.getColumn(0);
btVector3 toY = args->m_pos+mat.getColumn(1);
btVector3 toZ = args->m_pos+mat.getColumn(2)*pickDistance;
if (args->m_isPicking)
{ {
args->m_isPicking = false;
args->m_isDragging = true;
args->m_physicsServerPtr->pickBody(from,-toZ);
//printf("PICK!\n");
}
if (args->m_isDragging)
{
args->m_physicsServerPtr->movePickedBody(from,-toZ);
// printf(".");
}
if (args->m_isReleasing) btVector3 from = args->m_vrControllerPos[c];
{ btMatrix3x3 mat(args->m_vrControllerOrn[c]);
args->m_isDragging = false;
args->m_isReleasing = false; btScalar pickDistance = 1000.;
args->m_physicsServerPtr->removePickingConstraint(); btVector3 toX = from+mat.getColumn(0);
//printf("Release pick\n"); btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = from+mat.getColumn(2)*pickDistance;
if (args->m_isVrControllerTeleporting[c])
{
args->m_isVrControllerTeleporting[c] = false;
args->m_physicsServerPtr->pickBody(from,-toZ);
args->m_physicsServerPtr->removePickingConstraint();
}
if (args->m_isVrControllerPicking[c])
{
args->m_isVrControllerPicking[c] = false;
args->m_isVrControllerDragging[c] = true;
args->m_physicsServerPtr->pickBody(from,-toZ);
//printf("PICK!\n");
}
if (args->m_isVrControllerDragging[c])
{
args->m_physicsServerPtr->movePickedBody(from,-toZ);
// printf(".");
}
if (args->m_isVrControllerReleasing[c])
{
args->m_isVrControllerDragging[c] = false;
args->m_isVrControllerReleasing[c] = false;
args->m_physicsServerPtr->removePickingConstraint();
//printf("Release pick\n");
}
} }
//don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc) //don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
btClamp(deltaTimeInSeconds,0.,0.1); if (deltaTimeInSeconds>clampedDeltaTime)
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds); {
deltaTimeInSeconds = clampedDeltaTime;
b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
}
clock.reset(); clock.reset();
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
deltaTimeInSeconds = 0;
} }
args->m_physicsServerPtr->processClientCommands(); args->m_physicsServerPtr->processClientCommands();
@ -317,7 +347,10 @@ public:
m_childGuiHelper->render(0); 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) virtual int registerTexture(const unsigned char* texels, int width, int height)
{ {
@ -844,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() void PhysicsServerExample::renderScene()
{ {
///debug rendering ///debug rendering
//m_args[0].m_cs->lock(); //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(); m_physicsServer.renderScene();
if (m_args[0].m_isPicking || m_args[0].m_isDragging) for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{ {
btVector3 from = m_args[0].m_pos; if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
btMatrix3x3 mat(m_args[0].m_orn); {
btVector3 from = m_args[0].m_vrControllerPos[i];
btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]);
btVector3 toX = m_args[0].m_pos+mat.getColumn(0); btVector3 toX = from+mat.getColumn(0);
btVector3 toY = m_args[0].m_pos+mat.getColumn(1); btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = m_args[0].m_pos+mat.getColumn(2); btVector3 toZ = from+mat.getColumn(2);
int width = 2; int width = 2;
btVector4 color; btVector4 color;
color=btVector4(1,0,0,1); color=btVector4(1,0,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
color=btVector4(0,1,0,1); color=btVector4(0,1,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
color=btVector4(0,0,1,1); color=btVector4(0,0,1,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
}
} }
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{ {
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
static int frameCount=0; static int frameCount=0;
frameCount++;
char bla[1024];
static btScalar prevTime = m_clock.getTimeSeconds(); static btScalar prevTime = m_clock.getTimeSeconds();
btScalar curTime = m_clock.getTimeSeconds(); frameCount++;
static btScalar deltaTime = 0.f; static char line0[1024];
static int count = 10; static char line1[1024];
if (count-- < 0)
{ static btScalar worseFps = 1000000;
count = 10; int numFrames = 200;
deltaTime = curTime - prevTime; static int count = 0;
} count++;
if (deltaTime == 0)
deltaTime = 1000; 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]; float pos[4];
m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
pos[0]+=gVRTeleportPos[0];
pos[1]+=gVRTeleportPos[1];
pos[2]+=gVRTeleportPos[2];
btTransform viewTr; btTransform viewTr;
btScalar m[16]; btScalar m[16];
float mf[16]; float mf[16];
@ -907,17 +980,20 @@ void PhysicsServerExample::renderScene()
{ {
m[i] = mf[i]; m[i] = mf[i];
} }
m[12]=+gVRTeleportPos[0];
m[13]=+gVRTeleportPos[1];
m[14]=+gVRTeleportPos[2];
viewTr.setFromOpenGLMatrix(m); viewTr.setFromOpenGLMatrix(m);
btTransform viewTrInv = viewTr.inverse(); btTransform viewTrInv = viewTr.inverse();
float upMag = -.6; float upMag = -.6;
btVector3 side = viewTrInv.getBasis().getColumn(0); btVector3 side = viewTrInv.getBasis().getColumn(0);
btVector3 up = viewTrInv.getBasis().getColumn(1); btVector3 up = viewTrInv.getBasis().getColumn(1);
up+=0.35*side; up+=0.6*side;
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(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
//btVector3 fwd = viewTrInv.getBasis().getColumn(2); //btVector3 fwd = viewTrInv.getBasis().getColumn(2);
sprintf(bla,"VR line 2 sub-title text test, frame %d", frameCount/2);
upMag = -0.7; 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(); //m_args[0].m_cs->unlock();
@ -951,7 +1027,7 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y)
btVector3 camPos,camTarget; btVector3 camPos,camTarget;
renderer->getActiveCamera()->getCameraPosition(camPos); renderer->getActiveCamera()->getCameraPosition(camPos);
renderer->getActiveCamera()->getCameraTargetPosition(camTarget); renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
btVector3 rayFrom = camPos; btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget-camPos); btVector3 rayForward = (camTarget-camPos);
rayForward.normalize(); rayForward.normalize();
@ -1024,17 +1100,72 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt
} }
void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4]) void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
{ {
m_args[0].m_isPicking = (state!=0); //printf("controllerId %d, button=%d\n",controllerId, button);
m_args[0].m_isReleasing = (state==0);
m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]); if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS)
m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]); return;
if (button==1 && state==0)
{
gVRTeleportPos = gLastPickPos;
}
if (button==32 && state==0)
{
gDebugRenderToggle = !gDebugRenderToggle;
}
if (button==1)
{
m_args[0].m_isVrControllerTeleporting[controllerId] = true;
}
if (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]) 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) B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

19
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -223,6 +223,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4, SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4,
SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8, SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16, 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. ///Controlling a robot involves sending the desired state to its joint motor controllers.
@ -234,6 +235,7 @@ struct SendPhysicsSimulationParameters
int m_numSimulationSubSteps; int m_numSimulationSubSteps;
int m_numSolverIterations; int m_numSolverIterations;
bool m_allowRealTimeSimulation; bool m_allowRealTimeSimulation;
double m_defaultContactERP;
}; };
struct RequestActualStateArgs struct RequestActualStateArgs
@ -389,6 +391,21 @@ struct CalculateJacobianResultArgs
double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM]; double m_angularJacobian[3*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 struct CreateJointArgs
{ {
int m_parentBodyIndex; int m_parentBodyIndex;
@ -431,6 +448,7 @@ struct SharedMemoryCommand
struct CalculateJacobianArgs m_calculateJacobianArguments; struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CreateJointArgs m_createJointArguments; struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments; struct RequestContactDataArgs m_requestContactPointArguments;
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
}; };
}; };
@ -464,6 +482,7 @@ struct SharedMemoryStatus
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs; struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct CalculateJacobianResultArgs m_jacobianResultArgs; struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct SendContactDataArgs m_sendContactPointArgs; struct SendContactDataArgs m_sendContactPointArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
}; };
}; };

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -72,6 +72,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATED_JACOBIAN_FAILED, CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED, CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED, CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_MAX_SERVER_COMMANDS CMD_MAX_SERVER_COMMANDS
}; };

10
examples/SharedMemory/premake4.lua
View File

@ -10,13 +10,15 @@ end
includedirs {".","../../src", "../ThirdPartyLibs",} includedirs {".","../../src", "../ThirdPartyLibs",}
links { links {
"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath" "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
} }
language "C++" language "C++"
myfiles = myfiles =
{ {
"IKTrajectoryHelper.cpp",
"IKTrajectoryHelper.h",
"PhysicsClient.cpp", "PhysicsClient.cpp",
"PhysicsClientSharedMemory.cpp", "PhysicsClientSharedMemory.cpp",
"PhysicsClientExample.cpp", "PhysicsClientExample.cpp",
@ -134,10 +136,10 @@ else
end end
defines {"B3_USE_STANDALONE_EXAMPLE"} defines {"B3_USE_STANDALONE_EXAMPLE"}
includedirs {"../../src"} includedirs {"../../src", "../ThirdPartyLibs"}
links { links {
"BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common" "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common","BussIK"
} }
initOpenGL() initOpenGL()
initGlew() initGlew()
@ -211,7 +213,7 @@ if os.is("Windows") then
} }
links { 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 "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "LinearMath/btIDebugDraw.h"
int gSharedMemoryKey = -1; int gSharedMemoryKey = -1;
int gDebugDrawFlags = 0;
//how can you try typing on a keyboard, without seeing it? //how can you try typing on a keyboard, without seeing it?
//it is pretty funny, to see the desktop in VR! //it is pretty funny, to see the desktop in VR!
@ -32,7 +34,7 @@ int gSharedMemoryKey = -1;
CommonExampleInterface* sExample; CommonExampleInterface* sExample;
int sPrevPacketNum=0; int sPrevPacketNum=0;
GUIHelperInterface* sGuiPtr = 0; OpenGLGuiHelper* sGuiPtr = 0;
static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 }; static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 };
@ -384,6 +386,8 @@ bool CMainApplication::BInit()
sGuiPtr = new OpenGLGuiHelper(m_app,false); sGuiPtr = new OpenGLGuiHelper(m_app,false);
sGuiPtr->setVRMode(true);
//sGuiPtr = new DummyGUIHelper; //sGuiPtr = new DummyGUIHelper;
@ -467,7 +471,7 @@ bool CMainApplication::BInit()
m_fScaleSpacing = 4.0f; m_fScaleSpacing = 4.0f;
m_fNearClip = 0.1f; m_fNearClip = 0.1f;
m_fFarClip = 30.0f; m_fFarClip = 3000.0f;
m_iTexture = 0; m_iTexture = 0;
m_uiVertcount = 0; m_uiVertcount = 0;
@ -621,6 +625,9 @@ void CMainApplication::Shutdown()
} }
} }
sExample->exitPhysics();
delete sExample;
delete m_app; delete m_app;
m_app=0; m_app=0;
@ -668,6 +675,7 @@ bool CMainApplication::HandleInput()
{ {
//we need to have the 'move' events, so no early out here //we need to have the 'move' events, so no early out here
//if (sPrevStates[unDevice].unPacketNum != state.unPacketNum) //if (sPrevStates[unDevice].unPacketNum != state.unPacketNum)
if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
{ {
sPrevStates[unDevice].unPacketNum = state.unPacketNum; sPrevStates[unDevice].unPacketNum = state.unPacketNum;
@ -689,7 +697,19 @@ bool CMainApplication::HandleInput()
if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0) if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0)
{ {
// printf("Device PRESSED: %d, button %d\n", unDevice, button); // 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); sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
} }
else else
{ {
@ -699,16 +719,30 @@ bool CMainApplication::HandleInput()
} }
else else
{ {
//not pressed now, but pressed before -> raise a button up event if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0)
{ {
b3Transform tr; b3Transform tr;
getControllerTransform(unDevice, tr); getControllerTransform(unDevice, tr);
float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] }; float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] };
b3Quaternion born = tr.getRotation(); b3Quaternion born = tr.getRotation();
float orn[4] = { born[0], born[1], born[2], born[3] }; float orn[4] = { born[0], born[1], born[2], born[3] };
// printf("Device RELEASED: %d, button %d\n", unDevice,button); // printf("Device RELEASED: %d, button %d\n", unDevice,button);
sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn);
//not pressed now, but pressed before -> raise a button up event
if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0)
{
if (button==2)
{
gDebugDrawFlags = 0;
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL);
}
sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn);
} else
{
sExample->vrControllerMoveCallback(unDevice, pos, orn);
}
} }
} }
} }
@ -725,13 +759,11 @@ bool CMainApplication::HandleInput()
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
// Purpose: // Purpose:
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void CMainApplication::RunMainLoop() void CMainApplication::RunMainLoop()
{ {
bool bQuit = false; bool bQuit = false;
//SDL_StartTextInput();
//SDL_ShowCursor( SDL_DISABLE );
while ( !bQuit && !m_app->m_window->requestedExit()) while ( !bQuit && !m_app->m_window->requestedExit())
{ {
bQuit = HandleInput(); bQuit = HandleInput();
@ -739,7 +771,6 @@ void CMainApplication::RunMainLoop()
RenderFrame(); RenderFrame();
} }
//SDL_StopTextInput();
} }
@ -1603,11 +1634,19 @@ void CMainApplication::RenderStereoTargets()
m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId); m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId);
sExample->renderScene(); if (gDebugDrawFlags)
{
sExample->physicsDebugDraw(gDebugDrawFlags);
}
{
sExample->renderScene();
}
//m_app->m_instancingRenderer->renderScene(); //m_app->m_instancingRenderer->renderScene();
DrawGridData gridUp; DrawGridData gridUp;
gridUp.upAxis = m_app->getUpAxis(); gridUp.upAxis = m_app->getUpAxis();
m_app->drawGrid(gridUp); // m_app->drawGrid(gridUp);
glBindFramebuffer( GL_FRAMEBUFFER, 0 ); glBindFramebuffer( GL_FRAMEBUFFER, 0 );
@ -1645,8 +1684,17 @@ void CMainApplication::RenderStereoTargets()
m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId); m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId);
//m_app->m_renderer->renderScene(); //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 ); 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 <math.h>
#include "LinearR3.h" #include "LinearR3.h"
#include "../OpenGLWindow/OpenGLInclude.h"
/**************************************************************** /****************************************************************
Axes Axes

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

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

2
examples/TinyRenderer/our_gl.cpp
View File

@ -59,7 +59,7 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) {
return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator
} }
void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix, int objectIndex) void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix)
{ {
triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0); triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0);
} }

4
examples/pybullet/CMakeLists.txt
View File

@ -8,6 +8,8 @@ INCLUDE_DIRECTORIES(
SET(pybullet_SRCS SET(pybullet_SRCS
pybullet.c pybullet.c
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h ../../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 VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${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"} defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew") 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() initOpenGL()
initGlew() initGlew()
@ -36,6 +36,8 @@ project ("pybullet")
files { files {
"pybullet.c", "pybullet.c",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",

3121
examples/pybullet/pybullet.c
View File

File diff suppressed because it is too large Load Diff

2
src/Bullet3Common/b3Scalar.h
View File

@ -55,7 +55,7 @@ inline int b3GetVersion()
//#define B3_HAS_ALIGNED_ALLOCATOR //#define B3_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning #pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl 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 // #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define B3_FORCE_INLINE __forceinline #define B3_FORCE_INLINE __forceinline

4
src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
View File

@ -33,6 +33,8 @@ btCollisionObject::btCollisionObject()
m_friction(btScalar(0.5)), m_friction(btScalar(0.5)),
m_restitution(btScalar(0.)), m_restitution(btScalar(0.)),
m_rollingFriction(0.0f), m_rollingFriction(0.0f),
m_contactDamping(.1),
m_contactStiffness(1e4),
m_internalType(CO_COLLISION_OBJECT), m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0), m_userObjectPointer(0),
m_userIndex2(-1), m_userIndex2(-1),
@ -92,6 +94,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
dataOut->m_deactivationTime = m_deactivationTime; dataOut->m_deactivationTime = m_deactivationTime;
dataOut->m_friction = m_friction; dataOut->m_friction = m_friction;
dataOut->m_rollingFriction = m_rollingFriction; dataOut->m_rollingFriction = m_rollingFriction;
dataOut->m_contactDamping = m_contactDamping;
dataOut->m_contactStiffness = m_contactStiffness;
dataOut->m_restitution = m_restitution; dataOut->m_restitution = m_restitution;
dataOut->m_internalType = m_internalType; dataOut->m_internalType = m_internalType;

38
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@ -86,6 +86,10 @@ protected:
btScalar m_friction; btScalar m_friction;
btScalar m_restitution; btScalar m_restitution;
btScalar m_rollingFriction; btScalar m_rollingFriction;
btScalar m_contactDamping;
btScalar m_contactStiffness;
///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
///do not assign your own m_internalType unless you write a new dynamics object class. ///do not assign your own m_internalType unless you write a new dynamics object class.
@ -129,7 +133,8 @@ public:
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
CF_CHARACTER_OBJECT = 16, CF_CHARACTER_OBJECT = 16,
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128
}; };
enum CollisionObjectTypes enum CollisionObjectTypes
@ -319,7 +324,31 @@ public:
return m_rollingFriction; return m_rollingFriction;
} }
void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
{
m_updateRevision++;
m_contactStiffness = stiffness;
m_contactDamping = damping;
m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING;
//avoid divisions by zero...
if (m_contactStiffness< SIMD_EPSILON)
{
m_contactStiffness = SIMD_EPSILON;
}
}
btScalar getContactStiffness() const
{
return m_contactStiffness;
}
btScalar getContactDamping() const
{
return m_contactDamping;
}
///reserved for Bullet internal usage ///reserved for Bullet internal usage
int getInternalType() const int getInternalType() const
{ {
@ -541,6 +570,8 @@ struct btCollisionObjectDoubleData
double m_deactivationTime; double m_deactivationTime;
double m_friction; double m_friction;
double m_rollingFriction; double m_rollingFriction;
double m_contactDamping;
double m_contactStiffness;
double m_restitution; double m_restitution;
double m_hitFraction; double m_hitFraction;
double m_ccdSweptSphereRadius; double m_ccdSweptSphereRadius;
@ -574,7 +605,8 @@ struct btCollisionObjectFloatData
float m_deactivationTime; float m_deactivationTime;
float m_friction; float m_friction;
float m_rollingFriction; float m_rollingFriction;
float m_contactDamping;
float m_contactStiffness;
float m_restitution; float m_restitution;
float m_hitFraction; float m_hitFraction;
float m_ccdSweptSphereRadius; float m_ccdSweptSphereRadius;

27
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@ -25,7 +25,7 @@ ContactAddedCallback gContactAddedCallback=0;
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
{ {
btScalar friction = body0->getRollingFriction() * body1->getRollingFriction(); btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
@ -58,6 +58,22 @@ btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject*
return body0->getRestitution() * body1->getRestitution(); return body0->getRestitution() * body1->getRestitution();
} }
btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1)
{
return body0->getContactDamping() + body1->getContactDamping();
}
btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1)
{
btScalar s0 = body0->getContactStiffness();
btScalar s1 = body1->getContactStiffness();
btScalar tmp0 = btScalar(1)/s0;
btScalar tmp1 = btScalar(1)/s1;
btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1);
return combinedStiffness;
}
btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
@ -109,6 +125,15 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
{
newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
}
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);

3
src/BulletCollision/CollisionDispatch/btManifoldResult.h
View File

@ -145,6 +145,9 @@ public:
/// in the future we can let the user override the methods to combine restitution and friction /// in the future we can let the user override the methods to combine restitution and friction
static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1); static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1); static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
}; };
#endif //BT_MANIFOLD_RESULT_H #endif //BT_MANIFOLD_RESULT_H

15
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@ -40,6 +40,7 @@ enum btContactPointFlags
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1, BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
BT_CONTACT_FLAG_HAS_CONTACT_CFM=2, BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
BT_CONTACT_FLAG_HAS_CONTACT_ERP=4, BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
}; };
/// ManifoldContactPoint collects and maintains persistent contactpoints. /// ManifoldContactPoint collects and maintains persistent contactpoints.
@ -116,8 +117,18 @@ class btManifoldPoint
btScalar m_appliedImpulseLateral2; btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1; btScalar m_contactMotion1;
btScalar m_contactMotion2; btScalar m_contactMotion2;
btScalar m_contactCFM;
btScalar m_contactERP; union
{
btScalar m_contactCFM;
btScalar m_combinedContactStiffness1;
};
union
{
btScalar m_contactERP;
btScalar m_combinedContactDamping1;
};
btScalar m_frictionCFM; btScalar m_frictionCFM;

38
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@ -777,10 +777,35 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
relaxation = infoGlobal.m_sor; relaxation = infoGlobal.m_sor;
btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
cfm *= invTimeStep;
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2; //cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
btScalar cfm = infoGlobal.m_globalCfm;
btScalar erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
cfm *= invTimeStep;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@ -1053,8 +1078,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{ {
//disabled: only a single rollingFriction per manifold //only a single rollingFriction per manifold
// rollingFriction--; //rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{ {
relAngVel.normalize(); relAngVel.normalize();
@ -1068,6 +1093,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 axis0,axis1; btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
axis0.normalize();
axis1.normalize();
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);

2
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@ -299,7 +299,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
//compute rhs and remaining solverConstraint fields //compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; btScalar penetration = isFriction? 0 : posError;
btScalar rel_vel = 0.f; btScalar rel_vel = 0.f;
int ndofA = 0; int ndofA = 0;

90
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -224,11 +224,34 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
relaxation = infoGlobal.m_sor; relaxation = infoGlobal.m_sor;
btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
cfm *= invTimeStep; //cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
btScalar cfm = infoGlobal.m_globalCfm;
btScalar erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
cfm *= invTimeStep;
@ -565,12 +588,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
relaxation = infoGlobal.m_sor; relaxation = infoGlobal.m_sor;
btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
cfm *= invTimeStep;
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
if (multiBodyA) if (multiBodyA)
@ -713,7 +730,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
btScalar d = denom0+denom1+cfm; btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
if (d>SIMD_EPSILON) if (d>SIMD_EPSILON)
{ {
solverConstraint.m_jacDiagABInv = relaxation/(d); solverConstraint.m_jacDiagABInv = relaxation/(d);
@ -731,7 +748,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
btScalar restitution = 0.f; btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; btScalar penetration = isFriction? 0 : cp.getDistance();
btScalar rel_vel = 0.f; btScalar rel_vel = 0.f;
int ndofA = 0; int ndofA = 0;
@ -790,15 +807,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
if (penetration>0) if (penetration>0)
{ {
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep; velocityError -= penetration / infoGlobal.m_timeStep;
}
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhs = velocityImpulse;
@ -806,7 +817,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction;
solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv;
@ -951,45 +962,6 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
#define ENABLE_FRICTION #define ENABLE_FRICTION
#ifdef ENABLE_FRICTION #ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = frictionIndex; solverConstraint.m_frictionIndex = frictionIndex;
//#define ROLLING_FRICTION
#ifdef ROLLING_FRICTION
int rollingFriction=1;
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (mbA)
angVelA = mbA->getVelocityVector()>getLink(fcA->m_link).l>getAngularVelocity();
if (mbB)
angVelB = mbB->getAngularVelocity();
btVector3 relAngVel = angVelB-angVelA;
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//disabled: only a single rollingFriction per manifold
//rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();
applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (relAngVel.length()>0.001)
addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} else
{
addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (axis0.length()>0.001)
addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001)
addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
}
#endif //ROLLING_FRICTION
///Bullet has several options to set the friction directions ///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically every frame ///By default, each contact has only a single friction direction that is recomputed automatically every frame

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

@ -26,7 +26,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
m_desiredVelocity(desiredVelocity), m_desiredVelocity(desiredVelocity),
m_desiredPosition(0), m_desiredPosition(0),
m_kd(1.), m_kd(1.),
m_kp(0) m_kp(0),
m_erp(1)
{ {
m_maxAppliedImpulse = maxMotorImpulse; m_maxAppliedImpulse = maxMotorImpulse;
@ -57,7 +58,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int li
m_desiredVelocity(desiredVelocity), m_desiredVelocity(desiredVelocity),
m_desiredPosition(0), m_desiredPosition(0),
m_kd(1.), m_kd(1.),
m_kp(0) m_kp(0),
m_erp(1)
{ {
btAssert(linkDoF < body->getLink(link).m_dofCount); btAssert(linkDoF < body->getLink(link).m_dofCount);
@ -123,7 +125,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
int dof = 0; int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(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 velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; 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_desiredPosition;
btScalar m_kd; btScalar m_kd;
btScalar m_kp; btScalar m_kp;
btScalar m_erp;
public: public:
@ -57,6 +58,14 @@ public:
m_kp = kp; m_kp = kp;
} }
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void debugDraw(class btIDebugDraw* drawer) virtual void debugDraw(class btIDebugDraw* drawer)
{ {

4
src/BulletInverseDynamics/IDConfig.hpp
View File

@ -31,6 +31,10 @@
#include "IDErrorMessages.hpp" #include "IDErrorMessages.hpp"
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
/*
#include "IDConfigEigen.hpp"
#include "IDConfigBuiltin.hpp"
*/
#define INVDYN_INCLUDE_HELPER_2(x) #x #define INVDYN_INCLUDE_HELPER_2(x) #x
#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) #define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) #include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)

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"); error_message("invalid joint type\n");
// TODO add configurable abort/crash function // TODO add configurable abort/crash function
abort(); abort();
return 0;
} }
int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, 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 //#define BT_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning #pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl 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 // #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define SIMD_FORCE_INLINE __forceinline #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( LINK_LIBRARIES(
BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK
) )
IF (NOT WIN32) IF (NOT WIN32)
@ -23,6 +23,8 @@ ENDIF()
ADD_EXECUTABLE(Test_PhysicsClientServer ADD_EXECUTABLE(Test_PhysicsClientServer
gtestwrap.cpp gtestwrap.cpp
../../examples/SharedMemory/PhysicsClient.cpp ../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/SharedMemory/PhysicsClient.h ../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp ../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h ../../examples/SharedMemory/PhysicsServer.h

12
test/SharedMemory/premake4.lua
View File

@ -43,11 +43,14 @@ project ("Test_PhysicsServerLoopBack")
"Bullet3Common", "Bullet3Common",
"BulletDynamics", "BulletDynamics",
"BulletCollision", "BulletCollision",
"BussIK",
"LinearMath" "LinearMath"
} }
files { files {
"test.c", "test.c",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h", "../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp", "../../examples/SharedMemory/PhysicsServer.cpp",
@ -112,12 +115,15 @@ project ("Test_PhysicsServerLoopBack")
"BulletWorldImporter", "BulletWorldImporter",
"Bullet3Common", "Bullet3Common",
"BulletDynamics", "BulletDynamics",
"BulletCollision", "BulletCollision",
"BussIK",
"LinearMath" "LinearMath"
} }
files { files {
"test.c", "test.c",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h", "../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp", "../../examples/SharedMemory/PhysicsServer.cpp",
@ -187,7 +193,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
-- } -- }
hasCL = findOpenCL("clew") 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() initOpenGL()
initGlew() initGlew()
@ -214,6 +220,8 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
files { files {
"test.c", "test.c",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp", "../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.cpp",