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/ExtendedTutorials"
include "../examples/SharedMemory"
include "../examples/ThirdPartyLibs/BussIK"
include "../examples/MultiThreading"
if _OPTIONS["lua"] then

4
build_visual_studio.bat
View File

@ -2,9 +2,9 @@
rem premake4 --with-pe vs2010
rem premake4 --bullet2demos vs2010
cd build3
premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_pybullet --python_include_dir="c:/python-3.5.2/include" --python_lib_dir="c:/python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010
premake4 --enable_pybullet --python_include_dir="c:/python34/include" --python_lib_dir="c:/python34/libs" --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_openvr --enable_pybullet --targetdir="../bin" vs2010
rem premake4 --targetdir="../server2bin" vs2010
rem cd vs2010

4
data/cube_no_friction.urdf
View File

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

5
data/cube_small.urdf
View File

@ -3,10 +3,9 @@
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.3"/>
<rolling_friction value="0.01"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<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>
<!-- Husky wheel macros -->
<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>
<mass value="2.637"/>
<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>
</transmission>
<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>
<mass value="2.637"/>
<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>
</transmission>
<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>
<mass value="2.637"/>
<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>
</transmission>
<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>
<mass value="2.637"/>
<origin xyz="0 0 0"/>

18
data/kiva_shelf/model.sdf
View File

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

BIN
data/multibody.bullet
View File

Binary file not shown.

126
data/pr2_gripper.urdf Normal file
View File

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

29
data/samurai.urdf Normal file
View File

@ -0,0 +1,29 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link concave="yes" name="baseLink">
<contact>
<rolling_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="1.5707963 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="samurai_monastry.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision concave="yes">
<origin rpy="1.5707963 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="samurai_monastry.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

2
examples/CMakeLists.txt
View File

@ -1,6 +1,6 @@
SUBDIRS( HelloWorld BasicDemo )
IF(BUILD_BULLET3)
SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen OpenGLWindow )
SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK OpenGLWindow )
ENDIF()
IF(BUILD_PYBULLET)
SUBDIRS(pybullet)

1
examples/CommonInterfaces/CommonCameraInterface.h
View File

@ -9,6 +9,7 @@ struct CommonCameraInterface
virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0;
virtual void disableVRCamera()=0;
virtual bool isVRCamera() const =0;
virtual void setVRCameraOffsetTransform(const float offset[16])=0;
virtual void getCameraTargetPosition(float pos[3]) const = 0;
virtual void getCameraPosition(float pos[3]) const = 0;

3
examples/CommonInterfaces/CommonRenderInterface.h
View File

@ -27,8 +27,9 @@ struct CommonRenderInterface
virtual CommonCameraInterface* getActiveCamera()=0;
virtual void setActiveCamera(CommonCameraInterface* cam)=0;
virtual void renderScene()=0;
virtual void renderScene()=0;
virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){};
virtual int getScreenWidth() = 0;
virtual int getScreenHeight() = 0;

23
examples/ExampleBrowser/CMakeLists.txt
View File

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

42
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

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

2
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@ -56,6 +56,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size);
void renderInternalGl2(int pass, const btDiscreteDynamicsWorld* dynamicsWorld);
void setVRMode(bool vrMode);
};
#endif //OPENGL_GUI_HELPER_H

4
examples/ExampleBrowser/premake4.lua
View File

@ -10,7 +10,7 @@ project "App_BulletExampleBrowser"
initOpenCL("clew")
end
links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
initOpenGL()
initGlew()
@ -55,6 +55,8 @@ project "App_BulletExampleBrowser"
"../TinyRenderer/our_gl.cpp",
"../TinyRenderer/TinyRenderer.cpp",
"../RenderingExamples/TinyRendererSetup.cpp",
"../SharedMemory/IKTrajectoryHelper.cpp",
"../SharedMemory/IKTrajectoryHelper.h",
"../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp",

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

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

