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

Merge pull request #1155 from erwincoumans/master

Browse Source 
fix bodyIndex -> bodyUniqueId in pybullet.
This commit is contained in:
erwincoumans
2017-05-28 17:30:43 -07:00
committed by GitHub
parent 225c603a52 5436b8f048
commit 356d4104f0
11 changed files with 148 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
examples/SharedMemory/PhysicsCommandProcessorInterface.h
View File

@@ -1,7 +1,7 @@
#ifndef PHYSICS_COMMAND_PROCESSOR_INTERFACE_H #ifndef PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
#define PHYSICS_COMMAND_PROCESSOR_INTERFACE_H #define PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
enum PhysicsCOmmandRenderFlags enum PhysicsCommandRenderFlags
{ {
COV_DISABLE_SYNC_RENDERING=1 COV_DISABLE_SYNC_RENDERING=1
}; };
@@ -29,4 +29,27 @@ public:
}; };
class btVector3;
class CommandProcessorInterface : public PhysicsCommandProcessorInterface
{
public:
virtual ~CommandProcessorInterface(){}
virtual void syncPhysicsToGraphics()=0;
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)=0;
virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0;
virtual void enableCommandLogging(bool enable, const char* fileName)=0;
virtual void replayFromLogFile(const char* fileName)=0;
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes )=0;
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual void removePickingConstraint()=0;
};
#endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H #endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H

14
examples/SharedMemory/PhysicsServer.h
View File

@@ -25,18 +25,18 @@ public:
//@todo(erwincoumans) Should we have shared memory commands for picking objects? //@todo(erwincoumans) Should we have shared memory commands for picking objects?
///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise ///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
virtual void removePickingConstraint()=0; virtual void removePickingConstraint(){}
//for physicsDebugDraw and renderScene are mainly for debugging purposes //for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe //and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory //to a physics client, over shared memory
virtual void physicsDebugDraw(int debugDrawFlags)=0; virtual void physicsDebugDraw(int debugDrawFlags){}
virtual void renderScene(int renderFlags)=0; virtual void renderScene(int renderFlags){}
virtual void enableCommandLogging(bool enable, const char* fileName)=0; virtual void enableCommandLogging(bool enable, const char* fileName){}
virtual void replayFromLogFile(const char* fileName)=0; virtual void replayFromLogFile(const char* fileName){}
}; };

12
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -15,7 +15,7 @@ struct SharedMemLines
///todo: naming. Perhaps PhysicsSdkCommandprocessor? ///todo: naming. Perhaps PhysicsSdkCommandprocessor?
class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface class PhysicsServerCommandProcessor : public CommandProcessorInterface
{ {
struct PhysicsServerCommandProcessorInternalData* m_data; struct PhysicsServerCommandProcessorInternalData* m_data;
@@ -91,16 +91,16 @@ public:
virtual void removePickingConstraint(); virtual void removePickingConstraint();
//logging /playback the shared memory commands //logging /playback the shared memory commands
void enableCommandLogging(bool enable, const char* fileName); virtual void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName); virtual void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
//logging of object states (position etc) //logging of object states (position etc)
void logObjectStates(btScalar timeStep); void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep); void processCollisionForces(btScalar timeStep);
void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents); virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
void enableRealTimeSimulation(bool enableRealTimeSim); virtual void enableRealTimeSimulation(bool enableRealTimeSim);
void applyJointDamping(int bodyUniqueId); void applyJointDamping(int bodyUniqueId);
virtual void setTimeOut(double timeOutInSeconds); virtual void setTimeOut(double timeOutInSeconds);

