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

Merge pull request #925 from erwincoumans/master

Browse Source 
Prepare for Bullet 2.86 release, update version to 2.86 (release will…
This commit is contained in:
erwincoumans
2017-01-21 12:32:41 -08:00
committed by GitHub
parent 1fe70750f3 64957ece9f
commit d834bc72af
46 changed files with 1160 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

279
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -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 <vector>
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<btHashString,MyMJCFAsset> m_assets;
btAlignedObjectArray<UrdfModel*> m_models;
//<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/>
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<btCollisionShape*> 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)
{
// <mesh name="index0" file="index0.stl"/>
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<tinyobj::shape_t>& 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; f<faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(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<tinyobj::shape_t> 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<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i=0;i<glmesh->m_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;i<glmesh->m_numIndices/3;i++)
{
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
btVector3(v1[0],v1[1],v1[2]),
btVector3(v2[0],v2[1],v2[2]));
}
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
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)

1
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

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

96
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

7
examples/SharedMemory/PhysicsClientC_API.h
View File

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

11
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

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

1
examples/SharedMemory/PhysicsDirect.cpp
View File

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

29
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -423,6 +423,12 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<InternalBodyHandle> 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;i<numHandles;i++)
{
InteralBodyData* body = m_data->getHandle(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));
}

6
examples/SharedMemory/PhysicsServerExample.cpp
View File

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

2
examples/SharedMemory/SharedMemoryCommands.h
View File

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

4
examples/SharedMemory/SharedMemoryPublic.h
View File

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

3
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

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

32
examples/pybullet/hand.ino Normal file
View File

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

78
examples/pybullet/hand.py Normal file
View File

@@ -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 (v<minV):
v=minV
if (v>maxV):
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)

130
examples/pybullet/pybullet.c
View File

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

7
examples/pybullet/vrEvent.py
View File

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

76
examples/pybullet/vr_kuka_setup.py Normal file
View File

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