@ -410,7 +410,10 @@ void ConvertURDF2BulletInternal(
{
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
{

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

@ -19,7 +19,8 @@ enum URDF_LinkContactFlags
URDF_CONTACT_HAS_ROLLING_FRICTION=2,
URDF_CONTACT_HAS_INERTIA_SCALING=2,
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
@ -29,6 +30,9 @@ struct URDFLinkContactInfo
btScalar m_inertiaScaling;
btScalar m_contactCfm;
btScalar m_contactErp;
btScalar m_contactStiffness;
btScalar m_contactDamping;
int m_flags;
URDFLinkContactInfo()
@ -36,7 +40,9 @@ struct URDFLinkContactInfo
m_rollingFriction(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0)
m_contactErp(0),
m_contactStiffness(1e4),
m_contactDamping(1)
{
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)
collision.m_name = name_char;
const char *concave_char = config->Attribute("concave");
if (concave_char)
collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
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());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} else
{
if (!rolling_xml->Attribute("value"))
if (m_parseSDF)
{
logger->reportError("Link/contact: rolling friction element must have value attribute");
return false;
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
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;
};
enum UrdfCollisionFlags
{
URDF_FORCE_CONCAVE_TRIMESH=1,
};
struct UrdfCollision
{
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
int m_flags;
UrdfCollision()
:m_flags(0)
{
}
};
struct UrdfJoint;

21
examples/MultiBody/MultiBodySoftContact.cpp
View File

@ -36,7 +36,6 @@ public:
};
extern ContactAddedCallback gContactAddedCallback;
MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
@ -47,18 +46,7 @@ m_once(true)
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()
{
gContactAddedCallback = btMultiBodySoftContactCallback;
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
@ -109,12 +97,13 @@ void MultiBodySoftContact::initPhysics()
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
//setContactStiffnessAndDamping will enable compliant rigid body contact
body->setContactStiffnessAndDamping(300,10);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
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
bool useShadowMap = true;// true;//false;//true;
int shadowMapWidth=4096;//8192;
int shadowMapHeight= 4096;
float shadowMapWorldSize=25;
int shadowMapWidth= 2048;
int shadowMapHeight= 2048;
float shadowMapWorldSize=5;
#define MAX_POINTS_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)
{
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();
// return;
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);
} else

6
examples/OpenGLWindow/GLInstancingRenderer.h
View File

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

30
examples/OpenGLWindow/SimpleCamera.cpp
View File

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

2
examples/OpenGLWindow/SimpleCamera.h
View File

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

20
examples/RigidBody/RigidBodySoftContact.cpp
View File

@ -29,7 +29,7 @@ subject to the following restrictions:
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
extern ContactAddedCallback gContactAddedCallback;
struct RigidBodySoftContact : public CommonRigidBodyBase
@ -40,7 +40,7 @@ struct RigidBodySoftContact : public CommonRigidBodyBase
}
virtual ~RigidBodySoftContact()
{
gContactAddedCallback = 0;
}
virtual void initPhysics();
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()
{
gContactAddedCallback = btRigidBodySoftContactCallback;
m_guiHelper->setUpAxis(1);
@ -120,8 +111,9 @@ void RigidBodySoftContact::initPhysics()
{
btScalar mass(0.);
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.setNumSimulationSubSteps(4);
m_robotSim.setNumSimulationSubSteps(4);
}
if ((m_options & eTWO_POINT_GRASP)!=0)

31
examples/RoboticsLearning/KukaGraspExample.cpp
View File

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

2
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@ -117,7 +117,7 @@ public:
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
{
args.m_fileName = "cube.urdf";
args.m_fileName = "cube_soft.urdf";
args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0);
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);
}
/*
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, double targetPosition[3]);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointPositions);
*/
bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
{
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId,
args.m_currentJointPositions,args.m_endEffectorTargetPosition);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId,
&results.m_numPositions,
results.m_calculatedJointPositions);
return result;
}
b3RobotSimAPI::~b3RobotSimAPI()
{
delete m_data;

32
examples/RoboticsLearning/b3RobotSimAPI.h
View File

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

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

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

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

@ -22,6 +22,8 @@ public:
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
bool createFromMultiBody(class btMultiBody* mb);
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
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;
}
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP;
command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
return 0;
}
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@ -1303,4 +1316,61 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
}
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);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
@ -119,6 +120,15 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle
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);
///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 "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "IKTrajectoryHelper.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h"
@ -381,6 +381,9 @@ struct PhysicsServerCommandProcessorInternalData
bool m_allowRealTimeSimulation;
bool m_hasGround;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@ -389,6 +392,7 @@ struct PhysicsServerCommandProcessorInternalData
btScalar m_numSimulationSubSteps;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
@ -428,6 +432,8 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData()
:m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(false),
m_commandLogger(0),
m_logPlayback(0),
@ -552,10 +558,14 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.005;
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@ -1061,6 +1071,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//no timestamp yet
int timeStamp = 0;
//catch uninitialized cases
serverStatusOut.m_type = CMD_INVALID_STATUS;
//consume the command
switch (clientCmd.m_type)
@ -1868,6 +1881,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
{
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
@ -1961,6 +1979,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true;
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
break;
}
case CMD_CREATE_RIGID_BODY:
@ -2516,9 +2535,50 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
case CMD_CALCULATE_INVERSE_KINEMATICS:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;
if (ikHelperPtrPtr)
{
ikHelperPtr = *ikHelperPtrPtr;
}
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody))
{
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, ikHelperPtr);
ikHelperPtr = tmpHelper;
} else
{
delete tmpHelper;
}
}
if (ikHelperPtr)
{
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
}
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");
@ -2536,13 +2596,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
return hasStatus;
}
static int skip=1;
void PhysicsServerCommandProcessor::renderScene()
{
if (m_data->m_guiHelper)
{
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
}
@ -2560,6 +2620,7 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
}
}
btVector3 gLastPickPos(0,0,0);
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;
gLastPickPos = pickPos;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
@ -2591,6 +2653,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
} else
{
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
@ -2709,23 +2772,108 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
btVector3 gVRGripperPos(0,0,0);
btQuaternion gVRGripperOrn(0,0,0,1);
bool gVRGripperClosed = false;
int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (m_data->m_allowRealTimeSimulation)
if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper)
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(32768);
if (!m_data->m_hasGround)
{
m_data->m_hasGround = true;
int bodyId = 0;
btAlignedObjectArray<char> bufferServerToClient;
bufferServerToClient.resize(32768);
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
if (m_data->m_gripperRigidbodyFixed == 0)
{
int bodyId = 0;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(1);
btVector3 pivotInParent(0, 0, 0);
btMatrix3x3 frameInParent;
frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI);
m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
}
}
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime);
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
for (int i = 0; i < 2; i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
if (motor)
{
motor->setErp(0.005);
if (gVRGripperClosed)
{
motor->setPositionTarget(0.2, 1);
}
else
{
motor->setPositionTarget(SIMD_HALF_PI, 1);
}
motor->setVelocityTarget(0, 0.1);
btScalar maxImp = 550.*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
}
}
}
}
}
int maxSteps = 3;
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime);
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps)
{
gNumSteps = numSteps;
gDtInSec = dtInSec;
gSubStep = m_data->m_physicsDeltaTime;
}
}
}