10
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -600,7 +600,7 @@ struct ColorWidth
ATTRIBUTE_ALIGNED16( class )MultithreadedDebugDrawer : public btIDebugDraw ATTRIBUTE_ALIGNED16( class )MultithreadedDebugDrawer : public btIDebugDraw
{ {
class GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
int m_debugMode; int m_debugMode;
btAlignedObjectArray< btAlignedObjectArray<unsigned int> > m_sortedIndices; btAlignedObjectArray< btAlignedObjectArray<unsigned int> > m_sortedIndices;
@@ -1776,22 +1776,22 @@ void PhysicsServerExample::updateGraphics()
if (flag==COV_ENABLE_VR_TELEPORTING) if (flag==COV_ENABLE_VR_TELEPORTING)
{ {
gEnableTeleporting = enable; gEnableTeleporting = (enable!=0);
} }
if (flag == COV_ENABLE_VR_PICKING) if (flag == COV_ENABLE_VR_PICKING)
{ {
gEnablePicking = enable; gEnablePicking = (enable!=0);
} }
if (flag ==COV_ENABLE_SYNC_RENDERING_INTERNAL) if (flag ==COV_ENABLE_SYNC_RENDERING_INTERNAL)
{ {
gEnableSyncPhysicsRendering = enable; gEnableSyncPhysicsRendering = (enable!=0);
} }
if (flag == COV_ENABLE_RENDERING) if (flag == COV_ENABLE_RENDERING)
{ {
gEnableRendering = enable; gEnableRendering = (enable!=0);
} }
m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag,m_multiThreadedHelper->m_visualizerEnable); m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag,m_multiThreadedHelper->m_visualizerEnable);

