diff --git a/VERSION b/VERSION
index 7cf322cf9..0104e57c4 100644
--- a/VERSION
+++ b/VERSION
@@ -1 +1 @@
-2.85
+2.86
diff --git a/data/MPL/LICENSE.txt b/data/MPL/LICENSE.txt
new file mode 100644
index 000000000..a89a42bce
--- /dev/null
+++ b/data/MPL/LICENSE.txt
@@ -0,0 +1,19 @@
+
\ No newline at end of file
diff --git a/data/MPL/MPL.xml b/data/MPL/MPL.xml
new file mode 100644
index 000000000..5800a1200
--- /dev/null
+++ b/data/MPL/MPL.xml
@@ -0,0 +1,473 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/MPL/mesh/index0.STL b/data/MPL/mesh/index0.STL
new file mode 100644
index 000000000..1189fb830
Binary files /dev/null and b/data/MPL/mesh/index0.STL differ
diff --git a/data/MPL/mesh/index1.STL b/data/MPL/mesh/index1.STL
new file mode 100644
index 000000000..9c7a277da
Binary files /dev/null and b/data/MPL/mesh/index1.STL differ
diff --git a/data/MPL/mesh/index2.STL b/data/MPL/mesh/index2.STL
new file mode 100644
index 000000000..94e5b4d4f
Binary files /dev/null and b/data/MPL/mesh/index2.STL differ
diff --git a/data/MPL/mesh/index3.STL b/data/MPL/mesh/index3.STL
new file mode 100644
index 000000000..acdcd807e
Binary files /dev/null and b/data/MPL/mesh/index3.STL differ
diff --git a/data/MPL/mesh/middle0.STL b/data/MPL/mesh/middle0.STL
new file mode 100644
index 000000000..1189fb830
Binary files /dev/null and b/data/MPL/mesh/middle0.STL differ
diff --git a/data/MPL/mesh/middle1.STL b/data/MPL/mesh/middle1.STL
new file mode 100644
index 000000000..b29f6e640
Binary files /dev/null and b/data/MPL/mesh/middle1.STL differ
diff --git a/data/MPL/mesh/middle2.STL b/data/MPL/mesh/middle2.STL
new file mode 100644
index 000000000..0ae92882a
Binary files /dev/null and b/data/MPL/mesh/middle2.STL differ
diff --git a/data/MPL/mesh/middle3.STL b/data/MPL/mesh/middle3.STL
new file mode 100644
index 000000000..acdcd807e
Binary files /dev/null and b/data/MPL/mesh/middle3.STL differ
diff --git a/data/MPL/mesh/palm.STL b/data/MPL/mesh/palm.STL
new file mode 100644
index 000000000..7a4e9d0ae
Binary files /dev/null and b/data/MPL/mesh/palm.STL differ
diff --git a/data/MPL/mesh/pinky0.STL b/data/MPL/mesh/pinky0.STL
new file mode 100644
index 000000000..109b68199
Binary files /dev/null and b/data/MPL/mesh/pinky0.STL differ
diff --git a/data/MPL/mesh/pinky1.STL b/data/MPL/mesh/pinky1.STL
new file mode 100644
index 000000000..8e2e98211
Binary files /dev/null and b/data/MPL/mesh/pinky1.STL differ
diff --git a/data/MPL/mesh/pinky2.STL b/data/MPL/mesh/pinky2.STL
new file mode 100644
index 000000000..8413cc85b
Binary files /dev/null and b/data/MPL/mesh/pinky2.STL differ
diff --git a/data/MPL/mesh/pinky3.STL b/data/MPL/mesh/pinky3.STL
new file mode 100644
index 000000000..4e539d732
Binary files /dev/null and b/data/MPL/mesh/pinky3.STL differ
diff --git a/data/MPL/mesh/ring0.STL b/data/MPL/mesh/ring0.STL
new file mode 100644
index 000000000..1189fb830
Binary files /dev/null and b/data/MPL/mesh/ring0.STL differ
diff --git a/data/MPL/mesh/ring1.STL b/data/MPL/mesh/ring1.STL
new file mode 100644
index 000000000..b29f6e640
Binary files /dev/null and b/data/MPL/mesh/ring1.STL differ
diff --git a/data/MPL/mesh/ring2.STL b/data/MPL/mesh/ring2.STL
new file mode 100644
index 000000000..0ae92882a
Binary files /dev/null and b/data/MPL/mesh/ring2.STL differ
diff --git a/data/MPL/mesh/ring3.STL b/data/MPL/mesh/ring3.STL
new file mode 100644
index 000000000..acdcd807e
Binary files /dev/null and b/data/MPL/mesh/ring3.STL differ
diff --git a/data/MPL/mesh/thumb0.STL b/data/MPL/mesh/thumb0.STL
new file mode 100644
index 000000000..6764cb45c
Binary files /dev/null and b/data/MPL/mesh/thumb0.STL differ
diff --git a/data/MPL/mesh/thumb1.STL b/data/MPL/mesh/thumb1.STL
new file mode 100644
index 000000000..a13de7c3e
Binary files /dev/null and b/data/MPL/mesh/thumb1.STL differ
diff --git a/data/MPL/mesh/thumb2.STL b/data/MPL/mesh/thumb2.STL
new file mode 100644
index 000000000..3555fbd93
Binary files /dev/null and b/data/MPL/mesh/thumb2.STL differ
diff --git a/data/MPL/mesh/thumb3.STL b/data/MPL/mesh/thumb3.STL
new file mode 100644
index 000000000..2e2699eaf
Binary files /dev/null and b/data/MPL/mesh/thumb3.STL differ
diff --git a/data/MPL/mesh/wristx.STL b/data/MPL/mesh/wristx.STL
new file mode 100644
index 000000000..b9b81e611
Binary files /dev/null and b/data/MPL/mesh/wristx.STL differ
diff --git a/data/MPL/mesh/wristy.STL b/data/MPL/mesh/wristy.STL
new file mode 100644
index 000000000..cbd3fdbfd
Binary files /dev/null and b/data/MPL/mesh/wristy.STL differ
diff --git a/data/MPL/mesh/wristz.STL b/data/MPL/mesh/wristz.STL
new file mode 100644
index 000000000..a13bed861
Binary files /dev/null and b/data/MPL/mesh/wristz.STL differ
diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf
index 535caa8cf..9d0b61c0d 100644
Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index 65d882296..4041feb01 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -8,6 +8,10 @@
#include "../ImportURDFDemo/UrdfParser.h"
#include "../ImportURDFDemo/urdfStringSplit.h"
#include "../ImportURDFDemo/urdfLexicalCast.h"
+#include "../ImportObjDemo/LoadMeshFromObj.h"
+#include "../ImportSTLDemo/LoadMeshFromSTL.h"
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include "../ImportMeshUtility/b3ImportMeshUtility.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
@@ -16,9 +20,22 @@
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
+
+
+#include
+
+enum eMJCF_FILE_TYPE_ENUMS
+{
+ MJCF_FILE_STL = 1,
+ MJCF_FILE_OBJ = 2
+};
+
enum ePARENT_LINK_ENUMS
{
BASE_LINK_INDEX=-1,
@@ -112,6 +129,10 @@ static bool parseVector6(btVector3& v0, btVector3& v1, const std::string& vector
}
+struct MyMJCFAsset
+{
+ std::string m_fileName;
+};
struct BulletMJCFImporterInternalData
{
@@ -119,12 +140,20 @@ struct BulletMJCFImporterInternalData
char m_pathPrefix[1024];
std::string m_fileModelName;
+ btHashMap m_assets;
btAlignedObjectArray m_models;
+
+ //
+ std::string m_meshDir;
+ std::string m_textureDir;
+
+
int m_activeModel;
//todo: for full MJCF compatibility, we would need a stack of default values
int m_defaultCollisionGroup;
int m_defaultCollisionMask;
+ btScalar m_defaultCollisionMargin;
//those collision shapes are deleted by caller (todo: make sure this happens!)
btAlignedObjectArray m_allocatedCollisionShapes;
@@ -132,7 +161,8 @@ struct BulletMJCFImporterInternalData
BulletMJCFImporterInternalData()
:m_activeModel(-1),
m_defaultCollisionGroup(1),
- m_defaultCollisionMask(1)
+ m_defaultCollisionMask(1),
+ m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
{
m_pathPrefix[0] = 0;
}
@@ -151,6 +181,48 @@ struct BulletMJCFImporterInternalData
return 0;
}
+ void parseCompiler(TiXmlElement* root_xml, MJCFErrorLogger* logger)
+ {
+
+ const char* meshDirStr = root_xml->Attribute("meshdir");
+ if (meshDirStr)
+ {
+ m_meshDir = meshDirStr;
+ }
+ const char* textureDirStr = root_xml->Attribute("texturedir");
+ if (textureDirStr)
+ {
+ m_textureDir = textureDirStr;
+ }
+#if 0
+ for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
+ {
+ std::string n = child_xml->Value();
+ }
+#endif
+ }
+
+ void parseAssets(TiXmlElement* root_xml, MJCFErrorLogger* logger)
+ {
+ //
+ for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
+ {
+ std::string n = child_xml->Value();
+ if (n=="mesh")
+ {
+ const char* assetNameStr = child_xml->Attribute("name");
+ const char* fileNameStr = child_xml->Attribute("file");
+ if (assetNameStr && fileNameStr)
+ {
+ btHashString assetName = assetNameStr;
+ MyMJCFAsset asset;
+ asset.m_fileName = m_meshDir + fileNameStr;
+ m_assets.insert(assetName,asset);
+ }
+ }
+
+ }
+ }
bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger)
{
bool handled= false;
@@ -158,9 +230,14 @@ struct BulletMJCFImporterInternalData
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{
std::string n = child_xml->Value();
+
if (n=="inertial")
{
}
+ if (n=="asset")
+ {
+ parseAssets(child_xml,logger);
+ }
if (n=="geom")
{
//contype, conaffinity
@@ -534,6 +611,25 @@ struct BulletMJCFImporterInternalData
}
}
}
+ if (geomType=="mesh")
+ {
+ const char* meshStr = link_xml->Attribute("mesh");
+ if (meshStr)
+ {
+ MyMJCFAsset* assetPtr = m_assets[meshStr];
+ if (assetPtr)
+ {
+ handledGeomType = true;
+ geom.m_type = URDF_GEOM_MESH;
+ geom.m_meshFileName = assetPtr->m_fileName;
+ geom.m_meshScale.setValue(1,1,1);
+ //todo: parse mesh scale
+ if (sz)
+ {
+ }
+ }
+ }
+ }
#if 0
if (geomType == "cylinder")
{
@@ -1081,6 +1177,17 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
{
m_data->parseDefaults(link_xml,logger);
}
+
+ for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("compiler"); link_xml; link_xml = link_xml->NextSiblingElement("compiler"))
+ {
+ m_data->parseCompiler(link_xml,logger);
+ }
+
+
+ for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("asset"); link_xml; link_xml = link_xml->NextSiblingElement("asset"))
+ {
+ m_data->parseAssets(link_xml,logger);
+ }
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
{
@@ -1281,6 +1388,52 @@ int BulletMJCFImporter::getBodyUniqueId() const
{
return 0;
}
+
+
+static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale, btScalar collisionMargin)
+{
+ btCompoundShape* compound = new btCompoundShape();
+ compound->setMargin(collisionMargin);
+
+ btTransform identity;
+ identity.setIdentity();
+
+ for (int s = 0; s<(int)shapes.size(); s++)
+ {
+ btConvexHullShape* convexHull = new btConvexHullShape();
+ convexHull->setMargin(collisionMargin);
+ tinyobj::shape_t& shape = shapes[s];
+ int faceCount = shape.mesh.indices.size();
+
+ for (int f = 0; faddPoint(pt*geomScale,false);
+
+ pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
+ shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
+ shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
+ convexHull->addPoint(pt*geomScale, false);
+
+ pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
+ shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
+ shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+ convexHull->addPoint(pt*geomScale, false);
+ }
+
+ convexHull->recalcLocalAabb();
+ convexHull->optimizeConvexHull();
+ compound->addChildShape(identity,convexHull);
+ }
+
+ return compound;
+}
+
class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
@@ -1319,6 +1472,129 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
}
case URDF_GEOM_MESH:
{
+ //////////////////////
+ if (1)
+ {
+ if (col->m_geometry.m_meshFileName.length())
+ {
+ const char* filename = col->m_geometry.m_meshFileName.c_str();
+ //b3Printf("mesh->filename=%s\n",filename);
+ char fullPath[1024];
+ int fileType = 0;
+ sprintf(fullPath,"%s%s",pathPrefix,filename);
+ b3FileUtils::toLower(fullPath);
+ char tmpPathPrefix[1024];
+ int maxPathLen = 1024;
+ b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
+
+ char collisionPathPrefix[1024];
+ sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix);
+
+ if (strstr(fullPath,".stl"))
+ {
+ fileType = MJCF_FILE_STL;
+ }
+ if (strstr(fullPath,".obj"))
+ {
+ fileType = MJCF_FILE_OBJ;
+ }
+
+ sprintf(fullPath,"%s%s",pathPrefix,filename);
+ FILE* f = fopen(fullPath,"rb");
+ if (f)
+ {
+ fclose(f);
+ GLInstanceGraphicsShape* glmesh = 0;
+
+
+ switch (fileType)
+ {
+ case MJCF_FILE_OBJ:
+ {
+ if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+ {
+ glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
+ }
+ else
+ {
+ std::vector shapes;
+ std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
+ //create a convex hull for each shape, and store it in a btCompoundShape
+
+ childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
+
+ }
+ break;
+ }
+ case MJCF_FILE_STL:
+ {
+ glmesh = LoadMeshFromSTL(fullPath);
+ break;
+ }
+
+ default:
+ {
+ b3Warning("Unsupported file type in Collision: %s\n",fullPath);
+
+ }
+ }
+
+
+ if (!childShape && glmesh && (glmesh->m_numvertices>0))
+ {
+ //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
+ //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
+ //convex->setUserIndex(shapeId);
+ btAlignedObjectArray convertedVerts;
+ convertedVerts.reserve(glmesh->m_numvertices);
+ for (int i=0;im_numvertices;i++)
+ {
+ convertedVerts.push_back(btVector3(
+ glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
+ glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
+ glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
+ }
+
+ if (col->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);
+ childShape = trimesh;
+ } else
+ {
+ btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+ convexHull->optimizeConvexHull();
+ //convexHull->initializePolyhedralFeatures();
+ convexHull->setMargin(m_data->m_defaultCollisionMargin);
+ childShape = convexHull;
+ }
+ } else
+ {
+ b3Warning("issue extracting mesh from STL file %s\n", fullPath);
+ }
+
+ delete glmesh;
+
+ } else
+ {
+ b3Warning("mesh geometry not found %s\n",fullPath);
+ }
+
+ }
+ }
+
+ //////////////////////
break;
}
@@ -1349,6 +1625,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
}
default:
{
+
}
}
if (childShape)
diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
index 2008d52e9..75f6b83b3 100644
--- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
+++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
@@ -119,6 +119,7 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
if (gMCFJFileNameArray.size()==0)
{
+ gMCFJFileNameArray.push_back("MPL/MPL.xml");
gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
gMCFJFileNameArray.push_back("mjcf/ant.xml");
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 74c06f765..37e7cb78b 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -455,10 +455,13 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
- command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
- command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
- command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
-
+ b3Assert(dofIndex>=0);
+ if (dofIndex>=0)
+ {
+ command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
+ }
return 0;
}
@@ -466,10 +469,13 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
- command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
- command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
- command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
-
+ b3Assert(dofIndex>=0);
+ if (dofIndex>=0)
+ {
+ command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
+ }
return 0;
}
@@ -477,10 +483,13 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
- command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
- command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
- command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
-
+ b3Assert(dofIndex>=0);
+ if (dofIndex>=0)
+ {
+ command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
+ }
return 0;
}
@@ -489,10 +498,13 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
- command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
- command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
- command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
-
+ b3Assert(dofIndex>=0);
+ if (dofIndex>=0)
+ {
+ command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+ }
return 0;
}
@@ -500,10 +512,13 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
- command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
- command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
- command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
-
+ b3Assert(dofIndex>=0);
+ if (dofIndex>=0)
+ {
+ command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+ }
return 0;
}
@@ -520,7 +535,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
return (b3SharedMemoryCommandHandle) command;
}
-void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
+int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
@@ -529,23 +544,32 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
if (bodyIndex>=0)
{
b3JointInfo info;
- b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
- state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
- state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
- for (int ii(0); ii < 6; ++ii) {
- state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
+ bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
+ if (result)
+ {
+ state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
+ state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
+ for (int ii(0); ii < 6; ++ii) {
+ state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
+ }
+ state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
+ return 1;
}
- state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
}
+ return 0;
}
-void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
+int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex>=0);
- if (bodyIndex>=0)
+ b3Assert(linkIndex >= 0);
+ int numJoints = b3GetNumJoints(physClient,bodyIndex);
+ b3Assert(linkIndex < numJoints);
+
+ if ((bodyIndex>=0) && (linkIndex >= 0) && linkIndex < numJoints)
{
b3Transform wlf,com,inertial;
@@ -575,7 +599,9 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
{
state->m_worldLinkFrameOrientation[i] = wlfOrn[i];
}
+ return 1;
}
+ return 0;
}
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
@@ -1244,6 +1270,18 @@ void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastI
}
+///If you re-connected to an existing server, or server changed otherwise, sync the body info
+b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+
+ command->m_type =CMD_SYNC_BODY_INFO;
+ return (b3SharedMemoryCommandHandle) command;
+}
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 4a2333749..54f1ad1e1 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -57,6 +57,9 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[],
const double* jointReactionForces[]);
+///If you re-connected to an existing server, or server changed otherwise, sync the body info
+b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
+
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient);
@@ -293,8 +296,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
-void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
-void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
+int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
+int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
index 5b3f3ac81..df2aed6a7 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -811,6 +811,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("createConstraint failed");
break;
}
+ case CMD_ACTUAL_STATE_UPDATE_FAILED:
+ {
+ b3Warning("request actual state failed");
+ break;
+ }
+ case CMD_SYNC_BODY_INFO_COMPLETED:
+ {
+ break;
+ }
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@@ -830,7 +839,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
- if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED))
+ if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
{
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0)
diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
index 1fc760482..07dc561af 100644
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -670,6 +670,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break;
}
+ case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:
{
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 6c0f23b4f..af2d5c2d8 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -423,6 +423,12 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray m_bodyHandles;
int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list
+
+ int getNumHandles() const
+ {
+ return m_bodyHandles.size();
+ }
+
InternalBodyHandle* getHandle(int handle)
{
btAssert(handle>=0);
@@ -1741,6 +1747,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
+ case CMD_SYNC_BODY_INFO:
+ {
+ int numHandles = m_data->getNumHandles();
+ int actualNumBodies = 0;
+ for (int i=0;igetHandle(i);
+ if (body && (body->m_multiBody || body->m_rigidBody))
+ {
+ serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = i;
+ }
+ }
+ serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies;
+ serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
+ hasStatus = true;
+ break;
+ }
case CMD_REQUEST_BODY_INFO:
{
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
@@ -3672,7 +3695,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (userConstraintPtr->m_rbConstraint)
{
-
+ //todo
}
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
@@ -4367,7 +4390,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
-btVector3 gVRGripperPos(0.6, 0.4, 0.7);
+btVector3 gVRGripperPos(0.7, 0.3, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
@@ -4764,7 +4787,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
- m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
+ m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
}
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 23571a2f2..07b980dcb 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -1119,7 +1119,7 @@ public:
if (args.CheckCmdLineFlag("norobotassets"))
{
-// gCreateDefaultRobotAssets = false;
+ gCreateDefaultRobotAssets = false;
}
@@ -1671,8 +1671,8 @@ void PhysicsServerExample::drawUserDebugLines()
void PhysicsServerExample::renderScene()
{
btTransform vrTrans;
- gVRTeleportPos1 = gVRTeleportPosLocal;
- gVRTeleportOrn = gVRTeleportOrnLocal;
+ //gVRTeleportPos1 = gVRTeleportPosLocal;
+ //gVRTeleportOrn = gVRTeleportOrnLocal;
///little VR test to follow/drive Husky vehicle
if (gVRTrackingObjectUniqueId >= 0)
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index f87e44c0c..1c85c3b7d 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -33,7 +33,7 @@
#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
-#define MAX_SDF_BODIES 500
+#define MAX_SDF_BODIES 512
struct TmpFloat3
{
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 571aa05e7..e8350560a 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -47,7 +47,7 @@ enum EnumSharedMemoryClientCommand
CMD_USER_DEBUG_DRAW,
CMD_REQUEST_VR_EVENTS_DATA,
CMD_SET_VR_CAMERA_STATE,
-
+ CMD_SYNC_BODY_INFO,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -115,6 +115,8 @@ enum EnumSharedMemoryServerStatus
CMD_USER_CONSTRAINT_FAILED,
CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
+ CMD_SYNC_BODY_INFO_COMPLETED,
+ CMD_SYNC_BODY_INFO_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp
index 71196c11d..7ef40476c 100644
--- a/examples/StandaloneMain/hellovr_opengl_main.cpp
+++ b/examples/StandaloneMain/hellovr_opengl_main.cpp
@@ -2246,12 +2246,13 @@ int main(int argc, char *argv[])
if (sExample)
{
//until we have a proper VR gui, always assume we want the hard-coded default robot assets
+#if 0
char* newargv[2];
char* t0 = (char*)"--robotassets";
newargv[0] = t0;
newargv[1] = t0;
sExample->processCommandLineArgs(2,newargv);
-
+#endif
sExample->processCommandLineArgs(argc,argv);
}
diff --git a/examples/pybullet/hand.ino b/examples/pybullet/hand.ino
new file mode 100644
index 000000000..faf5b70bc
--- /dev/null
+++ b/examples/pybullet/hand.ino
@@ -0,0 +1,32 @@
+//arduino script for vr glove, sending analogue 'finger' readings
+//to be used with pybullet and hand.py
+
+
+int sensorPin0 = A0;
+int sensorPin1 = A1;
+int sensorPin2 = A2;
+int sensorPin3 = A3;
+
+void setup() {
+ // put your setup code here, to run once:
+ Serial.begin(115200);
+}
+
+void loop() {
+ // put your main code here, to run repeatedly:
+ int sensorValue0 = analogRead(sensorPin0);
+ int sensorValue1 = analogRead(sensorPin1);
+ int sensorValue2 = analogRead(sensorPin2);
+ int sensorValue3 = analogRead(sensorPin3);
+
+ Serial.print(",");
+ Serial.print(sensorValue0);
+ Serial.print(",");
+ Serial.print(sensorValue1);
+ Serial.print(",");
+ Serial.print(sensorValue2);
+ Serial.print(",");
+ Serial.print(sensorValue3);
+ Serial.println(",");
+ delay(10);
+}
diff --git a/examples/pybullet/hand.py b/examples/pybullet/hand.py
new file mode 100644
index 000000000..d0428c555
--- /dev/null
+++ b/examples/pybullet/hand.py
@@ -0,0 +1,78 @@
+#script to control a simulated robot hand using a VR glove
+#see https://twitter.com/erwincoumans/status/821953216271106048
+#and https://www.youtube.com/watch?v=I6s37aBXbV8
+#vr glove was custom build using Spectra Symbolflex sensors (4.5")
+#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
+#with BLE Link to receive serial (for wireless bluetooth serial)
+
+import serial
+import time
+import pybullet as p
+
+#first try to connect to shared memory (VR), if it fails use local GUI
+c = p.connect(p.SHARED_MEMORY)
+print(c)
+if (c<0):
+ p.connect(p.GUI)
+
+#load the MuJoCo MJCF hand
+objects = p.loadMJCF("MPL/mpl.xml")
+
+hand=objects[0]
+#clamp in range 400-600
+minV = 400
+maxV = 600
+
+p.setRealTimeSimulation(1)
+
+def convertSensor(x):
+ v = minV
+ try:
+ v = float(x)
+ except ValueError:
+ v = minV
+ if (vmaxV):
+ v=maxV
+ b = (v-minV)/float(maxV-minV)
+ return (1.0-b)
+
+ser = serial.Serial(port='COM13',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
+if (ser.isOpen()):
+ while True:
+ while ser.inWaiting() > 0:
+ line = str(ser.readline())
+ words = line.split(",")
+ if (len(words)==6):
+ middle = convertSensor(words[1])
+ pink = convertSensor(words[2])
+ index = convertSensor(words[3])
+ thumb = convertSensor(words[4])
+
+ p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb)
+ p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb)
+ p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
+ p.setJointMotorControl2(hand,13,p.POSITION_CONTROL,thumb)
+
+ p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
+ p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
+ p.setJointMotorControl2(hand,21,p.POSITION_CONTROL,index)
+
+ p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
+ p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
+ p.setJointMotorControl2(hand,28,p.POSITION_CONTROL,middle)
+
+ p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
+ p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
+ p.setJointMotorControl2(hand,44,p.POSITION_CONTROL,pink)
+
+ ringpos = 0.5*(pink+middle)
+ p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos)
+ p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos)
+ p.setJointMotorControl2(hand,36,p.POSITION_CONTROL,ringpos)
+
+ #print(middle)
+ #print(pink)
+ #print(index)
+ #print(thumb)
\ No newline at end of file
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index c02ff4bb7..a8a36617a 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -315,9 +315,25 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
if (freeIndex>=0)
{
+ b3SharedMemoryCommandHandle command;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+
+
sPhysicsClients1[freeIndex] = sm;
sNumPhysicsClients++;
+
+ command = b3InitSyncBodyInfoCommand(sm);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusType = b3GetStatusType(statusHandle);
+#if 0
+ if (statusType != CMD_BODY_INFO_COMPLETED) {
+ PyErr_SetString(SpamError, "b3InitSyncBodyInfoCommand failed.");
+ return NULL;
+ }
+#endif
}
+
}
return PyInt_FromLong(freeIndex);
}
@@ -1989,24 +2005,29 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject*
pyListJointState = PyTuple_New(sensorStateSize);
pyListJointForceTorque = PyTuple_New(forceTorqueSize);
- b3GetJointState(sm, status_handle, jointIndex, &sensorState);
+ if (b3GetJointState(sm, status_handle, jointIndex, &sensorState))
+ {
+ PyTuple_SetItem(pyListJointState, 0,
+ PyFloat_FromDouble(sensorState.m_jointPosition));
+ PyTuple_SetItem(pyListJointState, 1,
+ PyFloat_FromDouble(sensorState.m_jointVelocity));
- 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]));
+ }
- for (j = 0; j < forceTorqueSize; j++) {
- PyTuple_SetItem(pyListJointForceTorque, j,
- PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
- }
+ PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
- PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
+ PyTuple_SetItem(pyListJointState, 3,
+ PyFloat_FromDouble(sensorState.m_jointMotorTorque));
- PyTuple_SetItem(pyListJointState, 3,
- PyFloat_FromDouble(sensorState.m_jointMotorTorque));
-
- return pyListJointState;
+ return pyListJointState;
+ } else
+ {
+ PyErr_SetString(SpamError, "getJointState failed (2).");
+ return NULL;
+ }
}
}
@@ -2070,54 +2091,55 @@ b3PhysicsClientHandle sm = 0;
return NULL;
}
- b3GetLinkState(sm, status_handle, linkIndex, &linkState);
+ if (b3GetLinkState(sm, status_handle, linkIndex, &linkState))
+ {
+ pyLinkStateWorldPosition = PyTuple_New(3);
+ for (i = 0; i < 3; ++i) {
+ PyTuple_SetItem(pyLinkStateWorldPosition, i,
+ PyFloat_FromDouble(linkState.m_worldPosition[i]));
+ }
- pyLinkStateWorldPosition = PyTuple_New(3);
- for (i = 0; i < 3; ++i) {
- PyTuple_SetItem(pyLinkStateWorldPosition, i,
- PyFloat_FromDouble(linkState.m_worldPosition[i]));
- }
+ pyLinkStateWorldOrientation = PyTuple_New(4);
+ for (i = 0; i < 4; ++i) {
+ PyTuple_SetItem(pyLinkStateWorldOrientation, i,
+ PyFloat_FromDouble(linkState.m_worldOrientation[i]));
+ }
- pyLinkStateWorldOrientation = PyTuple_New(4);
- for (i = 0; i < 4; ++i) {
- PyTuple_SetItem(pyLinkStateWorldOrientation, i,
- PyFloat_FromDouble(linkState.m_worldOrientation[i]));
- }
+ pyLinkStateLocalInertialPosition = PyTuple_New(3);
+ for (i = 0; i < 3; ++i) {
+ PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
+ PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
+ }
- pyLinkStateLocalInertialPosition = PyTuple_New(3);
- for (i = 0; i < 3; ++i) {
- PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
- PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
- }
+ pyLinkStateLocalInertialOrientation = PyTuple_New(4);
+ for (i = 0; i < 4; ++i) {
+ PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
+ PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
+ }
- pyLinkStateLocalInertialOrientation = PyTuple_New(4);
- for (i = 0; i < 4; ++i) {
- PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
- PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
- }
+ pyLinkStateWorldLinkFramePosition = PyTuple_New(3);
+ for (i = 0; i < 3; ++i) {
+ PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i,
+ PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i]));
+ }
- pyLinkStateWorldLinkFramePosition = PyTuple_New(3);
- for (i = 0; i < 3; ++i) {
- PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i,
- PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i]));
- }
-
- pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
- for (i = 0; i < 4; ++i) {
- PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
- PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
- }
+ pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
+ for (i = 0; i < 4; ++i) {
+ PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
+ PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
+ }
- pyLinkState = PyTuple_New(6);
- PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
- PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
- PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
- PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
- PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition );
- PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation);
+ pyLinkState = PyTuple_New(6);
+ PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
+ PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
+ PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
+ PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
+ PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition );
+ PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation);
- return pyLinkState;
+ return pyLinkState;
+ }
}
}
diff --git a/examples/pybullet/vrEvent.py b/examples/pybullet/vrEvent.py
index 970757eae..beb369b1d 100644
--- a/examples/pybullet/vrEvent.py
+++ b/examples/pybullet/vrEvent.py
@@ -7,10 +7,15 @@ import pybullet as p
CONTROLLER_ID = 0
POSITION=1
+ORIENTATION=2
BUTTONS=6
#assume that the VR physics server is already started before
-p.connect(p.SHARED_MEMORY)
+c = p.connect(p.SHARED_MEMORY)
+print(c)
+if (c<0):
+ p.connect(p.GUI)
+
p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
p.loadURDF("plane.urdf")
diff --git a/examples/pybullet/vr_kuka_setup.py b/examples/pybullet/vr_kuka_setup.py
new file mode 100644
index 000000000..5a404cd46
--- /dev/null
+++ b/examples/pybullet/vr_kuka_setup.py
@@ -0,0 +1,76 @@
+import pybullet as p
+p.connect(p.SHARED_MEMORY)
+p.resetSimulation()
+
+objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
+pr2_gripper = objects[0]
+print ("pr2_gripper=")
+print (pr2_gripper)
+
+jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
+for jointIndex in range (p.getNumJoints(pr2_gripper)):
+ p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
+
+pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
+print ("pr2_cid")
+print (pr2_cid)
+
+objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
+kuka = objects[0]
+jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
+for jointIndex in range (p.getNumJoints(kuka)):
+ p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
+
+objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
+objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
+kuka_gripper = objects[0]
+print ("kuka gripper=")
+print(kuka_gripper)
+
+p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
+jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
+for jointIndex in range (p.getNumJoints(kuka_gripper)):
+ p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
+
+
+kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
+
+objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)]
+objects = p.loadSDF("kiva_shelf/model.sdf")
+ob = objects[0]
+p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
+objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
+objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+ob = objects[0]
+jointPositions=[ 0.000000 ]
+for jointIndex in range (p.getNumJoints(ob)):
+ p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
+
+objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
+ob = objects[0]
+jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
+for jointIndex in range (p.getNumJoints(ob)):
+ p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
+
+p.setGravity(0.000000,0.000000,0.000000)
+p.setGravity(0,0,-10)
+
+p.stepSimulation()
+
+p.disconnect()
diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h
index df907e128..00754567b 100644
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -29,7 +29,7 @@ subject to the following restrictions:
#include
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 285
+#define BT_BULLET_VERSION 286
inline int btGetVersion()
{
diff --git a/src/LinearMath/btSerializer.h b/src/LinearMath/btSerializer.h
index 6f03df158..9d95e1c8a 100644
--- a/src/LinearMath/btSerializer.h
+++ b/src/LinearMath/btSerializer.h
@@ -505,7 +505,7 @@ public:
buffer[9] = '2';
buffer[10] = '8';
- buffer[11] = '5';
+ buffer[11] = '6';
}