AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 14:10:11 +00:00
Code Issues 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
commit 356d4104f0
11 changed files with 148 additions and 94 deletions
Show Stats 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')

141
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,12 +1269,18 @@ 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))
{
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)) &targetPositionsObj, &targetVelocitiesObj, &forcesObj, &kpsObj, &kdsObj, &physicsClientId))
{ {
return NULL; 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,12 +1548,19 @@ 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))
{
//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)) &targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId))
{ {
return NULL; 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,11 +5755,17 @@ 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))
{
//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; 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,18 +5918,24 @@ 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))
{
static char* kwlist2[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId)) &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 */