diff --git a/build3/premake4.lua b/build3/premake4.lua
index 3f92bba58..f0de611fd 100644
--- a/build3/premake4.lua
+++ b/build3/premake4.lua
@@ -249,6 +249,7 @@ end
include "../examples/InverseDynamics"
include "../examples/ExtendedTutorials"
include "../examples/SharedMemory"
+ include "../examples/ThirdPartyLibs/BussIK"
include "../examples/MultiThreading"
if _OPTIONS["lua"] then
diff --git a/build_visual_studio.bat b/build_visual_studio.bat
index 08e6b70b6..9cefeb7d2 100644
--- a/build_visual_studio.bat
+++ b/build_visual_studio.bat
@@ -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
diff --git a/data/cube_no_friction.urdf b/data/cube_no_friction.urdf
index aef2bfa15..6df23a865 100644
--- a/data/cube_no_friction.urdf
+++ b/data/cube_no_friction.urdf
@@ -4,8 +4,8 @@
-
-
+
+
diff --git a/data/cube_small.urdf b/data/cube_small.urdf
index 23ae7fa3b..a0e3cc609 100644
--- a/data/cube_small.urdf
+++ b/data/cube_small.urdf
@@ -3,10 +3,9 @@
-
+
-
-
+
diff --git a/data/cube_soft.urdf b/data/cube_soft.urdf
new file mode 100644
index 000000000..af40dd379
--- /dev/null
+++ b/data/cube_soft.urdf
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/husky/husky.urdf b/data/husky/husky.urdf
index e9762701b..4fab377ae 100644
--- a/data/husky/husky.urdf
+++ b/data/husky/husky.urdf
@@ -102,6 +102,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
+
+
+
+
+
+
+
@@ -146,6 +153,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
+
+
+
+
+
+
+
@@ -190,6 +204,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
+
+
+
+
+
+
+
@@ -234,6 +255,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
+
+
+
+
+
+
+
diff --git a/data/kiva_shelf/model.sdf b/data/kiva_shelf/model.sdf
index 3e506a9b0..138f88d6d 100644
--- a/data/kiva_shelf/model.sdf
+++ b/data/kiva_shelf/model.sdf
@@ -2,18 +2,18 @@
1
- 0 2 0 0 0 0
+ 0 1 0 0 0 0
0.0 .0 1.2045 0 0 0
- 76.26
+ 0
- 47
- -0.003456
- 0.001474
- 13.075
- -0.014439
- 47
+ 0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
@@ -30,7 +30,7 @@
-
+
0 0 0 1.5707 0 0
diff --git a/data/multibody.bullet b/data/multibody.bullet
index 31800308c..19dacc389 100644
Binary files a/data/multibody.bullet and b/data/multibody.bullet differ
diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf
new file mode 100644
index 000000000..33d833629
--- /dev/null
+++ b/data/pr2_gripper.urdf
@@ -0,0 +1,126 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/samurai.urdf b/data/samurai.urdf
new file mode 100644
index 000000000..f15d47fb6
--- /dev/null
+++ b/data/samurai.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 8a974c55b..47728c244 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -1,6 +1,6 @@
SUBDIRS( HelloWorld BasicDemo )
IF(BUILD_BULLET3)
- SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen OpenGLWindow )
+ SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK OpenGLWindow )
ENDIF()
IF(BUILD_PYBULLET)
SUBDIRS(pybullet)
diff --git a/examples/CommonInterfaces/CommonCameraInterface.h b/examples/CommonInterfaces/CommonCameraInterface.h
index 9c1740788..1e9fa2f25 100644
--- a/examples/CommonInterfaces/CommonCameraInterface.h
+++ b/examples/CommonInterfaces/CommonCameraInterface.h
@@ -9,6 +9,7 @@ struct CommonCameraInterface
virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0;
virtual void disableVRCamera()=0;
virtual bool isVRCamera() const =0;
+ virtual void setVRCameraOffsetTransform(const float offset[16])=0;
virtual void getCameraTargetPosition(float pos[3]) const = 0;
virtual void getCameraPosition(float pos[3]) const = 0;
diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h
index 0ac09975f..888607dd9 100644
--- a/examples/CommonInterfaces/CommonRenderInterface.h
+++ b/examples/CommonInterfaces/CommonRenderInterface.h
@@ -27,8 +27,9 @@ struct CommonRenderInterface
virtual CommonCameraInterface* getActiveCamera()=0;
virtual void setActiveCamera(CommonCameraInterface* cam)=0;
- virtual void renderScene()=0;
+ virtual void renderScene()=0;
+ virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){};
virtual int getScreenWidth() = 0;
virtual int getScreenHeight() = 0;
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 717d57f28..32f4c18f6 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -47,20 +47,20 @@ IF (BUILD_SHARED_LIBS)
IF (WIN32)
TARGET_LINK_LIBRARIES(
BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
- BulletInverseDynamics LinearMath OpenGLWindow gwen
+ BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
)
ELSE(WIN32)
IF(APPLE)
TARGET_LINK_LIBRARIES(
BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
- BulletInverseDynamics LinearMath OpenGLWindow gwen
+ BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
)
ELSE(APPLE)
TARGET_LINK_LIBRARIES(
BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
- BulletInverseDynamics LinearMath OpenGLWindow gwen
+ BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
pthread dl
)
ENDIF(APPLE)
@@ -81,7 +81,7 @@ INCLUDE_DIRECTORIES(
LINK_LIBRARIES(
- BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen
+ BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
)
IF (WIN32)
@@ -129,6 +129,8 @@ SET(BulletExampleBrowser_SRCS
../TinyRenderer/TinyRenderer.cpp
../SharedMemory/TinyRendererVisualShapeConverter.cpp
../SharedMemory/TinyRendererVisualShapeConverter.h
+ ../SharedMemory/IKTrajectoryHelper.cpp
+ ../SharedMemory/IKTrajectoryHelper.h
../RenderingExamples/TinyRendererSetup.cpp
../SharedMemory/PhysicsServer.cpp
../SharedMemory/PhysicsClientSharedMemory.cpp
@@ -218,8 +220,6 @@ SET(BulletExampleBrowser_SRCS
../RoboticsLearning/R2D2GraspExample.h
../RoboticsLearning/KukaGraspExample.cpp
../RoboticsLearning/KukaGraspExample.h
- ../RoboticsLearning/IKTrajectoryHelper.cpp
- ../RoboticsLearning/IKTrajectoryHelper.h
../RenderingExamples/CoordinateSystemDemo.cpp
../RenderingExamples/CoordinateSystemDemo.h
../RenderingExamples/RaytracerSetup.cpp
@@ -307,17 +307,6 @@ SET(BulletExampleBrowser_SRCS
../ThirdPartyLibs/stb_image/stb_image.cpp
../ThirdPartyLibs/stb_image/stb_image.h
- ../ThirdPartyLibs/BussIK/Jacobian.cpp
- ../ThirdPartyLibs/BussIK/Tree.cpp
- ../ThirdPartyLibs/BussIK/Node.cpp
- ../ThirdPartyLibs/BussIK/LinearR2.cpp
- ../ThirdPartyLibs/BussIK/LinearR3.cpp
- ../ThirdPartyLibs/BussIK/LinearR4.cpp
- ../ThirdPartyLibs/BussIK/MatrixRmn.cpp
- ../ThirdPartyLibs/BussIK/VectorRn.cpp
- ../ThirdPartyLibs/BussIK/Misc.cpp
-
-
../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../ThirdPartyLibs/tinyxml/tinystr.cpp
../ThirdPartyLibs/tinyxml/tinyxml.cpp
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
index 8001b11e3..ecd1ed45d 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
@@ -146,11 +146,26 @@ struct OpenGLGuiHelperInternalData
struct CommonGraphicsApp* m_glApp;
class MyDebugDrawer* m_debugDraw;
GL_ShapeDrawer* m_gl2ShapeDrawer;
-
+ bool m_vrMode;
+ int m_vrSkipShadowPass;
+
btAlignedObjectArray m_rgbaPixelBuffer1;
btAlignedObjectArray m_depthBuffer1;
+
+ OpenGLGuiHelperInternalData()
+ :m_vrMode(false),
+ m_vrSkipShadowPass(0)
+ {
+ }
+
};
+void OpenGLGuiHelper::setVRMode(bool vrMode)
+{
+ m_data->m_vrMode = vrMode;
+ m_data->m_vrSkipShadowPass = 0;
+}
+
OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2)
@@ -269,6 +284,10 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
}
void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
{
+ //in VR mode, we skip the synchronization for the second eye
+ if (m_data->m_vrMode && m_data->m_vrSkipShadowPass==1)
+ return;
+
int numCollisionObjects = rbWorld->getNumCollisionObjects();
for (int i = 0; im_glApp->m_renderer->renderScene();
+ if (m_data->m_vrMode)
+ {
+ //in VR, we skip the shadow generation for the second eye
+
+ if (m_data->m_vrSkipShadowPass>=1)
+ {
+ m_data->m_glApp->m_renderer->renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
+ m_data->m_vrSkipShadowPass=0;
+
+ } else
+ {
+ m_data->m_glApp->m_renderer->renderScene();
+ m_data->m_vrSkipShadowPass++;
+ }
+ } else
+ {
+ m_data->m_glApp->m_renderer->renderScene();
+ }
+
//backwards compatible OpenGL2 rendering
if (m_data->m_gl2ShapeDrawer && rbWorld)
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h
index 2fac0313e..eaa72a7be 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.h
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.h
@@ -56,6 +56,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size);
void renderInternalGl2(int pass, const btDiscreteDynamicsWorld* dynamicsWorld);
+
+ void setVRMode(bool vrMode);
};
#endif //OPENGL_GUI_HELPER_H
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 2010c744e..5bd7eec42 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -10,7 +10,7 @@ project "App_BulletExampleBrowser"
initOpenCL("clew")
end
- links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+ links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
initOpenGL()
initGlew()
@@ -55,6 +55,8 @@ project "App_BulletExampleBrowser"
"../TinyRenderer/our_gl.cpp",
"../TinyRenderer/TinyRenderer.cpp",
"../RenderingExamples/TinyRendererSetup.cpp",
+ "../SharedMemory/IKTrajectoryHelper.cpp",
+ "../SharedMemory/IKTrajectoryHelper.h",
"../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp",
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 992840850..b2e528677 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -577,13 +577,32 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1],
glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2]));
}
- //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
- btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
- //cylZShape->initializePolyhedralFeatures();
- //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
- //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
- cylZShape->setMargin(0.001);
- shape = cylZShape;
+
+ if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+ {
+ btTriangleMesh* meshInterface = new btTriangleMesh();
+ for (int i=0;im_numIndices/3;i++)
+ {
+ float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
+ float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
+ float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
+ meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
+ btVector3(v1[0],v1[1],v1[2]),
+ btVector3(v2[0],v2[1],v2[2]));
+ }
+
+ btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
+ shape = trimesh;
+ } else
+ {
+ //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
+ btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+ //cylZShape->initializePolyhedralFeatures();
+ //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+ //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+ cylZShape->setMargin(0.001);
+ shape = cylZShape;
+ }
} else
{
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
@@ -1001,7 +1020,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
//delete textures
for (int i=0;isetRollingFriction(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
{
diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h
index 2a6bdfef4..08c984268 100644
--- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h
+++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h
@@ -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;
}
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index bb0af5f79..20e6a920f 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -489,6 +489,9 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
if (name_char)
collision.m_name = name_char;
+ const char *concave_char = config->Attribute("concave");
+ if (concave_char)
+ collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
return true;
}
@@ -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(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(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(rolling_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
+
}
-
- link.m_contactInfo.m_rollingFriction = urdfLexicalCast(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(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(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(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(damping_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+
+ }
+ }
+ }
}
}
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index 25f7e3e38..0ed995f33 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -75,11 +75,22 @@ struct UrdfVisual
UrdfMaterial m_localMaterial;
};
+enum UrdfCollisionFlags
+{
+ URDF_FORCE_CONCAVE_TRIMESH=1,
+};
+
+
struct UrdfCollision
{
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
+ int m_flags;
+ UrdfCollision()
+ :m_flags(0)
+ {
+ }
};
struct UrdfJoint;
diff --git a/examples/MultiBody/MultiBodySoftContact.cpp b/examples/MultiBody/MultiBodySoftContact.cpp
index cc5d1ff9a..b7ce1c9f6 100644
--- a/examples/MultiBody/MultiBodySoftContact.cpp
+++ b/examples/MultiBody/MultiBodySoftContact.cpp
@@ -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);
}
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp
index e9c5e38fa..1a8ee2063 100644
--- a/examples/OpenGLWindow/GLInstancingRenderer.cpp
+++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp
@@ -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(srcIndexm_totalNumInstances);
+ b3Assert(srcIndex>=0);
+ position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0];
+ position[1] = m_data->m_instance_positions_ptr[srcIndex*4+1];
+ position[2] = m_data->m_instance_positions_ptr[srcIndex*4+2];
+
+ orientation[0] = m_data->m_instance_quaternion_ptr[srcIndex*4+0];
+ orientation[1] = m_data->m_instance_quaternion_ptr[srcIndex*4+1];
+ orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
+ orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
+}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
{
m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
@@ -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
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h
index 135f44133..9fae49db5 100644
--- a/examples/OpenGLWindow/GLInstancingRenderer.h
+++ b/examples/OpenGLWindow/GLInstancingRenderer.h
@@ -41,7 +41,7 @@ class GLInstancingRenderer : public CommonRenderInterface
int m_upAxis;
bool m_enableBlend;
- void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
+
@@ -52,6 +52,7 @@ public:
virtual void init();
virtual void renderScene();
+ virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
void InitShaders();
void CleanupShaders();
@@ -73,6 +74,7 @@ public:
void writeTransforms();
+
virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex);
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)
{
@@ -90,6 +92,8 @@ public:
}
+ virtual void readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation);
+
virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp
index 43729b236..f0ba375d2 100644
--- a/examples/OpenGLWindow/SimpleCamera.cpp
+++ b/examples/OpenGLWindow/SimpleCamera.cpp
@@ -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
{
diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h
index 5a057baf0..7221f01a6 100644
--- a/examples/OpenGLWindow/SimpleCamera.h
+++ b/examples/OpenGLWindow/SimpleCamera.h
@@ -15,7 +15,9 @@ struct SimpleCamera : public CommonCameraInterface
virtual void getCameraViewMatrix(float m[16]) const;
virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]);
+ virtual void setVRCameraOffsetTransform(const float offset[16]);
virtual void disableVRCamera();
+
virtual bool isVRCamera() const;
virtual void getCameraTargetPosition(float pos[3]) const;
diff --git a/examples/RigidBody/RigidBodySoftContact.cpp b/examples/RigidBody/RigidBodySoftContact.cpp
index 1ea1bb002..1e7cd2c54 100644
--- a/examples/RigidBody/RigidBodySoftContact.cpp
+++ b/examples/RigidBody/RigidBodySoftContact.cpp
@@ -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);
+
}
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index 434454985..9a7ab8e7b 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -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)
diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp
index cb4b6c521..3e0de985d 100644
--- a/examples/RoboticsLearning/KukaGraspExample.cpp
+++ b/examples/RoboticsLearning/KukaGraspExample.cpp
@@ -1,6 +1,6 @@
#include "KukaGraspExample.h"
-#include "IKTrajectoryHelper.h"
+#include "../SharedMemory/IKTrajectoryHelper.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
@@ -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;
diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp
index e4a412d73..e0c9cadd1 100644
--- a/examples/RoboticsLearning/R2D2GraspExample.cpp
+++ b/examples/RoboticsLearning/R2D2GraspExample.cpp
@@ -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);
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp
index cb7f231f1..7a6667c62 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.cpp
+++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp
@@ -469,6 +469,33 @@ void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps)
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
}
+/*
+b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+ const double* jointPositionsQ, double targetPosition[3]);
+
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+ int* bodyUniqueId,
+ int* dofCount,
+ double* jointPositions);
+*/
+bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
+{
+
+ b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId,
+ args.m_currentJointPositions,args.m_endEffectorTargetPosition);
+
+ b3SharedMemoryStatusHandle statusHandle;
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+
+ bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+ &results.m_bodyUniqueId,
+ &results.m_numPositions,
+ results.m_calculatedJointPositions);
+
+ return result;
+}
+
+
b3RobotSimAPI::~b3RobotSimAPI()
{
delete m_data;
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h
index b57a8a7ca..54ebec698 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.h
+++ b/examples/RoboticsLearning/b3RobotSimAPI.h
@@ -50,6 +50,7 @@ struct b3RobotSimLoadFileArgs
}
};
+
struct b3RobotSimLoadFileResults
{
b3AlignedObjectArray m_uniqueObjectIds;
@@ -81,6 +82,35 @@ struct b3JointMotorArgs
}
};
+enum b3InverseKinematicsFlags
+{
+ B3_HAS_IK_TARGET_ORIENTATION=1,
+};
+
+struct b3RobotSimInverseKinematicArgs
+{
+ int m_bodyUniqueId;
+ double* m_currentJointPositions;
+ int m_numPositions;
+ double m_endEffectorTargetPosition[3];
+ double m_endEffectorTargetOrientation[3];
+ int m_flags;
+
+ b3RobotSimInverseKinematicArgs()
+ :m_bodyUniqueId(-1),
+ m_currentJointPositions(0),
+ m_numPositions(0),
+ m_flags(0)
+ {
+ }
+};
+
+struct b3RobotSimInverseKinematicsResults
+{
+ int m_bodyUniqueId;
+ double* m_calculatedJointPositions;
+ int m_numPositions;
+};
class b3RobotSimAPI
{
@@ -113,6 +143,8 @@ public:
void setNumSimulationSubSteps(int numSubSteps);
+ bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results);
+
void renderScene();
void debugDraw(int debugDrawMode);
diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp
similarity index 94%
rename from examples/RoboticsLearning/IKTrajectoryHelper.cpp
rename to examples/SharedMemory/IKTrajectoryHelper.cpp
index 94f6600c9..735cf389f 100644
--- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp
+++ b/examples/SharedMemory/IKTrajectoryHelper.cpp
@@ -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;
-}
\ No newline at end of file
+}
diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h
similarity index 93%
rename from examples/RoboticsLearning/IKTrajectoryHelper.h
rename to examples/SharedMemory/IKTrajectoryHelper.h
index b57a17ce9..bfbc4cdec 100644
--- a/examples/RoboticsLearning/IKTrajectoryHelper.h
+++ b/examples/SharedMemory/IKTrajectoryHelper.h
@@ -22,6 +22,8 @@ public:
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
+ bool createFromMultiBody(class btMultiBody* mb);
+
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double* q_old, int numQ,
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index c49b88c04..d6d869a4d 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -163,6 +163,19 @@ int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int
return 0;
}
+
+int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+ command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP;
+ command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
+ return 0;
+}
+
+
+int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
+
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -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;
}
\ No newline at end of file
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 931a1d9d9..08a179680 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -89,6 +89,7 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
+int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
@@ -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
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 8d6fcd70c..f45610c15 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -15,7 +15,7 @@
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
-
+#include "IKTrajectoryHelper.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h"
@@ -381,6 +381,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 m_multiBodyJointFeedbacks;
btHashMap m_inverseDynamicsBodies;
+ btHashMap 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 gBufferServerToClient;
+ gBufferServerToClient.resize(32768);
+
+
if (!m_data->m_hasGround)
{
m_data->m_hasGround = true;
int bodyId = 0;
- btAlignedObjectArray bufferServerToClient;
- bufferServerToClient.resize(32768);
+
+ loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &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;
+ }
}
}
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 0ff5b4f93..f84e7356c 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -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 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;cm_isPicking = false;
- args->m_isDragging = true;
- args->m_physicsServerPtr->pickBody(from,-toZ);
- //printf("PICK!\n");
- }
-
- if (args->m_isDragging)
- {
- args->m_physicsServerPtr->movePickedBody(from,-toZ);
- // printf(".");
- }
- if (args->m_isReleasing)
- {
- args->m_isDragging = false;
- args->m_isReleasing = false;
- args->m_physicsServerPtr->removePickingConstraint();
- //printf("Release pick\n");
+ btVector3 from = args->m_vrControllerPos[c];
+ btMatrix3x3 mat(args->m_vrControllerOrn[c]);
+
+ btScalar pickDistance = 1000.;
+ btVector3 toX = from+mat.getColumn(0);
+ btVector3 toY = from+mat.getColumn(1);
+ btVector3 toZ = from+mat.getColumn(2)*pickDistance;
+
+ if (args->m_isVrControllerTeleporting[c])
+ {
+ args->m_isVrControllerTeleporting[c] = false;
+ args->m_physicsServerPtr->pickBody(from,-toZ);
+ args->m_physicsServerPtr->removePickingConstraint();
+ }
+
+ if (args->m_isVrControllerPicking[c])
+ {
+ args->m_isVrControllerPicking[c] = false;
+ args->m_isVrControllerDragging[c] = true;
+ args->m_physicsServerPtr->pickBody(from,-toZ);
+ //printf("PICK!\n");
+ }
+
+ if (args->m_isVrControllerDragging[c])
+ {
+ args->m_physicsServerPtr->movePickedBody(from,-toZ);
+ // printf(".");
+ }
+
+ if (args->m_isVrControllerReleasing[c])
+ {
+ args->m_isVrControllerDragging[c] = false;
+ args->m_isVrControllerReleasing[c] = false;
+ args->m_physicsServerPtr->removePickingConstraint();
+ //printf("Release pick\n");
+ }
}
//don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
- btClamp(deltaTimeInSeconds,0.,0.1);
- args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
+ if (deltaTimeInSeconds>clampedDeltaTime)
+ {
+ deltaTimeInSeconds = clampedDeltaTime;
+ b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
+ }
+
clock.reset();
+ args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
+ deltaTimeInSeconds = 0;
+
}
args->m_physicsServerPtr->processClientCommands();
@@ -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;igetAppInterface()->m_renderer->drawLine(from,toX,color,width);
- color=btVector4(0,1,0,1);
- m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
- color=btVector4(0,0,1,1);
- m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
+ btVector4 color;
+ color=btVector4(1,0,0,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
+ color=btVector4(0,1,0,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
+ color=btVector4(0,0,1,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
+ }
}
+ if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
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)
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 64089286b..ee7f2e592 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -223,6 +223,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4,
SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
+ SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32
};
///Controlling a robot involves sending the desired state to its joint motor controllers.
@@ -234,6 +235,7 @@ struct SendPhysicsSimulationParameters
int m_numSimulationSubSteps;
int m_numSolverIterations;
bool m_allowRealTimeSimulation;
+ double m_defaultContactERP;
};
struct RequestActualStateArgs
@@ -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;
};
};
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 5a43bb598..b9a5c9eb5 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -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
};
diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua
index ba761dd82..0dd45af08 100644
--- a/examples/SharedMemory/premake4.lua
+++ b/examples/SharedMemory/premake4.lua
@@ -10,13 +10,15 @@ end
includedirs {".","../../src", "../ThirdPartyLibs",}
links {
- "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath"
+ "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
}
language "C++"
myfiles =
{
+ "IKTrajectoryHelper.cpp",
+ "IKTrajectoryHelper.h",
"PhysicsClient.cpp",
"PhysicsClientSharedMemory.cpp",
"PhysicsClientExample.cpp",
@@ -134,10 +136,10 @@ else
end
defines {"B3_USE_STANDALONE_EXAMPLE"}
-includedirs {"../../src"}
+includedirs {"../../src", "../ThirdPartyLibs"}
links {
- "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common","BussIK"
}
initOpenGL()
initGlew()
@@ -211,7 +213,7 @@ if os.is("Windows") then
}
links {
- "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api","BussIK"
}
diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp
index e85b8af0e..d806ad27c 100644
--- a/examples/StandaloneMain/hellovr_opengl_main.cpp
+++ b/examples/StandaloneMain/hellovr_opengl_main.cpp
@@ -13,7 +13,9 @@
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "LinearMath/btIDebugDraw.h"
int gSharedMemoryKey = -1;
+int gDebugDrawFlags = 0;
//how can you try typing on a keyboard, without seeing it?
//it is pretty funny, to see the desktop in VR!
@@ -32,7 +34,7 @@ int gSharedMemoryKey = -1;
CommonExampleInterface* sExample;
int sPrevPacketNum=0;
-GUIHelperInterface* sGuiPtr = 0;
+OpenGLGuiHelper* sGuiPtr = 0;
static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 };
@@ -384,6 +386,8 @@ bool CMainApplication::BInit()
sGuiPtr = new OpenGLGuiHelper(m_app,false);
+ sGuiPtr->setVRMode(true);
+
//sGuiPtr = new DummyGUIHelper;
@@ -467,7 +471,7 @@ bool CMainApplication::BInit()
m_fScaleSpacing = 4.0f;
m_fNearClip = 0.1f;
- m_fFarClip = 30.0f;
+ m_fFarClip = 3000.0f;
m_iTexture = 0;
m_uiVertcount = 0;
@@ -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 );
diff --git a/examples/ThirdPartyLibs/BussIK/CMakeLists.txt b/examples/ThirdPartyLibs/BussIK/CMakeLists.txt
new file mode 100644
index 000000000..96dc16025
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/CMakeLists.txt
@@ -0,0 +1,10 @@
+
+INCLUDE_DIRECTORIES(
+ .
+)
+
+
+FILE(GLOB BussIK_SRCS "*.cpp" )
+FILE(GLOB BussIK_HDRS "*.h" )
+
+ADD_LIBRARY(BussIK ${BussIK_SRCS} ${BussIK_HDRS})
diff --git a/examples/ThirdPartyLibs/BussIK/Misc.cpp b/examples/ThirdPartyLibs/BussIK/Misc.cpp
index 6aa696d55..330997909 100644
--- a/examples/ThirdPartyLibs/BussIK/Misc.cpp
+++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp
@@ -23,7 +23,7 @@ subject to the following restrictions:
#include
#include "LinearR3.h"
-#include "../OpenGLWindow/OpenGLInclude.h"
+
/****************************************************************
Axes
diff --git a/examples/ThirdPartyLibs/BussIK/premake4.lua b/examples/ThirdPartyLibs/BussIK/premake4.lua
new file mode 100644
index 000000000..74a0630a0
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/premake4.lua
@@ -0,0 +1,11 @@
+ project "BussIK"
+
+ kind "StaticLib"
+
+ includedirs {
+ "."
+ }
+ files {
+ "*.cpp",
+ "*.h",
+ }
diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp
index 639db9b0b..eb5a05a78 100644
--- a/examples/TinyRenderer/our_gl.cpp
+++ b/examples/TinyRenderer/our_gl.cpp
@@ -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);
}
diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt
index a5afa5d66..f49c97b25 100644
--- a/examples/pybullet/CMakeLists.txt
+++ b/examples/pybullet/CMakeLists.txt
@@ -8,6 +8,8 @@ INCLUDE_DIRECTORIES(
SET(pybullet_SRCS
pybullet.c
+ ../../examples/SharedMemory/IKTrajectoryHelper.cpp
+ ../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
@@ -76,6 +78,6 @@ ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS})
SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION})
-TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen Bullet3Common ${PYTHON_LIBRARIES})
+TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common ${PYTHON_LIBRARIES})
diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua
index cacb7218e..86b4eedc5 100644
--- a/examples/pybullet/premake4.lua
+++ b/examples/pybullet/premake4.lua
@@ -10,7 +10,7 @@ project ("pybullet")
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
- links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+ links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
initOpenGL()
initGlew()
@@ -36,6 +36,8 @@ project ("pybullet")
files {
"pybullet.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 8bc52cfbe..69df944ce 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -2,129 +2,109 @@
#include "../SharedMemory/PhysicsDirectC_API.h"
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
-
-
#ifdef __APPLE__
#include
#else
#include
#endif
-
#if PY_MAJOR_VERSION >= 3
#define PyInt_FromLong PyLong_FromLong
#define PyString_FromString PyBytes_FromString
-#endif
+#endif
-enum eCONNECT_METHOD
-{
- eCONNECT_GUI=1,
- eCONNECT_DIRECT=2,
- eCONNECT_SHARED_MEMORY=3,
+enum eCONNECT_METHOD {
+ eCONNECT_GUI = 1,
+ eCONNECT_DIRECT = 2,
+ eCONNECT_SHARED_MEMORY = 3,
};
-
-
-static PyObject *SpamError;
-static b3PhysicsClientHandle sm=0;
+static PyObject* SpamError;
+static b3PhysicsClientHandle sm = 0;
// Step through one timestep of the simulation
-static PyObject *
-pybullet_stepSimulation(PyObject *self, PyObject *args)
-{
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
+static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+
+ if (b3CanSubmitCommand(sm)) {
+ statusHandle = b3SubmitClientCommandAndWaitStatus(
+ sm, b3InitStepSimulationCommand(sm));
+ statusType = b3GetStatusType(statusHandle);
}
+ }
- {
- b3SharedMemoryStatusHandle statusHandle;
- int statusType;
-
- if (b3CanSubmitCommand(sm))
- {
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
- statusType = b3GetStatusType(statusHandle);
- }
- }
-
- Py_INCREF(Py_None);
- return Py_None;
+ Py_INCREF(Py_None);
+ return Py_None;
}
-static PyObject *
-pybullet_connectPhysicsServer(PyObject *self, PyObject *args)
-{
- if (0!=sm)
- {
- PyErr_SetString(SpamError, "Already connected to physics server, disconnect first.");
- return NULL;
- }
-
- {
- int method=eCONNECT_GUI;
- if (!PyArg_ParseTuple(args, "i", &method))
- {
- PyErr_SetString(SpamError, "connectPhysicsServer expected argument eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY");
- return NULL;
- }
+static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) {
+ if (0 != sm) {
+ PyErr_SetString(SpamError,
+ "Already connected to physics server, disconnect first.");
+ return NULL;
+ }
- switch (method)
- {
- case eCONNECT_GUI:
- {
- int argc=0;
- char* argv[1]={0};
+ {
+ int method = eCONNECT_GUI;
+ if (!PyArg_ParseTuple(args, "i", &method)) {
+ PyErr_SetString(SpamError,
+ "connectPhysicsServer expected argument eCONNECT_GUI, "
+ "eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY");
+ return NULL;
+ }
+
+ switch (method) {
+ case eCONNECT_GUI: {
+ int argc = 0;
+ char* argv[1] = {0};
#ifdef __APPLE__
- sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
+ sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else
- sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
+ sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif
- break;
- }
- case eCONNECT_DIRECT:
- {
- sm = b3ConnectPhysicsDirect();
- break;
- }
- case eCONNECT_SHARED_MEMORY:
- {
- sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
- break;
- }
+ break;
+ }
+ case eCONNECT_DIRECT: {
+ sm = b3ConnectPhysicsDirect();
+ break;
+ }
+ case eCONNECT_SHARED_MEMORY: {
+ sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+ break;
+ }
+ default: {
+ PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument");
+ return NULL;
+ }
+ };
+ }
- default:
- {
- PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument");
- return NULL;
- }
- };
-
-
- }
-
- Py_INCREF(Py_None);
- return Py_None;
+ Py_INCREF(Py_None);
+ return Py_None;
}
-static PyObject *
-pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args)
-{
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
- {
- b3DisconnectSharedMemory(sm);
- sm = 0;
- }
-
- Py_INCREF(Py_None);
- return Py_None;
+static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
+ PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+ {
+ b3DisconnectSharedMemory(sm);
+ sm = 0;
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
}
// Load a URDF file indicating the links and joints of an object
@@ -132,439 +112,404 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args)
// to position (0,0,1) with orientation(0,0,0,1)
// els(x,y,z) or
// loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w)
-static PyObject *
-pybullet_loadURDF(PyObject* self, PyObject* args)
-{
-
- int size= PySequence_Size(args);
-
- int bodyIndex = -1;
- const char* urdfFileName="";
-
- float startPosX =0;
- float startPosY =0;
- float startPosZ = 0;
- float startOrnX = 0;
- float startOrnY = 0;
- float startOrnZ = 0;
- float startOrnW = 1;
+static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
+ int size = PySequence_Size(args);
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
+ int bodyIndex = -1;
+ const char* urdfFileName = "";
+
+ double startPosX = 0.0;
+ double startPosY = 0.0;
+ double startPosZ = 0.0;
+ double startOrnX = 0.0;
+ double startOrnY = 0.0;
+ double startOrnZ = 0.0;
+ double startOrnW = 1.0;
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+ if (size == 1) {
+ if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL;
+ }
+ if (size == 4) {
+ if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY,
+ &startPosZ))
+ return NULL;
+ }
+ if (size == 8) {
+ if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX,
+ &startPosY, &startPosZ, &startOrnX, &startOrnY,
+ &startOrnZ, &startOrnW))
+ return NULL;
+ }
+
+ if (strlen(urdfFileName)) {
+ // printf("(%f, %f, %f) (%f, %f, %f, %f)\n",
+ // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW);
+
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle command =
+ b3LoadUrdfCommandInit(sm, urdfFileName);
+
+ // setting the initial position, orientation and other arguments are
+ // optional
+ b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ);
+ b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,
+ startOrnZ, startOrnW);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType != CMD_URDF_LOADING_COMPLETED) {
+ PyErr_SetString(SpamError, "Cannot load URDF file.");
+ return NULL;
}
- if (size==1)
- {
- if (!PyArg_ParseTuple(args, "s", &urdfFileName))
- return NULL;
- }
- if (size == 4)
- {
- if (!PyArg_ParseTuple(args, "sfff", &urdfFileName,
- &startPosX,&startPosY,&startPosZ))
- return NULL;
- }
- if (size==8)
- {
- if (!PyArg_ParseTuple(args, "sfffffff", &urdfFileName,
- &startPosX,&startPosY,&startPosZ,
- &startOrnX,&startOrnY,&startOrnZ, &startOrnW))
- return NULL;
- }
-
- if (strlen(urdfFileName))
- {
- // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW);
-
- b3SharedMemoryStatusHandle statusHandle;
- int statusType;
- b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
-
- //setting the initial position, orientation and other arguments are optional
- b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
- b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,startOrnZ, startOrnW );
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
- statusType = b3GetStatusType(statusHandle);
- if (statusType!=CMD_URDF_LOADING_COMPLETED)
- {
- PyErr_SetString(SpamError, "Cannot load URDF file.");
- return NULL;
- }
- bodyIndex = b3GetStatusBodyIndex(statusHandle);
- } else
- {
- PyErr_SetString(SpamError, "Empty filename, method expects 1, 4 or 8 arguments.");
- return NULL;
- }
- return PyLong_FromLong(bodyIndex);
+ bodyIndex = b3GetStatusBodyIndex(statusHandle);
+ } else {
+ PyErr_SetString(SpamError,
+ "Empty filename, method expects 1, 4 or 8 arguments.");
+ return NULL;
+ }
+ return PyLong_FromLong(bodyIndex);
}
-static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index)
-{
- float v = 0.f;
- PyObject* item;
-
- if (PyList_Check(seq))
- {
- item = PyList_GET_ITEM(seq, index);
- v = PyFloat_AsDouble(item);
- }
- else
- {
- item = PyTuple_GET_ITEM(seq,index);
- v = PyFloat_AsDouble(item);
- }
- return v;
-}
+static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
+ double v = 0.0;
+ PyObject* item;
+ if (PyList_Check(seq)) {
+ item = PyList_GET_ITEM(seq, index);
+ v = PyFloat_AsDouble(item);
+ } else {
+ item = PyTuple_GET_ITEM(seq, index);
+ v = PyFloat_AsDouble(item);
+ }
+ return v;
+}
#define MAX_SDF_BODIES 512
-static PyObject*
-pybullet_loadSDF(PyObject* self, PyObject* args)
-{
- const char* sdfFileName="";
- int size= PySequence_Size(args);
- int numBodies = 0;
- int i;
- int bodyIndicesOut[MAX_SDF_BODIES];
- PyObject* pylist=0;
- b3SharedMemoryStatusHandle statusHandle;
- int statusType;
- b3SharedMemoryCommandHandle commandHandle;
+static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) {
+ const char* sdfFileName = "";
+ int size = PySequence_Size(args);
+ int numBodies = 0;
+ int i;
+ int bodyIndicesOut[MAX_SDF_BODIES];
+ PyObject* pylist = 0;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle commandHandle;
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
- if (size==1)
- {
- if (!PyArg_ParseTuple(args, "s", &sdfFileName))
- return NULL;
- }
+ if (size == 1) {
+ if (!PyArg_ParseTuple(args, "s", &sdfFileName)) return NULL;
+ }
- commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
- statusType = b3GetStatusType(statusHandle);
- if (statusType!=CMD_SDF_LOADING_COMPLETED)
- {
- PyErr_SetString(SpamError, "Cannot load SDF file.");
- return NULL;
- }
+ commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType != CMD_SDF_LOADING_COMPLETED) {
+ PyErr_SetString(SpamError, "Cannot load SDF file.");
+ return NULL;
+ }
- numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
- if (numBodies > MAX_SDF_BODIES)
- {
- PyErr_SetString(SpamError, "SDF exceeds body capacity");
- return NULL;
- }
+ numBodies =
+ b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
+ if (numBodies > MAX_SDF_BODIES) {
+ PyErr_SetString(SpamError, "SDF exceeds body capacity");
+ return NULL;
+ }
- pylist = PyTuple_New(numBodies);
+ pylist = PyTuple_New(numBodies);
- if (numBodies >0 && numBodies <= MAX_SDF_BODIES)
- {
- for (i=0;i 0 && numBodies <= MAX_SDF_BODIES) {
+ for (i = 0; i < numBodies; i++) {
+ PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i]));
+ }
+ }
+ return pylist;
}
// Reset the simulation to remove all loaded objects
-static PyObject *
-pybullet_resetSimulation(PyObject* self, PyObject* args)
-{
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
-
- {
- b3SharedMemoryStatusHandle statusHandle;
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
- }
- Py_INCREF(Py_None);
- return Py_None;
-}
-
-static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
-{
- int size;
- int bodyIndex, jointIndex, controlMode;
-
- double targetPosition=0;
- double targetVelocity=0;
- double maxForce=100000;
- double appliedForce = 0;
- double kp=0.1;
- double kd=1.0;
- int valid = 0;
-
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
-
- size= PySequence_Size(args);
- if (size==4)
- {
- double targetValue = 0;
- // see switch statement below for convertsions dependent on controlMode
- if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
- {
- PyErr_SetString(SpamError, "Error parsing arguments");
- return NULL;
- }
- valid = 1;
- switch (controlMode)
- {
- case CONTROL_MODE_POSITION_VELOCITY_PD:
- {
- targetPosition = targetValue;
- break;
- }
- case CONTROL_MODE_VELOCITY:
- {
- targetVelocity = targetValue;
- break;
- }
- case CONTROL_MODE_TORQUE:
- {
- appliedForce = targetValue;
- break;
- }
- default:
- {
- valid = 0;
- }
- }
-
- }
- if (size==5)
- {
- double targetValue = 0;
- //See switch statement for conversions
- if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
- {
- PyErr_SetString(SpamError, "Error parsing arguments");
- return NULL;
- }
- valid = 1;
-
- switch (controlMode)
- {
- case CONTROL_MODE_POSITION_VELOCITY_PD:
- {
- targetPosition = targetValue;
- break;
- }
- case CONTROL_MODE_VELOCITY:
- {
- targetVelocity = targetValue;
- break;
- }
- case CONTROL_MODE_TORQUE:
- {
- valid = 0;
- break;
- }
- default:
- {
- valid = 0;
- }
- }
- }
- if (size==6)
- {
- double gain;
- double targetValue = 0;
- if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain))
- {
- PyErr_SetString(SpamError, "Error parsing arguments");
- return NULL;
- }
- valid = 1;
-
- switch (controlMode)
- {
- case CONTROL_MODE_POSITION_VELOCITY_PD:
- {
- targetPosition = targetValue;
- kp = gain;
- break;
- }
- case CONTROL_MODE_VELOCITY:
- {
- targetVelocity = targetValue;
- kd = gain;
- break;
- }
- case CONTROL_MODE_TORQUE:
- {
- valid = 0;
- break;
- }
- default:
- {
- valid = 0;
- }
- }
- }
- if (size==8)
- {
- // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
- if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd))
- {
- PyErr_SetString(SpamError, "Error parsing arguments");
- return NULL;
- }
- valid = 1;
- }
-
-
-
- if (valid)
- {
-
- int numJoints;
- b3SharedMemoryCommandHandle commandHandle;
- b3SharedMemoryStatusHandle statusHandle;
- struct b3JointInfo info;
-
- numJoints = b3GetNumJoints(sm,bodyIndex);
- if ((jointIndex >= numJoints) || (jointIndex < 0))
- {
- PyErr_SetString(SpamError, "Joint index out-of-range.");
- return NULL;
- }
-
- if ((controlMode != CONTROL_MODE_VELOCITY) &&
- (controlMode != CONTROL_MODE_TORQUE) &&
- (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
- {
- PyErr_SetString(SpamError, "Illegral control mode.");
- return NULL;
- }
-
- commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode);
-
- b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
-
- switch (controlMode)
- {
- case CONTROL_MODE_VELOCITY:
- {
- b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
- b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
- b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
- break;
- }
-
- case CONTROL_MODE_TORQUE:
- {
- b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce);
- break;
- }
-
- case CONTROL_MODE_POSITION_VELOCITY_PD:
- {
- b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
- b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
- b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
- b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
- b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
- break;
- }
- default:
- {
- }
- };
-
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
-
- Py_INCREF(Py_None);
- return Py_None;
- }
- PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
+static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
+ }
+
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ statusHandle = b3SubmitClientCommandAndWaitStatus(
+ sm, b3InitResetSimulationCommand(sm));
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
}
-static PyObject *
-pybullet_setRealTimeSimulation(PyObject* self, PyObject* args)
-{
- if (0 == sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
+static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) {
+ int size;
+ int bodyIndex, jointIndex, controlMode;
- {
- int enableRealTimeSimulation = 0;
- int ret;
+ double targetPosition = 0.0;
+ double targetVelocity = 0.0;
+ double maxForce = 100000.0;
+ double appliedForce = 0.0;
+ double kp = 0.1;
+ double kd = 1.0;
+ int valid = 0;
- b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
- b3SharedMemoryStatusHandle statusHandle;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
- if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation))
- {
- PyErr_SetString(SpamError, "setRealTimeSimulation expected a single value (integer).");
- return NULL;
- }
- ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
+ size = PySequence_Size(args);
+ if (size == 4) {
+ double targetValue = 0.0;
+ // see switch statement below for convertsions dependent on controlMode
+ if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode,
+ &targetValue)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
+ switch (controlMode) {
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ targetPosition = targetValue;
+ break;
+ }
+ case CONTROL_MODE_VELOCITY: {
+ targetVelocity = targetValue;
+ break;
+ }
+ case CONTROL_MODE_TORQUE: {
+ appliedForce = targetValue;
+ break;
+ }
+ default: { valid = 0; }
+ }
+ }
+ if (size == 5) {
+ double targetValue = 0.0;
+ // See switch statement for conversions
+ if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode,
+ &targetValue, &maxForce)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
- //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
- }
+ switch (controlMode) {
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ targetPosition = targetValue;
+ break;
+ }
+ case CONTROL_MODE_VELOCITY: {
+ targetVelocity = targetValue;
+ break;
+ }
+ case CONTROL_MODE_TORQUE: {
+ valid = 0;
+ break;
+ }
+ default: { valid = 0; }
+ }
+ }
+ if (size == 6) {
+ double gain = 0.0;
+ double targetValue = 0.0;
+ if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode,
+ &targetValue, &maxForce, &gain)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
- Py_INCREF(Py_None);
- return Py_None;
+ switch (controlMode) {
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ targetPosition = targetValue;
+ kp = gain;
+ break;
+ }
+ case CONTROL_MODE_VELOCITY: {
+ targetVelocity = targetValue;
+ kd = gain;
+ break;
+ }
+ case CONTROL_MODE_TORQUE: {
+ valid = 0;
+ break;
+ }
+ default: { valid = 0; }
+ }
+ }
+ if (size == 8) {
+ // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
+ if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex,
+ &controlMode, &targetPosition, &targetVelocity,
+ &maxForce, &kp, &kd)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
+ }
+
+ if (valid) {
+ int numJoints;
+ b3SharedMemoryCommandHandle commandHandle;
+ b3SharedMemoryStatusHandle statusHandle;
+ struct b3JointInfo info;
+
+ numJoints = b3GetNumJoints(sm, bodyIndex);
+ if ((jointIndex >= numJoints) || (jointIndex < 0)) {
+ PyErr_SetString(SpamError, "Joint index out-of-range.");
+ return NULL;
+ }
+
+ if ((controlMode != CONTROL_MODE_VELOCITY) &&
+ (controlMode != CONTROL_MODE_TORQUE) &&
+ (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) {
+ PyErr_SetString(SpamError, "Illegral control mode.");
+ return NULL;
+ }
+
+ commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode);
+
+ b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
+
+ switch (controlMode) {
+ case CONTROL_MODE_VELOCITY: {
+ b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
+ targetVelocity);
+ b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+ b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
+ break;
+ }
+
+ case CONTROL_MODE_TORQUE: {
+ b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex,
+ appliedForce);
+ break;
+ }
+
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex,
+ targetPosition);
+ b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+ b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
+ targetVelocity);
+ b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+ b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
+ break;
+ }
+ default: {}
+ };
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+
+ Py_INCREF(Py_None);
+ return Py_None;
+ }
+ PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
+ return NULL;
}
+static PyObject* pybullet_setRealTimeSimulation(PyObject* self,
+ PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+ {
+ int enableRealTimeSimulation = 0;
+ int ret;
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) {
+ PyErr_SetString(
+ SpamError,
+ "setRealTimeSimulation expected a single value (integer).");
+ return NULL;
+ }
+ ret =
+ b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
// Set the gravity of the world with (x, y, z) arguments
-static PyObject *
-pybullet_setGravity(PyObject* self, PyObject* args)
-{
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
+static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ double gravX = 0.0;
+ double gravY = 0.0;
+ double gravZ = -10.0;
+ int ret;
+
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "ddd", &gravX, &gravY, &gravZ)) {
+ PyErr_SetString(SpamError, "setGravity expected (x,y,z) values.");
+ return NULL;
}
+ ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ);
+ // ret = b3PhysicsParamSetTimeStep(command, timeStep);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
- {
- float gravX=0;
- float gravY=0;
- float gravZ=-10;
- int ret;
+ Py_INCREF(Py_None);
+ return Py_None;
+}
- b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
- b3SharedMemoryStatusHandle statusHandle;
+static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
- if (!PyArg_ParseTuple(args, "fff", &gravX,&gravY,&gravZ))
- {
- PyErr_SetString(SpamError, "setGravity expected (x,y,z) values.");
- return NULL;
- }
- ret = b3PhysicsParamSetGravity(command, gravX,gravY, gravZ);
- //ret = b3PhysicsParamSetTimeStep(command, timeStep);
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
- //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
- }
+ {
+ double timeStep = 0.001;
+ int ret;
- Py_INCREF(Py_None);
- return Py_None;
+
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "d", &timeStep)) {
+ PyErr_SetString(SpamError,
+ "setTimeStep expected a single value (double).");
+ return NULL;
+ }
+ ret = b3PhysicsParamSetTimeStep(command, timeStep);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
}
static PyObject *
-pybullet_setTimeStep(PyObject* self, PyObject* args)
+pybullet_setDefaultContactERP(PyObject* self, PyObject* args)
{
if (0==sm)
{
@@ -573,20 +518,20 @@ pybullet_setTimeStep(PyObject* self, PyObject* args)
}
{
- double timeStep=0.001;
+ double defaultContactERP=0.005;
int ret;
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
- if (!PyArg_ParseTuple(args, "d", &timeStep))
+ if (!PyArg_ParseTuple(args, "d", &defaultContactERP))
{
- PyErr_SetString(SpamError, "setTimeStep expected a single value (double).");
+ PyErr_SetString(SpamError, "default Contact ERP expected a single value (double).");
return NULL;
}
- ret = b3PhysicsParamSetTimeStep(command, timeStep);
+ ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP);
+
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
- //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
Py_INCREF(Py_None);
@@ -594,293 +539,262 @@ pybullet_setTimeStep(PyObject* self, PyObject* args)
}
-
// Internal function used to get the base position and orientation
// Orientation is returned in quaternions
-static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
-{
- basePosition[0] = 0.;
- basePosition[1] = 0.;
- basePosition[2] = 0.;
-
- baseOrientation[0] = 0.;
- baseOrientation[1] = 0.;
- baseOrientation[2] = 0.;
- baseOrientation[3] = 1.;
+static int pybullet_internalGetBasePositionAndOrientation(
+ int bodyIndex, double basePosition[3], double baseOrientation[3]) {
+ basePosition[0] = 0.;
+ basePosition[1] = 0.;
+ basePosition[2] = 0.;
- {
-
- {
- b3SharedMemoryCommandHandle cmd_handle =
- b3RequestActualStateCommandInit(sm, bodyIndex);
- b3SharedMemoryStatusHandle status_handle =
- b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+ baseOrientation[0] = 0.;
+ baseOrientation[1] = 0.;
+ baseOrientation[2] = 0.;
+ baseOrientation[3] = 1.;
- const int status_type = b3GetStatusType(status_handle);
- const double* actualStateQ;
- // const double* jointReactionForces[];
- int i;
+ {
+ {
+ b3SharedMemoryCommandHandle cmd_handle =
+ b3RequestActualStateCommandInit(sm, bodyIndex);
+ b3SharedMemoryStatusHandle status_handle =
+ b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
- if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
- {
- PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
- return 0;
- }
-
- b3GetStatusActualState(status_handle, 0/* body_unique_id */,
- 0/* num_degree_of_freedom_q */,
- 0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/,
- &actualStateQ , 0 /* actual_state_q_dot */,
- 0 /* joint_reaction_forces */);
+ const int status_type = b3GetStatusType(status_handle);
+ const double* actualStateQ;
+ // const double* jointReactionForces[];
+ int i;
- // printf("joint reaction forces=");
- // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
- // printf("%f ", jointReactionForces[i]);
- // }
- //now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
- //and orientation x,y,z,w = actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
- basePosition[0] = actualStateQ[0];
- basePosition[1] = actualStateQ[1];
- basePosition[2] = actualStateQ[2];
-
- baseOrientation[0] = actualStateQ[3];
- baseOrientation[1] = actualStateQ[4];
- baseOrientation[2] = actualStateQ[5];
- baseOrientation[3] = actualStateQ[6];
-
- }
- }
- return 1;
+ if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+ PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
+ return 0;
+ }
+
+ b3GetStatusActualState(
+ status_handle, 0 /* body_unique_id */,
+ 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
+ 0 /*root_local_inertial_frame*/, &actualStateQ,
+ 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */);
+
+ // printf("joint reaction forces=");
+ // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
+ // printf("%f ", jointReactionForces[i]);
+ // }
+ // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
+ // and orientation x,y,z,w =
+ // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
+ basePosition[0] = actualStateQ[0];
+ basePosition[1] = actualStateQ[1];
+ basePosition[2] = actualStateQ[2];
+
+ baseOrientation[0] = actualStateQ[3];
+ baseOrientation[1] = actualStateQ[4];
+ baseOrientation[2] = actualStateQ[5];
+ baseOrientation[3] = actualStateQ[6];
+ }
+ }
+ return 1;
}
// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion
// values for the base link of your object
// Object is retrieved based on body index, which is the order
// the object was loaded into the simulation (0-based)
-static PyObject *
-pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
-{
- int bodyIndex = -1;
- double basePosition[3];
- double baseOrientation[4];
- PyObject *pylistPos;
- PyObject *pylistOrientation;
+static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
+ PyObject* args) {
+ int bodyIndex = -1;
+ double basePosition[3];
+ double baseOrientation[4];
+ PyObject* pylistPos;
+ PyObject* pylistOrientation;
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
-
- if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
- {
- PyErr_SetString(SpamError, "Expected a body index (integer).");
- return NULL;
- }
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
- if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation))
- {
- PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?).");
- return NULL;
- }
-
- {
-
- PyObject *item;
- int i;
- int num=3;
- pylistPos = PyTuple_New(num);
- for (i = 0; i < num; i++)
- {
- item = PyFloat_FromDouble(basePosition[i]);
- PyTuple_SetItem(pylistPos, i, item);
- }
-
- }
+ if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
+ PyErr_SetString(SpamError, "Expected a body index (integer).");
+ return NULL;
+ }
- {
-
- PyObject *item;
- int i;
- int num=4;
- pylistOrientation = PyTuple_New(num);
- for (i = 0; i < num; i++)
- {
- item = PyFloat_FromDouble(baseOrientation[i]);
- PyTuple_SetItem(pylistOrientation, i, item);
- }
-
+ if (0 == pybullet_internalGetBasePositionAndOrientation(
+ bodyIndex, basePosition, baseOrientation)) {
+ PyErr_SetString(SpamError,
+ "GetBasePositionAndOrientation failed (#joints/links "
+ "exceeds maximum?).");
+ return NULL;
+ }
+
+ {
+ PyObject* item;
+ int i;
+ int num = 3;
+ pylistPos = PyTuple_New(num);
+ for (i = 0; i < num; i++) {
+ item = PyFloat_FromDouble(basePosition[i]);
+ PyTuple_SetItem(pylistPos, i, item);
}
-
- {
- PyObject *pylist;
- pylist = PyTuple_New(2);
- PyTuple_SetItem(pylist,0,pylistPos);
- PyTuple_SetItem(pylist,1,pylistOrientation);
- return pylist;
+ }
+
+ {
+ PyObject* item;
+ int i;
+ int num = 4;
+ pylistOrientation = PyTuple_New(num);
+ for (i = 0; i < num; i++) {
+ item = PyFloat_FromDouble(baseOrientation[i]);
+ PyTuple_SetItem(pylistOrientation, i, item);
}
-
+ }
+
+ {
+ PyObject* pylist;
+ pylist = PyTuple_New(2);
+ PyTuple_SetItem(pylist, 0, pylistPos);
+ PyTuple_SetItem(pylist, 1, pylistOrientation);
+ return pylist;
+ }
}
// Return the number of joints in an object based on
// body index; body index is based on order of sequence
// the object is loaded into simulation
-static PyObject *
-pybullet_getNumJoints(PyObject* self, PyObject* args)
-{
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
+static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
- {
- int bodyIndex = -1;
- int numJoints=0;
- if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
- {
- PyErr_SetString(SpamError, "Expected a body index (integer).");
- return NULL;
- }
- numJoints = b3GetNumJoints(sm,bodyIndex);
+ {
+ int bodyIndex = -1;
+ int numJoints = 0;
+ if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
+ PyErr_SetString(SpamError, "Expected a body index (integer).");
+ return NULL;
+ }
+ numJoints = b3GetNumJoints(sm, bodyIndex);
#if PY_MAJOR_VERSION >= 3
- return PyLong_FromLong(numJoints);
+ return PyLong_FromLong(numJoints);
#else
- return PyInt_FromLong(numJoints);
+ return PyInt_FromLong(numJoints);
#endif
- }
+ }
}
// Initalize all joint positions given a list of values
-static PyObject*
-pybullet_resetJointState(PyObject* self, PyObject* args)
-{
- int size;
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
-
-
- size= PySequence_Size(args);
-
- if (size==3)
- {
- int bodyIndex;
- int jointIndex;
- double targetValue;
-
- if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue))
- {
- b3SharedMemoryCommandHandle commandHandle;
- b3SharedMemoryStatusHandle statusHandle;
- int numJoints;
-
- numJoints = b3GetNumJoints(sm,bodyIndex);
- if ((jointIndex >= numJoints) || (jointIndex < 0))
- {
- PyErr_SetString(SpamError, "Joint index out-of-range.");
- return NULL;
- }
-
- commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
-
- b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue);
-
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
- Py_INCREF(Py_None);
- return Py_None;
- }
-
- }
- PyErr_SetString(SpamError, "error in resetJointState.");
+static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) {
+ int size;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
-}
+ }
-// Reset the position and orientation of the base/root link, position [x,y,z] and orientation quaternion [x,y,z,w]
-static PyObject*
-pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args)
-{
- int size;
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
+ size = PySequence_Size(args);
+
+ if (size == 3) {
+ int bodyIndex;
+ int jointIndex;
+ double targetValue;
+
+ if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) {
+ b3SharedMemoryCommandHandle commandHandle;
+ b3SharedMemoryStatusHandle statusHandle;
+ int numJoints;
+
+ numJoints = b3GetNumJoints(sm, bodyIndex);
+ if ((jointIndex >= numJoints) || (jointIndex < 0)) {
+ PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
- }
-
-
- size= PySequence_Size(args);
-
- if (size==3)
- {
- int bodyIndex;
- PyObject* posObj;
- PyObject* ornObj;
- double pos[3];
- double orn[4];//as a quaternion
-
- if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj))
- {
- b3SharedMemoryCommandHandle commandHandle;
- b3SharedMemoryStatusHandle statusHandle;
-
- {
- PyObject* seq;
- int len,i;
- seq = PySequence_Fast(posObj, "expected a sequence");
- len = PySequence_Size(posObj);
- if (len==3)
- {
- for (i = 0; i < 3; i++)
- {
- pos[i] = pybullet_internalGetFloatFromSequence(seq,i);
- }
- } else
- {
- PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
- Py_DECREF(seq);
- return NULL;
- }
- Py_DECREF(seq);
- }
+ }
- {
- PyObject* seq;
- int len,i;
- seq = PySequence_Fast(ornObj, "expected a sequence");
- len = PySequence_Size(ornObj);
- if (len==4)
- {
- for (i = 0; i < 4; i++)
- {
- orn[i] = pybullet_internalGetFloatFromSequence(seq,i);
- }
- } else
- {
- PyErr_SetString(SpamError, "orientation needs a 4 coordinates, quaternion [x,y,z,w].");
- Py_DECREF(seq);
- return NULL;
- }
- Py_DECREF(seq);
- }
-
- commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
-
- b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]);
- b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]);
+ commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
- Py_INCREF(Py_None);
- return Py_None;
- }
-
+ b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex,
+ targetValue);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ Py_INCREF(Py_None);
+ return Py_None;
}
- PyErr_SetString(SpamError, "error in resetJointState.");
- return NULL;
+ }
+ PyErr_SetString(SpamError, "error in resetJointState.");
+ return NULL;
}
+// Reset the position and orientation of the base/root link, position [x,y,z]
+// and orientation quaternion [x,y,z,w]
+static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
+ PyObject* args) {
+ int size;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ size = PySequence_Size(args);
+
+ if (size == 3) {
+ int bodyIndex;
+ PyObject* posObj;
+ PyObject* ornObj;
+ double pos[3];
+ double orn[4]; // as a quaternion
+
+ if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) {
+ b3SharedMemoryCommandHandle commandHandle;
+ b3SharedMemoryStatusHandle statusHandle;
+
+ {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(posObj, "expected a sequence");
+ len = PySequence_Size(posObj);
+ if (len == 3) {
+ for (i = 0; i < 3; i++) {
+ pos[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ }
+
+ {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(ornObj, "expected a sequence");
+ len = PySequence_Size(ornObj);
+ if (len == 4) {
+ for (i = 0; i < 4; i++) {
+ orn[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(
+ SpamError,
+ "orientation needs a 4 coordinates, quaternion [x,y,z,w].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ }
+
+ commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
+
+ b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]);
+ b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1],
+ orn[2], orn[3]);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ Py_INCREF(Py_None);
+ return Py_None;
+ }
+ }
+ PyErr_SetString(SpamError, "error in resetJointState.");
+ return NULL;
+}
// Get the a single joint info for a specific bodyIndex
//
@@ -892,83 +806,66 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args)
// index, name, type, q-index, u-index,
// flags, joint damping, joint friction
//
-// The format of the returned list is
+// The format of the returned list is
// [int, str, int, int, int, int, float, float]
//
// TODO(hellojas): get joint positions for a body
-static PyObject*
-pybullet_getJointInfo(PyObject* self, PyObject* args)
-{
- PyObject *pyListJointInfo;
-
+static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) {
+ PyObject* pyListJointInfo;
+
struct b3JointInfo info;
-
+
int bodyIndex = -1;
int jointIndex = -1;
- int jointInfoSize = 8; //size of struct b3JointInfo
-
- int size= PySequence_Size(args);
+ int jointInfoSize = 8; // size of struct b3JointInfo
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
+ int size = PySequence_Size(args);
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
-
- if (size==2) // get body index and joint index
+ if (size == 2) // get body index and joint index
{
- if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
- {
-
+ if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
// printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex);
- pyListJointInfo = PyTuple_New(jointInfoSize);
+ pyListJointInfo = PyTuple_New(jointInfoSize);
- if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info))
- {
-
- // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
- // info.m_jointIndex,
- // info.m_jointName,
- // info.m_jointType,
- // info.m_qIndex,
- // info.m_uIndex);
- // printf(" flags=%d jointDamping=%f jointFriction=%f\n",
- // info.m_flags,
- // info.m_jointDamping,
- // info.m_jointFriction);
- PyTuple_SetItem(pyListJointInfo, 0,
- PyInt_FromLong(info.m_jointIndex));
- PyTuple_SetItem(pyListJointInfo, 1,
- PyString_FromString(info.m_jointName));
- PyTuple_SetItem(pyListJointInfo, 2,
- PyInt_FromLong(info.m_jointType));
- PyTuple_SetItem(pyListJointInfo, 3,
- PyInt_FromLong(info.m_qIndex));
- PyTuple_SetItem(pyListJointInfo, 4,
- PyInt_FromLong(info.m_uIndex));
- PyTuple_SetItem(pyListJointInfo, 5,
- PyInt_FromLong(info.m_flags));
- PyTuple_SetItem(pyListJointInfo, 6,
- PyFloat_FromDouble(info.m_jointDamping));
- PyTuple_SetItem(pyListJointInfo, 7,
- PyFloat_FromDouble(info.m_jointFriction));
- return pyListJointInfo;
- }
- else
- {
- PyErr_SetString(SpamError, "GetJointInfo failed.");
- return NULL;
- }
+ if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) {
+ // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
+ // info.m_jointIndex,
+ // info.m_jointName,
+ // info.m_jointType,
+ // info.m_qIndex,
+ // info.m_uIndex);
+ // printf(" flags=%d jointDamping=%f jointFriction=%f\n",
+ // info.m_flags,
+ // info.m_jointDamping,
+ // info.m_jointFriction);
+ PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex));
+ PyTuple_SetItem(pyListJointInfo, 1,
+ PyString_FromString(info.m_jointName));
+ PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType));
+ PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex));
+ PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex));
+ PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags));
+ PyTuple_SetItem(pyListJointInfo, 6,
+ PyFloat_FromDouble(info.m_jointDamping));
+ PyTuple_SetItem(pyListJointInfo, 7,
+ PyFloat_FromDouble(info.m_jointFriction));
+ return pyListJointInfo;
+ } else {
+ PyErr_SetString(SpamError, "GetJointInfo failed.");
+ return NULL;
+ }
}
}
-
- Py_INCREF(Py_None);
- return Py_None;
-}
+ Py_INCREF(Py_None);
+ return Py_None;
+}
// Returns the state of a specific joint in a given bodyIndex
//
@@ -980,111 +877,99 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
// position, velocity, force torque (6 values), and motor torque
// The returned pylist is an array of [float, float, float[6], float]
-// TODO(hellojas): check accuracy of position and velocity
+// TODO(hellojas): check accuracy of position and velocity
// TODO(hellojas): check force torque values
-static PyObject*
-pybullet_getJointState(PyObject* self, PyObject* args)
-{
- PyObject *pyListJointForceTorque;
- PyObject *pyListJointState;
- PyObject *item;
-
+static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
+ PyObject* pyListJointForceTorque;
+ PyObject* pyListJointState;
+ PyObject* item;
+
struct b3JointInfo info;
struct b3JointSensorState sensorState;
-
+
int bodyIndex = -1;
int jointIndex = -1;
- int sensorStateSize = 4; // size of struct b3JointSensorState
- int forceTorqueSize = 6; // size of force torque list from b3JointSensorState
+ int sensorStateSize = 4; // size of struct b3JointSensorState
+ int forceTorqueSize = 6; // size of force torque list from b3JointSensorState
int i, j;
-
-
- int size= PySequence_Size(args);
- if (0==sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
+ int size = PySequence_Size(args);
- if (size==2) // get body index and joint index
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (size == 2) // get body index and joint index
{
- if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
- {
- int status_type = 0;
- b3SharedMemoryCommandHandle cmd_handle =
+ if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
+ int status_type = 0;
+ b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
- b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
-
- status_type = b3GetStatusType(status_handle);
- if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
- {
- PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
- return NULL;
- }
+ b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
- pyListJointState = PyTuple_New(sensorStateSize);
- pyListJointForceTorque = PyTuple_New(forceTorqueSize);
+ status_type = b3GetStatusType(status_handle);
+ if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+ PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
+ return NULL;
+ }
-
- b3GetJointState(sm, status_handle, jointIndex, &sensorState);
-
- PyTuple_SetItem(pyListJointState, 0,
- PyFloat_FromDouble(sensorState.m_jointPosition));
- PyTuple_SetItem(pyListJointState, 1,
- PyFloat_FromDouble(sensorState.m_jointVelocity));
-
- for (j = 0; j < forceTorqueSize; j++) {
- PyTuple_SetItem(pyListJointForceTorque, j,
- PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
+ pyListJointState = PyTuple_New(sensorStateSize);
+ pyListJointForceTorque = PyTuple_New(forceTorqueSize);
+
+ b3GetJointState(sm, status_handle, jointIndex, &sensorState);
+
+ PyTuple_SetItem(pyListJointState, 0,
+ PyFloat_FromDouble(sensorState.m_jointPosition));
+ PyTuple_SetItem(pyListJointState, 1,
+ PyFloat_FromDouble(sensorState.m_jointVelocity));
+
+ for (j = 0; j < forceTorqueSize; j++) {
+ PyTuple_SetItem(pyListJointForceTorque, j,
+ PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
+ }
+
+ PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
+
+ PyTuple_SetItem(pyListJointState, 3,
+ PyFloat_FromDouble(sensorState.m_jointMotorTorque));
+
+ return pyListJointState;
}
-
- PyTuple_SetItem(pyListJointState, 2,
- pyListJointForceTorque);
-
- PyTuple_SetItem(pyListJointState, 3,
- PyFloat_FromDouble(sensorState.m_jointMotorTorque));
-
- return pyListJointState;
- }
- } else
- {
- PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index).");
- return NULL;
+ } else {
+ PyErr_SetString(
+ SpamError,
+ "getJointState expects 2 arguments (objectUniqueId and joint index).");
+ return NULL;
}
-
- Py_INCREF(Py_None);
- return Py_None;
+
+ Py_INCREF(Py_None);
+ return Py_None;
}
-
-
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// matrix - float[16] which will be set by values from objMat
-static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
-{
- int i, len;
- PyObject* seq;
+static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
+ int i, len;
+ PyObject* seq;
- seq = PySequence_Fast(objMat, "expected a sequence");
- len = PySequence_Size(objMat);
- if (len==16)
- {
- for (i = 0; i < len; i++)
- {
- matrix[i] = pybullet_internalGetFloatFromSequence(seq,i);
- }
- Py_DECREF(seq);
- return 1;
+ seq = PySequence_Fast(objMat, "expected a sequence");
+ len = PySequence_Size(objMat);
+ if (len == 16) {
+ for (i = 0; i < len; i++) {
+ matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
- return 0;
+ return 1;
+ }
+ Py_DECREF(seq);
+ return 0;
}
// internal function to set a float vector[3]
@@ -1092,137 +977,140 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
// a view and projection matrix in renderImage()
//
// // Args:
-// matrix - float[16] which will be set by values from objMat
-static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
-{
- int i, len;
- PyObject* seq;
+// vector - float[3] which will be set by values from objMat
+static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
+ int i, len;
+ PyObject* seq;
- seq = PySequence_Fast(objMat, "expected a sequence");
- len = PySequence_Size(objMat);
- if (len==3)
- {
- for (i = 0; i < len; i++)
- {
- vector[i] = pybullet_internalGetFloatFromSequence(seq,i);
- }
- Py_DECREF(seq);
- return 1;
+ seq = PySequence_Fast(objMat, "expected a sequence");
+ len = PySequence_Size(objMat);
+ if (len == 3) {
+ for (i = 0; i < len; i++) {
+ vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
- return 0;
+ return 1;
+ }
+ Py_DECREF(seq);
+ return 0;
}
+static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
+ int size = PySequence_Size(args);
+ int objectUniqueIdA = -1;
+ int objectUniqueIdB = -1;
+ b3SharedMemoryCommandHandle commandHandle;
+ struct b3ContactInformation contactPointData;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ int i;
+ PyObject* pyResultList = 0;
+ if (size == 1) {
+ if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) {
+ PyErr_SetString(SpamError, "Error parsing object unique id");
+ return NULL;
+ }
+ }
+ if (size == 2) {
+ if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA, &objectUniqueIdB)) {
+ PyErr_SetString(SpamError, "Error parsing object unique id");
+ return NULL;
+ }
+ }
-static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args)
-{
- int size= PySequence_Size(args);
- int objectUniqueIdA = -1;
- int objectUniqueIdB = -1;
- b3SharedMemoryCommandHandle commandHandle;
- struct b3ContactInformation contactPointData;
- b3SharedMemoryStatusHandle statusHandle;
- int statusType;
-
- PyObject* pyResultList=0;
-
- if (size==1)
- {
- if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA))
- {
- PyErr_SetString(SpamError, "Error parsing object unique id");
- return NULL;
- }
+ commandHandle = b3InitRequestContactPointInformation(sm);
+ b3SetContactFilterBodyA(commandHandle, objectUniqueIdA);
+ b3SetContactFilterBodyB(commandHandle, objectUniqueIdB);
+ b3SubmitClientCommand(sm, commandHandle);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
+ /*
+ 0 int m_contactFlags;
+ 1 int m_bodyUniqueIdA;
+ 2 int m_bodyUniqueIdB;
+ 3 int m_linkIndexA;
+ 4 int m_linkIndexB;
+ 5-6-7 double m_positionOnAInWS[3];//contact point location on object A,
+ in world space coordinates
+ 8-9-10 double m_positionOnBInWS[3];//contact point location on object
+ A, in world space coordinates
+ 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact
+ normal, pointing from object B towards object A
+ 14 double m_contactDistance;//negative number is penetration, positive
+ is distance.
+
+ 15 double m_normalForce;
+ */
+
+ b3GetContactPointInformation(sm, &contactPointData);
+ pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
+ for (i = 0; i < contactPointData.m_numContactPoints; i++) {
+ PyObject* contactObList = PyTuple_New(16); // see above 16 fields
+ PyObject* item;
+ item =
+ PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
+ PyTuple_SetItem(contactObList, 0, item);
+ item = PyInt_FromLong(
+ contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
+ PyTuple_SetItem(contactObList, 1, item);
+ item = PyInt_FromLong(
+ contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
+ PyTuple_SetItem(contactObList, 2, item);
+ item =
+ PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
+ PyTuple_SetItem(contactObList, 3, item);
+ item =
+ PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
+ PyTuple_SetItem(contactObList, 4, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
+ PyTuple_SetItem(contactObList, 5, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
+ PyTuple_SetItem(contactObList, 6, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
+ PyTuple_SetItem(contactObList, 7, item);
+
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
+ PyTuple_SetItem(contactObList, 8, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
+ PyTuple_SetItem(contactObList, 9, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
+ PyTuple_SetItem(contactObList, 10, item);
+
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
+ PyTuple_SetItem(contactObList, 11, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
+ PyTuple_SetItem(contactObList, 12, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
+ PyTuple_SetItem(contactObList, 13, item);
+
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactDistance);
+ PyTuple_SetItem(contactObList, 14, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_normalForce);
+ PyTuple_SetItem(contactObList, 15, item);
+
+ PyTuple_SetItem(pyResultList, i, contactObList);
}
- if (size==2)
- {
- if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB))
- {
- PyErr_SetString(SpamError, "Error parsing object unique id");
- return NULL;
- }
- }
-
- commandHandle = b3InitRequestContactPointInformation(sm);
- b3SetContactFilterBodyA(commandHandle,objectUniqueIdA);
- b3SetContactFilterBodyB(commandHandle,objectUniqueIdB);
- b3SubmitClientCommand(sm, commandHandle);
- int i;
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
- statusType = b3GetStatusType(statusHandle);
- if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED)
- {
-
- /*
- 0 int m_contactFlags;
- 1 int m_bodyUniqueIdA;
- 2 int m_bodyUniqueIdB;
- 3 int m_linkIndexA;
- 4 int m_linkIndexB;
- 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
- 8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
- 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
- 14 double m_contactDistance;//negative number is penetration, positive is distance.
-
- 15 double m_normalForce;
- */
-
- b3GetContactPointInformation(sm, &contactPointData);
- pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
- for (i=0;i euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
+/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
+static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
+ PyObject* args) {
+ double squ;
+ double sqx;
+ double sqy;
+ double sqz;
-static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, PyObject* args)
-{
+ double quat[4];
+
+ PyObject* quatObj;
+
+ if (PyArg_ParseTuple(args, "O", &quatObj)) {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(quatObj, "expected a sequence");
+ len = PySequence_Size(quatObj);
+ if (len == 4) {
+ for (i = 0; i < 4; i++) {
+ quat[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ } else {
+ PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
+ return NULL;
+ }
+
+ {
double rpy[3];
-
- PyObject* eulerObj;
-
- if (PyArg_ParseTuple(args, "O", &eulerObj))
+ double sarg;
+ sqx = quat[0] * quat[0];
+ sqy = quat[1] * quat[1];
+ sqz = quat[2] * quat[2];
+ squ = quat[3] * quat[3];
+ rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
+ squ - sqx - sqy + sqz);
+ sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
+ rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
+ : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
+ rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
+ squ + sqx - sqy - sqz);
{
- PyObject* seq;
- int len,i;
- seq = PySequence_Fast(eulerObj, "expected a sequence");
- len = PySequence_Size(eulerObj);
- if (len==3)
- {
- for (i = 0; i < 3; i++)
- {
- rpy[i] = pybullet_internalGetFloatFromSequence(seq,i);
- }
- } else
- {
- PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw].");
- Py_DECREF(seq);
- return NULL;
+ PyObject* pylist;
+ int i;
+ pylist = PyTuple_New(3);
+ for (i = 0; i < 3; i++)
+ PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i]));
+ return pylist;
+ }
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+/// Given an object id, joint positions, joint velocities and joint
+/// accelerations,
+/// compute the joint forces using Inverse Dynamics
+static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
+ PyObject* args) {
+ int size;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ size = PySequence_Size(args);
+ if (size == 4) {
+ int bodyIndex;
+ PyObject* objPositionsQ;
+ PyObject* objVelocitiesQdot;
+ PyObject* objAccelerations;
+
+ if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ,
+ &objVelocitiesQdot, &objAccelerations)) {
+ int szObPos = PySequence_Size(objPositionsQ);
+ int szObVel = PySequence_Size(objVelocitiesQdot);
+ int szObAcc = PySequence_Size(objAccelerations);
+ int numJoints = b3GetNumJoints(sm, bodyIndex);
+ if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) &&
+ (szObAcc == numJoints)) {
+ int szInBytes = sizeof(double) * numJoints;
+ int i;
+ PyObject* pylist = 0;
+ double* jointPositionsQ = (double*)malloc(szInBytes);
+ double* jointVelocitiesQdot = (double*)malloc(szInBytes);
+ double* jointAccelerations = (double*)malloc(szInBytes);
+ double* jointForcesOutput = (double*)malloc(szInBytes);
+
+ for (i = 0; i < numJoints; i++) {
+ jointPositionsQ[i] =
+ pybullet_internalGetFloatFromSequence(objPositionsQ, i);
+ jointVelocitiesQdot[i] =
+ pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i);
+ jointAccelerations[i] =
+ pybullet_internalGetFloatFromSequence(objAccelerations, i);
}
- Py_DECREF(seq);
- } else
- {
- PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw].");
+
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle commandHandle =
+ b3CalculateInverseDynamicsCommandInit(
+ sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot,
+ jointAccelerations);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+
+ statusType = b3GetStatusType(statusHandle);
+
+ if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) {
+ int bodyUniqueId;
+ int dofCount;
+
+ b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
+ &dofCount, 0);
+
+ if (dofCount) {
+ b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
+ jointForcesOutput);
+ {
+ {
+ int i;
+ pylist = PyTuple_New(dofCount);
+ for (i = 0; i < dofCount; i++)
+ PyTuple_SetItem(pylist, i,
+ PyFloat_FromDouble(jointForcesOutput[i]));
+ }
+ }
+ }
+
+ } else {
+ PyErr_SetString(SpamError,
+ "Internal error in calculateInverseDynamics");
+ }
+ }
+ free(jointPositionsQ);
+ free(jointVelocitiesQdot);
+ free(jointAccelerations);
+ free(jointForcesOutput);
+ if (pylist) return pylist;
+ } else {
+ PyErr_SetString(SpamError,
+ "calculateInverseDynamics numJoints needs to be "
+ "positive and [joint positions], [joint velocities], "
+ "[joint accelerations] need to match the number of "
+ "joints.");
return NULL;
+ }
+
+ } else {
+ PyErr_SetString(SpamError,
+ "calculateInverseDynamics expects 4 arguments, body "
+ "index, [joint positions], [joint velocities], [joint "
+ "accelerations].");
+ return NULL;
}
-
- {
- double phi, the, psi;
- double roll = rpy[0];
- double pitch = rpy[1];
- double yaw = rpy[2];
- phi = roll / 2.0;
- the = pitch / 2.0;
- psi = yaw / 2.0;
- {
- double quat[4] = {
- sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
- cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
- cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
- cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
-
- //normalize the quaternion
- double len = sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
- quat[0] /= len;
- quat[1] /= len;
- quat[2] /= len;
- quat[3] /= len;
- {
- PyObject *pylist;
- int i;
- pylist = PyTuple_New(4);
- for (i=0;i<4;i++)
- PyTuple_SetItem(pylist,i,PyFloat_FromDouble(quat[i]));
- return pylist;
- }
- }
- }
- Py_INCREF(Py_None);
- return Py_None;
-
+ } else {
+ PyErr_SetString(SpamError,
+ "calculateInverseDynamics expects 4 arguments, body index, "
+ "[joint positions], [joint velocities], [joint "
+ "accelerations].");
+ return NULL;
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
}
-///quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
-///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
-static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args)
-{
-
- double squ;
- double sqx;
- double sqy;
- double sqz;
-
- double quat[4];
-
- PyObject* quatObj;
-
- if (PyArg_ParseTuple(args, "O", &quatObj))
- {
- PyObject* seq;
- int len,i;
- seq = PySequence_Fast(quatObj, "expected a sequence");
- len = PySequence_Size(quatObj);
- if (len==4)
- {
- for (i = 0; i < 4; i++)
- {
- quat[i] = pybullet_internalGetFloatFromSequence(seq,i);
- }
- } else
- {
- PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
- Py_DECREF(seq);
- return NULL;
- }
- Py_DECREF(seq);
- } else
- {
- PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
- return NULL;
- }
-
- {
- double rpy[3];
- double sarg;
- sqx = quat[0] * quat[0];
- sqy = quat[1] * quat[1];
- sqz = quat[2] * quat[2];
- squ = quat[3] * quat[3];
- rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz);
- sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]);
- rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg));
- rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz);
- {
- PyObject *pylist;
- int i;
- pylist = PyTuple_New(3);
- for (i=0;i<3;i++)
- PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i]));
- return pylist;
- }
- }
- Py_INCREF(Py_None);
- return Py_None;
-}
-
-///Given an object id, joint positions, joint velocities and joint accelerations,
-///compute the joint forces using Inverse Dynamics
-static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args)
-{
- int size;
- if (0 == sm)
- {
- PyErr_SetString(SpamError, "Not connected to physics server.");
- return NULL;
- }
-
- size = PySequence_Size(args);
- if (size==4)
- {
-
- int bodyIndex;
- PyObject* objPositionsQ;
- PyObject* objVelocitiesQdot;
- PyObject* objAccelerations;
-
- if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, &objVelocitiesQdot, &objAccelerations))
- {
- int szObPos = PySequence_Size(objPositionsQ);
- int szObVel = PySequence_Size(objVelocitiesQdot);
- int szObAcc = PySequence_Size(objAccelerations);
- int numJoints = b3GetNumJoints(sm, bodyIndex);
- if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && (szObAcc == numJoints))
- {
- int szInBytes = sizeof(double)*numJoints;
- int i;
- PyObject* pylist = 0;
- double* jointPositionsQ = (double*)malloc(szInBytes);
- double* jointVelocitiesQdot = (double*)malloc(szInBytes);
- double* jointAccelerations = (double*)malloc(szInBytes);
- double* jointForcesOutput = (double*)malloc(szInBytes);
-
- for (i = 0; i < numJoints; i++)
- {
- jointPositionsQ[i] = pybullet_internalGetFloatFromSequence(objPositionsQ, i);
- jointVelocitiesQdot[i] = pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i);
- jointAccelerations[i] = pybullet_internalGetFloatFromSequence(objAccelerations, i);
- }
-
- {
- b3SharedMemoryStatusHandle statusHandle;
- int statusType;
- b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(sm,
- bodyIndex, jointPositionsQ, jointVelocitiesQdot, jointAccelerations);
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
-
- statusType = b3GetStatusType(statusHandle);
-
- if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
- {
- int bodyUniqueId;
- int dofCount;
-
- b3GetStatusInverseDynamicsJointForces(statusHandle,
- &bodyUniqueId,
- &dofCount,
- 0);
-
- if (dofCount)
- {
- b3GetStatusInverseDynamicsJointForces(statusHandle,
- 0,
- 0,
- jointForcesOutput);
- {
- {
-
- int i;
- pylist = PyTuple_New(dofCount);
- for (i = 0; i= 3
- static struct PyModuleDef moduledef = {
- PyModuleDef_HEAD_INIT,
- "pybullet", /* m_name */
- "Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)", /* m_doc */
- -1, /* m_size */
- SpamMethods, /* m_methods */
- NULL, /* m_reload */
- NULL, /* m_traverse */
- NULL, /* m_clear */
- NULL, /* m_free */
- };
+static struct PyModuleDef moduledef = {
+ PyModuleDef_HEAD_INIT, "pybullet", /* m_name */
+ "Python bindings for Bullet Physics Robotics API (also known as Shared "
+ "Memory API)", /* m_doc */
+ -1, /* m_size */
+ SpamMethods, /* m_methods */
+ NULL, /* m_reload */
+ NULL, /* m_traverse */
+ NULL, /* m_clear */
+ NULL, /* m_free */
+};
#endif
PyMODINIT_FUNC
@@ -1953,39 +1832,37 @@ initpybullet(void)
#endif
{
- PyObject *m;
+ PyObject* m;
#if PY_MAJOR_VERSION >= 3
- m = PyModule_Create(&moduledef);
+ m = PyModule_Create(&moduledef);
#else
- m = Py_InitModule3("pybullet",
- SpamMethods, "Python bindings for Bullet");
+ m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet");
#endif
#if PY_MAJOR_VERSION >= 3
- if (m == NULL)
- return m;
+ if (m == NULL) return m;
#else
- if (m == NULL)
- return;
+ if (m == NULL) return;
#endif
-
-
- PyModule_AddIntConstant (m, "SHARED_MEMORY", eCONNECT_SHARED_MEMORY); // user read
- PyModule_AddIntConstant (m, "DIRECT", eCONNECT_DIRECT); // user read
- PyModule_AddIntConstant (m, "GUI", eCONNECT_GUI); // user read
-
- PyModule_AddIntConstant (m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
- PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read
- PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read
-
- PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME);
- PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME);
-
- SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
- Py_INCREF(SpamError);
- PyModule_AddObject(m, "error", SpamError);
+
+ PyModule_AddIntConstant(m, "SHARED_MEMORY",
+ eCONNECT_SHARED_MEMORY); // user read
+ PyModule_AddIntConstant(m, "DIRECT", eCONNECT_DIRECT); // user read
+ PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read
+
+ PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
+ PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
+ CONTROL_MODE_VELOCITY); // user read
+ PyModule_AddIntConstant(m, "POSITION_CONTROL",
+ CONTROL_MODE_POSITION_VELOCITY_PD); // user read
+
+ PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
+ PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME);
+
+ SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
+ Py_INCREF(SpamError);
+ PyModule_AddObject(m, "error", SpamError);
#if PY_MAJOR_VERSION >= 3
- return m;
+ return m;
#endif
}
-
diff --git a/src/Bullet3Common/b3Scalar.h b/src/Bullet3Common/b3Scalar.h
index 60b7f1cfd..16a9ac7a7 100644
--- a/src/Bullet3Common/b3Scalar.h
+++ b/src/Bullet3Common/b3Scalar.h
@@ -55,7 +55,7 @@ inline int b3GetVersion()
//#define B3_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
-// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+ #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define B3_FORCE_INLINE __forceinline
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index 032a5828d..e9c889b2c 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -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;
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
index a96809f91..5e08609e3 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -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;
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index 4b2986a00..0536a2ea2 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -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);
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 977b9a02f..66d11417d 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -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
diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
index 1bb7a7b99..5ae072ea0 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
@@ -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;
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 5e1677e27..57ea4e69e 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -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);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 119a24c60..01a59890c 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -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;
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index b40c6d72b..9633acaab 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -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
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index 326a6ac48..accd62a00 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -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;
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
index a8bcf606c..ceac3b65e 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
@@ -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)
{
diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp
index 8e6577912..c34f98941 100644
--- a/src/BulletInverseDynamics/IDConfig.hpp
+++ b/src/BulletInverseDynamics/IDConfig.hpp
@@ -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)
diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
index 8d9aa77eb..847e5c6c7 100644
--- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
@@ -609,6 +609,7 @@ static inline int jointNumDoFs(const JointType &type) {
error_message("invalid joint type\n");
// TODO add configurable abort/crash function
abort();
+ return 0;
}
int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h
index 011fa1392..df907e128 100644
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -58,7 +58,7 @@ inline int btGetVersion()
//#define BT_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
-// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+ #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define SIMD_FORCE_INLINE __forceinline
diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt
index b7bc9a255..496ca5076 100644
--- a/test/SharedMemory/CMakeLists.txt
+++ b/test/SharedMemory/CMakeLists.txt
@@ -13,7 +13,7 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA
LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest
+ BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK
)
IF (NOT WIN32)
@@ -23,6 +23,8 @@ ENDIF()
ADD_EXECUTABLE(Test_PhysicsClientServer
gtestwrap.cpp
../../examples/SharedMemory/PhysicsClient.cpp
+ ../../examples/SharedMemory/IKTrajectoryHelper.cpp
+ ../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua
index 466ad76a4..1974ba471 100644
--- a/test/SharedMemory/premake4.lua
+++ b/test/SharedMemory/premake4.lua
@@ -43,11 +43,14 @@ project ("Test_PhysicsServerLoopBack")
"Bullet3Common",
"BulletDynamics",
"BulletCollision",
+ "BussIK",
"LinearMath"
}
files {
"test.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -112,12 +115,15 @@ project ("Test_PhysicsServerLoopBack")
"BulletWorldImporter",
"Bullet3Common",
"BulletDynamics",
- "BulletCollision",
+ "BulletCollision",
+ "BussIK",
"LinearMath"
}
files {
"test.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -187,7 +193,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
-- }
hasCL = findOpenCL("clew")
- links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+ links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"}
initOpenGL()
initGlew()
@@ -214,6 +220,8 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
files {
"test.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/PhysicsClient.cpp",