297
examples/SharedMemory/PhysicsServerExample.cpp
View File

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

19
examples/SharedMemory/SharedMemoryCommands.h
View File

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

10
examples/SharedMemory/premake4.lua
View File

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

76
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@ -13,7 +13,9 @@
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "LinearMath/btIDebugDraw.h"
int gSharedMemoryKey = -1;
int gDebugDrawFlags = 0;
//how can you try typing on a keyboard, without seeing it?
//it is pretty funny, to see the desktop in VR!
@ -32,7 +34,7 @@ int gSharedMemoryKey = -1;
CommonExampleInterface* sExample;
int sPrevPacketNum=0;
GUIHelperInterface* sGuiPtr = 0;
OpenGLGuiHelper* sGuiPtr = 0;
static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 };
@ -384,6 +386,8 @@ bool CMainApplication::BInit()
sGuiPtr = new OpenGLGuiHelper(m_app,false);
sGuiPtr->setVRMode(true);
//sGuiPtr = new DummyGUIHelper;
@ -467,7 +471,7 @@ bool CMainApplication::BInit()
m_fScaleSpacing = 4.0f;
m_fNearClip = 0.1f;
m_fFarClip = 30.0f;
m_fFarClip = 3000.0f;
m_iTexture = 0;
m_uiVertcount = 0;
@ -621,6 +625,9 @@ void CMainApplication::Shutdown()
}
}
sExample->exitPhysics();
delete sExample;
delete m_app;
m_app=0;
@ -668,6 +675,7 @@ bool CMainApplication::HandleInput()
{
//we need to have the 'move' events, so no early out here
//if (sPrevStates[unDevice].unPacketNum != state.unPacketNum)
if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
{
sPrevStates[unDevice].unPacketNum = state.unPacketNum;
@ -689,7 +697,19 @@ bool CMainApplication::HandleInput()
if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0)
{
// printf("Device PRESSED: %d, button %d\n", unDevice, button);
if (button==2)
{
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
//add a special debug drawer that deals with this
//gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints
//btIDebugDraw::DBG_DrawConstraintLimits+
//btIDebugDraw::DBG_DrawConstraints
;
}
sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
}
else
{
@ -699,16 +719,30 @@ bool CMainApplication::HandleInput()
}
else
{
//not pressed now, but pressed before -> raise a button up event
if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0)
if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
{
b3Transform tr;
getControllerTransform(unDevice, tr);
float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] };
b3Quaternion born = tr.getRotation();
float orn[4] = { born[0], born[1], born[2], born[3] };
// printf("Device RELEASED: %d, button %d\n", unDevice,button);
sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn);
// printf("Device RELEASED: %d, button %d\n", unDevice,button);
//not pressed now, but pressed before -> raise a button up event
if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0)
{
if (button==2)
{
gDebugDrawFlags = 0;
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL);
}
sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn);
} else
{
sExample->vrControllerMoveCallback(unDevice, pos, orn);
}
}
}
}
@ -725,13 +759,11 @@ bool CMainApplication::HandleInput()
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CMainApplication::RunMainLoop()
{
bool bQuit = false;
//SDL_StartTextInput();
//SDL_ShowCursor( SDL_DISABLE );
while ( !bQuit && !m_app->m_window->requestedExit())
{
bQuit = HandleInput();
@ -739,7 +771,6 @@ void CMainApplication::RunMainLoop()
RenderFrame();
}
//SDL_StopTextInput();
}
@ -1603,11 +1634,19 @@ void CMainApplication::RenderStereoTargets()
m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId);
sExample->renderScene();
if (gDebugDrawFlags)
{
sExample->physicsDebugDraw(gDebugDrawFlags);
}
{
sExample->renderScene();
}
//m_app->m_instancingRenderer->renderScene();
DrawGridData gridUp;
gridUp.upAxis = m_app->getUpAxis();
m_app->drawGrid(gridUp);
// m_app->drawGrid(gridUp);
glBindFramebuffer( GL_FRAMEBUFFER, 0 );
@ -1645,8 +1684,17 @@ void CMainApplication::RenderStereoTargets()
m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId);
//m_app->m_renderer->renderScene();
sExample->renderScene();
m_app->drawGrid(gridUp);
if (gDebugDrawFlags)
{
sExample->physicsDebugDraw(gDebugDrawFlags);
}
{
sExample->renderScene();
}
//m_app->drawGrid(gridUp);
glBindFramebuffer( GL_FRAMEBUFFER, 0 );

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

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

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

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

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

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

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