9
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -30,7 +30,7 @@ struct PhysicsServerSharedMemoryInternalData
int m_sharedMemoryKey; int m_sharedMemoryKey;
bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS]; bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS];
bool m_verboseOutput; bool m_verboseOutput;
PhysicsServerCommandProcessor* m_commandProcessor; CommandProcessorInterface* m_commandProcessor;
PhysicsServerSharedMemoryInternalData() PhysicsServerSharedMemoryInternalData()
:m_sharedMemory(0), :m_sharedMemory(0),
@@ -89,7 +89,7 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar
PhysicsServerSharedMemory::~PhysicsServerSharedMemory() PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
{ {
m_data->m_commandProcessor->deleteDynamicsWorld(); //m_data->m_commandProcessor->deleteDynamicsWorld();
delete m_data->m_commandProcessor; delete m_data->m_commandProcessor;
if (m_data->m_sharedMemory) if (m_data->m_sharedMemory)
@@ -109,11 +109,12 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
delete m_data; delete m_data;
} }
void PhysicsServerSharedMemory::resetDynamicsWorld() /*void PhysicsServerSharedMemory::resetDynamicsWorld()
{ {
m_data->m_commandProcessor->deleteDynamicsWorld(); m_data->m_commandProcessor->deleteDynamicsWorld();
m_data->m_commandProcessor ->createEmptyDynamicsWorld(); m_data->m_commandProcessor ->createEmptyDynamicsWorld();
} }
*/
void PhysicsServerSharedMemory::setSharedMemoryKey(int key) void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
{ {
m_data->m_sharedMemoryKey = key; m_data->m_sharedMemoryKey = key;
@@ -188,7 +189,7 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
{ {
m_data->m_commandProcessor->deleteDynamicsWorld(); //m_data->m_commandProcessor->deleteDynamicsWorld();
m_data->m_commandProcessor->setGuiHelper(0); m_data->m_commandProcessor->setGuiHelper(0);

1
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@@ -48,7 +48,6 @@ public:
void enableCommandLogging(bool enable, const char* fileName); void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName); void replayFromLogFile(const char* fileName);
void resetDynamicsWorld();
}; };

10
examples/SharedMemory/SharedMemoryCommandProcessor.cpp
View File

@@ -73,7 +73,15 @@ bool SharedMemoryCommandProcessor::connect()
if (m_data->m_testBlock1) { if (m_data->m_testBlock1) {
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
b3Error("Error: please start server before client\n");
if ((m_data->m_testBlock1->m_magicId < 211705023) &&
(m_data->m_testBlock1->m_magicId >=201705023))
{
b3Error("Error: physics server version mismatch (expected %d got %d)\n",SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
} else
{
b3Error("Error connecting to shared memory: please start server before client\n");
}
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
SHARED_MEMORY_SIZE); SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0; m_data->m_testBlock1 = 0;

0
data/block_grasp_log.bin → examples/pybullet/examples/data/block_grasp_log.bin
View File

10
examples/pybullet/examples/kuka_grasp_block_playback.py
View File

@@ -1,14 +1,6 @@
import pybullet as p import pybullet as p
import time
import math
from datetime import datetime
from numpy import *
from pylab import *
import struct import struct
import sys
import os, fnmatch
import argparse
from time import sleep
def readLogFile(filename, verbose = True): def readLogFile(filename, verbose = True):
f = open(filename, 'rb') f = open(filename, 'rb')

147
examples/pybullet/pybullet.c
View File

@@ -695,7 +695,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
int status_type = 0; int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle; b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle; b3SharedMemoryStatusHandle status_handle;
struct b3DynamicsInfo info;
if (bodyUniqueId < 0) if (bodyUniqueId < 0)
{ {
PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid bodyUniqueId"); PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid bodyUniqueId");
@@ -714,7 +714,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid return status"); PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid return status");
return NULL; return NULL;
} }
struct b3DynamicsInfo info;
if (b3GetDynamicsInfo(status_handle, &info)) if (b3GetDynamicsInfo(status_handle, &info))
{ {
PyObject* pyDynamicsInfo = PyTuple_New(2); PyObject* pyDynamicsInfo = PyTuple_New(2);
@@ -840,7 +840,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL}; static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL};
static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL}; static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL};
int bodyIndex = -1; int bodyUniqueId= -1;
const char* urdfFileName = ""; const char* urdfFileName = "";
double startPosX = 0.0; double startPosX = 0.0;
@@ -954,7 +954,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
PyErr_SetString(SpamError, "Cannot load URDF file."); PyErr_SetString(SpamError, "Cannot load URDF file.");
return NULL; return NULL;
} }
bodyIndex = b3GetStatusBodyIndex(statusHandle); bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
} }
else else
{ {
@@ -962,7 +962,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
"Empty filename, method expects 1, 4 or 8 arguments."); "Empty filename, method expects 1, 4 or 8 arguments.");
return NULL; return NULL;
} }
return PyLong_FromLong(bodyIndex); return PyLong_FromLong(bodyUniqueId);
} }
static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keywds)
@@ -1049,7 +1049,7 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje
static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{ {
int size; int size;
int bodyIndex, jointIndex, controlMode; int bodyUniqueId, jointIndex, controlMode;
double targetPosition = 0.0; double targetPosition = 0.0;
double targetVelocity = 0.0; double targetVelocity = 0.0;
@@ -1072,7 +1072,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{ {
double targetValue = 0.0; double targetValue = 0.0;
// see switch statement below for convertsions dependent on controlMode // see switch statement below for convertsions dependent on controlMode
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, if (!PyArg_ParseTuple(args, "iiid", &bodyUniqueId, &jointIndex, &controlMode,
&targetValue)) &targetValue))
{ {
PyErr_SetString(SpamError, "Error parsing arguments"); PyErr_SetString(SpamError, "Error parsing arguments");
@@ -1106,7 +1106,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{ {
double targetValue = 0.0; double targetValue = 0.0;
// See switch statement for conversions // See switch statement for conversions
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, if (!PyArg_ParseTuple(args, "iiidd", &bodyUniqueId, &jointIndex, &controlMode,
&targetValue, &maxForce)) &targetValue, &maxForce))
{ {
PyErr_SetString(SpamError, "Error parsing arguments"); PyErr_SetString(SpamError, "Error parsing arguments");
@@ -1141,7 +1141,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{ {
double gain = 0.0; double gain = 0.0;
double targetValue = 0.0; double targetValue = 0.0;
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, if (!PyArg_ParseTuple(args, "iiiddd", &bodyUniqueId, &jointIndex, &controlMode,
&targetValue, &maxForce, &gain)) &targetValue, &maxForce, &gain))
{ {
PyErr_SetString(SpamError, "Error parsing arguments"); PyErr_SetString(SpamError, "Error parsing arguments");
@@ -1177,7 +1177,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
if (size == 8) if (size == 8)
{ {
// only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, if (!PyArg_ParseTuple(args, "iiiddddd", &bodyUniqueId, &jointIndex,
&controlMode, &targetPosition, &targetVelocity, &controlMode, &targetPosition, &targetVelocity,
&maxForce, &kp, &kd)) &maxForce, &kp, &kd))
{ {
@@ -1194,7 +1194,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
struct b3JointInfo info; struct b3JointInfo info;
numJoints = b3GetNumJoints(sm, bodyIndex); numJoints = b3GetNumJoints(sm, bodyUniqueId);
if ((jointIndex >= numJoints) || (jointIndex < 0)) if ((jointIndex >= numJoints) || (jointIndex < 0))
{ {
PyErr_SetString(SpamError, "Joint index out-of-range."); PyErr_SetString(SpamError, "Joint index out-of-range.");
@@ -1209,9 +1209,9 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
return NULL; return NULL;
} }
commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info); b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
switch (controlMode) switch (controlMode)
{ {
@@ -1258,7 +1258,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int bodyIndex, controlMode; int bodyUniqueId, controlMode;
PyObject* jointIndicesObj = 0; PyObject* jointIndicesObj = 0;
PyObject* targetPositionsObj = 0; PyObject* targetPositionsObj = 0;
PyObject* targetVelocitiesObj = 0; PyObject* targetVelocitiesObj = 0;
@@ -1269,11 +1269,17 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int physicsClientId = 0; int physicsClientId = 0;
static char* kwlist[] = {"bodyIndex", "jointIndices", "controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "jointIndices", "controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOi|OOOOOi", kwlist, &bodyIndex, &jointIndicesObj, &controlMode, if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOi|OOOOOi", kwlist, &bodyUniqueId, &jointIndicesObj, &controlMode,
&targetPositionsObj, &targetVelocitiesObj, &forcesObj, &kpsObj, &kdsObj, &physicsClientId)) &targetPositionsObj, &targetVelocitiesObj, &forcesObj, &kpsObj, &kdsObj, &physicsClientId))
{ {
return NULL; static char* kwlist2[] = {"bodyIndex", "jointIndices", "controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOi|OOOOOi", kwlist2, &bodyUniqueId, &jointIndicesObj, &controlMode,
&targetPositionsObj, &targetVelocitiesObj, &forcesObj, &kpsObj, &kdsObj, &physicsClientId))
{
return NULL;
}
} }
sm = getPhysicsClient(physicsClientId); sm = getPhysicsClient(physicsClientId);
if (sm == 0) if (sm == 0)
@@ -1297,7 +1303,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
PyObject* kpsSeq = 0; PyObject* kpsSeq = 0;
PyObject* kdsSeq = 0; PyObject* kdsSeq = 0;
numJoints = b3GetNumJoints(sm, bodyIndex); numJoints = b3GetNumJoints(sm, bodyUniqueId);
if ((controlMode != CONTROL_MODE_VELOCITY) && if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) && (controlMode != CONTROL_MODE_TORQUE) &&
@@ -1445,39 +1451,45 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
kdsSeq = PySequence_Fast(kdsObj, "expected a sequence of kds"); kdsSeq = PySequence_Fast(kdsObj, "expected a sequence of kds");
} }
commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode);
for (i=0;i<numControlledDofs;i++) for (i=0;i<numControlledDofs;i++)
{ {
double targetVelocity = 0.0; double targetVelocity = 0.0;
double targetPosition = 0.0;
double force = 100000.0;
double kp = 0.1;
double kd = 1.0;
int jointIndex;
if (targetVelocitiesSeq) if (targetVelocitiesSeq)
{ {
targetVelocity = pybullet_internalGetFloatFromSequence(targetVelocitiesSeq, i); targetVelocity = pybullet_internalGetFloatFromSequence(targetVelocitiesSeq, i);
} }
double targetPosition = 0.0;
if (targetPositionsSeq) if (targetPositionsSeq)
{ {
targetPosition = pybullet_internalGetFloatFromSequence(targetPositionsSeq, i); targetPosition = pybullet_internalGetFloatFromSequence(targetPositionsSeq, i);
} }
double force = 100000.0;
if (forcesSeq) if (forcesSeq)
{ {
force = pybullet_internalGetFloatFromSequence(forcesSeq, i); force = pybullet_internalGetFloatFromSequence(forcesSeq, i);
} }
double kp = 0.1;
if (kpsSeq) if (kpsSeq)
{ {
kp = pybullet_internalGetFloatFromSequence(kpsSeq, i); kp = pybullet_internalGetFloatFromSequence(kpsSeq, i);
} }
double kd = 1.0;
if (kdsSeq) if (kdsSeq)
{ {
kd = pybullet_internalGetFloatFromSequence(kdsSeq, i); kd = pybullet_internalGetFloatFromSequence(kdsSeq, i);
} }
int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i); jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info); b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
switch (controlMode) switch (controlMode)
{ {
@@ -1526,7 +1538,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int bodyIndex, jointIndex, controlMode; int bodyUniqueId, jointIndex, controlMode;
double targetPosition = 0.0; double targetPosition = 0.0;
double targetVelocity = 0.0; double targetVelocity = 0.0;
@@ -1536,11 +1548,18 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args,
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int physicsClientId = 0; int physicsClientId = 0;
static char* kwlist[] = {"bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist, &bodyIndex, &jointIndex, &controlMode, if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
&targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId)) &targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId))
{ {
return NULL; //backward compatibility, bodyIndex -> bodyUniqueId, don't need to update this function: people have to migrate to bodyUniqueId
static char* kwlist2[] = {"bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist2, &bodyUniqueId, &jointIndex, &controlMode,
&targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId))
{
return NULL;
}
} }
sm = getPhysicsClient(physicsClientId); sm = getPhysicsClient(physicsClientId);
if (sm == 0) if (sm == 0)
@@ -1555,7 +1574,7 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args,
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
struct b3JointInfo info; struct b3JointInfo info;
numJoints = b3GetNumJoints(sm, bodyIndex); numJoints = b3GetNumJoints(sm, bodyUniqueId);
if ((jointIndex >= numJoints) || (jointIndex < 0)) if ((jointIndex >= numJoints) || (jointIndex < 0))
{ {
PyErr_SetString(SpamError, "Joint index out-of-range."); PyErr_SetString(SpamError, "Joint index out-of-range.");
@@ -1570,9 +1589,9 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args,
return NULL; return NULL;
} }
commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info); b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
switch (controlMode) switch (controlMode)
{ {
@@ -1789,7 +1808,7 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args, PyObject* keywds)
} }
static int pybullet_internalGetBaseVelocity( static int pybullet_internalGetBaseVelocity(
int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3], b3PhysicsClientHandle sm) int bodyUniqueId, double baseLinearVelocity[3], double baseAngularVelocity[3], b3PhysicsClientHandle sm)
{ {
baseLinearVelocity[0] = 0.; baseLinearVelocity[0] = 0.;
baseLinearVelocity[1] = 0.; baseLinearVelocity[1] = 0.;
@@ -1808,7 +1827,7 @@ static int pybullet_internalGetBaseVelocity(
{ {
{ {
b3SharedMemoryCommandHandle cmd_handle = b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex); b3RequestActualStateCommandInit(sm, bodyUniqueId);
b3SharedMemoryStatusHandle status_handle = b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
@@ -1850,7 +1869,7 @@ static int pybullet_internalGetBaseVelocity(
// Internal function used to get the base position and orientation // Internal function used to get the base position and orientation
// Orientation is returned in quaternions // Orientation is returned in quaternions
static int pybullet_internalGetBasePositionAndOrientation( static int pybullet_internalGetBasePositionAndOrientation(
int bodyIndex, double basePosition[3], double baseOrientation[4], b3PhysicsClientHandle sm) int bodyUniqueId, double basePosition[3], double baseOrientation[4], b3PhysicsClientHandle sm)
{ {
basePosition[0] = 0.; basePosition[0] = 0.;
basePosition[1] = 0.; basePosition[1] = 0.;
@@ -1870,7 +1889,7 @@ static int pybullet_internalGetBasePositionAndOrientation(
{ {
{ {
b3SharedMemoryCommandHandle cmd_handle = b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex); b3RequestActualStateCommandInit(sm, bodyUniqueId);
b3SharedMemoryStatusHandle status_handle = b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
@@ -2389,7 +2408,7 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb
static char* kwlist[] = {"objectUniqueId", "linearVelocity", "angularVelocity", "physicsClientId", NULL}; static char* kwlist[] = {"objectUniqueId", "linearVelocity", "angularVelocity", "physicsClientId", NULL};
{ {
int bodyIndex = 0; int bodyUniqueId = 0;
PyObject* linVelObj = 0; PyObject* linVelObj = 0;
PyObject* angVelObj = 0; PyObject* angVelObj = 0;
double linVel[3] = {0, 0, 0}; double linVel[3] = {0, 0, 0};
@@ -2397,7 +2416,7 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb
int physicsClientId = 0; int physicsClientId = 0;
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist, &bodyIndex, &linVelObj, &angVelObj, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist, &bodyUniqueId, &linVelObj, &angVelObj, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -2414,7 +2433,7 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
if (linVelObj) if (linVelObj)
{ {
@@ -2529,10 +2548,10 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
return Py_None; return Py_None;
} }
// Get the a single joint info for a specific bodyIndex // Get the a single joint info for a specific bodyUniqueId
// //
// Args: // Args:
// bodyIndex - integer indicating body in simulation // bodyUniqueId - integer indicating body in simulation
// jointIndex - integer indicating joint for a specific body // jointIndex - integer indicating joint for a specific body
// //
// Joint information includes: // Joint information includes:
@@ -2568,7 +2587,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
{ {
{ {
// printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); // printf("body index = %d, joint index =%d\n", bodyUniqueId, jointIndex);
pyListJointInfo = PyTuple_New(jointInfoSize); pyListJointInfo = PyTuple_New(jointInfoSize);
@@ -2620,10 +2639,10 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
return Py_None; return Py_None;
} }
// Returns the state of a specific joint in a given bodyIndex // Returns the state of a specific joint in a given bodyUniqueId
// //
// Args: // Args:
// bodyIndex - integer indicating body in simulation // bodyUniqueId - integer indicating body in simulation
// jointIndex - integer indicating joint for a specific body // jointIndex - integer indicating joint for a specific body
// //
// The state of a joint includes the following: // The state of a joint includes the following:
@@ -2668,7 +2687,7 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject
if (bodyUniqueId < 0) if (bodyUniqueId < 0)
{ {
PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex"); PyErr_SetString(SpamError, "getJointState failed; invalid bodyUniqueId");
return NULL; return NULL;
} }
if (jointIndex < 0) if (jointIndex < 0)
@@ -2764,7 +2783,7 @@ static PyObject* pybullet_getJointStates(PyObject* self, PyObject* args, PyObjec
if (bodyUniqueId < 0) if (bodyUniqueId < 0)
{ {
PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex"); PyErr_SetString(SpamError, "getJointState failed; invalid bodyUniqueId");
return NULL; return NULL;
} }
numJoints = b3GetNumJoints(sm, bodyUniqueId); numJoints = b3GetNumJoints(sm, bodyUniqueId);
@@ -2888,7 +2907,7 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject*
if (bodyUniqueId < 0) if (bodyUniqueId < 0)
{ {
PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex"); PyErr_SetString(SpamError, "getLinkState failed; invalid bodyUniqueId");
return NULL; return NULL;
} }
if (linkIndex < 0) if (linkIndex < 0)
@@ -5722,7 +5741,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* args, PyObject* keywds) PyObject* args, PyObject* keywds)
{ {
int bodyIndex; int bodyUniqueId;
int endEffectorLinkIndex; int endEffectorLinkIndex;
PyObject* targetPosObj = 0; PyObject* targetPosObj = 0;
@@ -5736,10 +5755,16 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* restPosesObj = 0; PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0; PyObject* jointDampingObj = 0;
static char* kwlist[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId))
{ {
return NULL; //backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version
static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist2, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId))
{
return NULL;
}
} }
sm = getPhysicsClient(physicsClientId); sm = getPhysicsClient(physicsClientId);
if (sm == 0) if (sm == 0)
@@ -5759,7 +5784,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0; int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0;
int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0; int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0;
int numJoints = b3GetNumJoints(sm, bodyIndex); int numJoints = b3GetNumJoints(sm, bodyUniqueId);
int hasNullSpace = 0; int hasNullSpace = 0;
int hasJointDamping = 0; int hasJointDamping = 0;
@@ -5812,7 +5837,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int resultBodyIndex; int resultBodyIndex;
int result; int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyIndex); b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
if (hasNullSpace) if (hasNullSpace)
{ {
@@ -5893,17 +5918,23 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
PyObject* args, PyObject* keywds) PyObject* args, PyObject* keywds)
{ {
{ {
int bodyIndex; int bodyUniqueId;
PyObject* objPositionsQ; PyObject* objPositionsQ;
PyObject* objVelocitiesQdot; PyObject* objVelocitiesQdot;
PyObject* objAccelerations; PyObject* objAccelerations;
int physicsClientId = 0; int physicsClientId = 0;
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyIndex, &objPositionsQ, if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId)) &objVelocitiesQdot, &objAccelerations, &physicsClientId))
{ {
static char* kwlist2[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId))
{
return NULL; return NULL;
}
} }
sm = getPhysicsClient(physicsClientId); sm = getPhysicsClient(physicsClientId);
if (sm == 0) if (sm == 0)
@@ -5916,7 +5947,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
int szObPos = PySequence_Size(objPositionsQ); int szObPos = PySequence_Size(objPositionsQ);
int szObVel = PySequence_Size(objVelocitiesQdot); int szObVel = PySequence_Size(objVelocitiesQdot);
int szObAcc = PySequence_Size(objAccelerations); int szObAcc = PySequence_Size(objAccelerations);
int numJoints = b3GetNumJoints(sm, bodyIndex); int numJoints = b3GetNumJoints(sm, bodyUniqueId);
if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) &&
(szObAcc == numJoints)) (szObAcc == numJoints))
{ {
@@ -5943,7 +5974,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
int statusType; int statusType;
b3SharedMemoryCommandHandle commandHandle = b3SharedMemoryCommandHandle commandHandle =
b3CalculateInverseDynamicsCommandInit( b3CalculateInverseDynamicsCommandInit(
sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot, sm, bodyUniqueId, jointPositionsQ, jointVelocitiesQdot,
jointAccelerations); jointAccelerations);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);

4
src/Bullet3Common/b3HashMap.h
View File

@@ -44,8 +44,8 @@ struct b3HashString
/* Fowler / Noll / Vo (FNV) Hash */ /* Fowler / Noll / Vo (FNV) Hash */
unsigned int hash = InitialFNV; unsigned int hash = InitialFNV;
int len = m_string.length();
for(int i = 0; m_string[i]; i++) for(int i = 0; i<len; i++)
{ {
hash = hash ^ (m_string[i]); /* xor the low 8 bits */ hash = hash ^ (m_string[i]); /* xor the low 8 bits */
hash = hash * FNVMultiple; /* multiply by the magic number */ hash = hash * FNVMultiple; /* multiply by the magic number */