4
examples/pybullet/CMakeLists.txt
View File

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

4
examples/pybullet/premake4.lua
View File

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

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
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define B3_FORCE_INLINE __forceinline

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

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

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

@ -86,6 +86,10 @@ protected:
btScalar m_friction;
btScalar m_restitution;
btScalar m_rollingFriction;
btScalar m_contactDamping;
btScalar m_contactStiffness;
///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.
@ -129,7 +133,8 @@ public:
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
CF_CHARACTER_OBJECT = 16,
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
@ -319,7 +324,31 @@ public:
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
int getInternalType() const
{
@ -541,6 +570,8 @@ struct btCollisionObjectDoubleData
double m_deactivationTime;
double m_friction;
double m_rollingFriction;
double m_contactDamping;
double m_contactStiffness;
double m_restitution;
double m_hitFraction;
double m_ccdSweptSphereRadius;
@ -574,7 +605,8 @@ struct btCollisionObjectFloatData
float m_deactivationTime;
float m_friction;
float m_rollingFriction;
float m_contactDamping;
float m_contactStiffness;
float m_restitution;
float m_hitFraction;
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;
inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
{
btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
@ -58,6 +58,22 @@ btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject*
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)
@ -109,6 +125,15 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
newPt.m_combinedFriction = calculateCombinedFriction(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());
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);

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
static btScalar calculateCombinedRestitution(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

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

@ -40,6 +40,7 @@ enum btContactPointFlags
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
};
/// ManifoldContactPoint collects and maintains persistent contactpoints.
@ -116,8 +117,18 @@ class btManifoldPoint
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;
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;

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

@ -777,10 +777,35 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
relaxation = infoGlobal.m_sor;
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);
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))
{
//disabled: only a single rollingFriction per manifold
// rollingFriction--;
//only a single rollingFriction per manifold
//rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
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);
btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
axis0.normalize();
axis1.normalize();
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,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
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
btScalar penetration = isFriction? 0 : posError;
btScalar rel_vel = 0.f;
int ndofA = 0;

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

@ -224,11 +224,34 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
relaxation = infoGlobal.m_sor;
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;
@ -565,12 +588,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
relaxation = infoGlobal.m_sor;
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)
@ -713,7 +730,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
btScalar d = denom0+denom1+cfm;
btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
@ -731,7 +748,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
btScalar penetration = isFriction? 0 : cp.getDistance();
btScalar rel_vel = 0.f;
int ndofA = 0;
@ -790,15 +807,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
if (penetration>0)
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
@ -806,7 +817,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
solverConstraint.m_lowerLimit = -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
#ifdef ENABLE_FRICTION
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
///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_desiredPosition(0),
m_kd(1.),
m_kp(0)
m_kp(0),
m_erp(1)
{
m_maxAppliedImpulse = maxMotorImpulse;
@ -57,7 +58,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int li
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0)
m_kp(0),
m_erp(1)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@ -123,7 +125,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;

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

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

4
src/BulletInverseDynamics/IDConfig.hpp
View File

@ -31,6 +31,10 @@
#include "IDErrorMessages.hpp"
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
/*
#include "IDConfigEigen.hpp"
#include "IDConfigBuiltin.hpp"
*/
#define INVDYN_INCLUDE_HELPER_2(x) #x
#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
#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");
// TODO add configurable abort/crash function
abort();
return 0;
}
int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,

2
src/LinearMath/btScalar.h
View File

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

4
test/SharedMemory/CMakeLists.txt
View File

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

12
test/SharedMemory/premake4.lua
View File

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