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

[pybullet] getNumConstraints, getConstraintInfo APIs.

Browse Source 
[pybullet] updated pybullet_quickstartguide.pdf
Fail clearly (assert, return BT_INFINITY) if link index is out of range for btMultiBody methods localPosToWorld,worldPosToLocal,localDirToWorld,worldDirToLocal.
pybullet getConstraintInfo
Fix warnings due to Mac OSX 10.12 upgrade (with backward compatibility)
This commit is contained in:
Erwin Coumans
2017-01-22 19:08:31 -08:00
parent f237c4440f
commit cf9f022d39
17 changed files with 841 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -336,9 +336,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
{
//const char* nodeName = node->Attribute("id");
//printf("processing node %s\n", nodeName);
btMatrix4x4 nodeTrans;
nodeTrans.setIdentity();

97
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -10,6 +10,61 @@
#include <stddef.h>
#include <string.h>
#include <Availability.h>
//aargh, Mac OSX 10.12 broke backwards compatibility, fix it here for now
#ifdef __MAC_10_12
#define MyNSTitledWindowMask NSWindowStyleMaskTitled
#define MyNSResizableWindowMask NSWindowStyleMaskResizable
#define MyNSClosableWindowMask NSWindowStyleMaskClosable
#define MyNSMiniaturizableWindowMask NSWindowStyleMaskMiniaturizable
#define MyNSAnyEventMask NSEventMaskAny
#define MyNSEventTypeFlagsChanged NSEventTypeFlagsChanged
#define MyNSEventModifierFlagShift NSEventModifierFlagShift
#define MyNSEventModifierFlagControl NSEventModifierFlagControl
#define MyNSEventModifierFlagOption NSEventModifierFlagOption
#define MyNSKeyUp NSEventTypeKeyUp
#define MyNSKeyDown NSEventTypeKeyDown
#define MyNSRightMouseDown NSEventTypeRightMouseDown
#define MyNSLeftMouseDown NSEventTypeLeftMouseDown
#define MyNSOtherMouseDown NSEventTypeOtherMouseDown
#define MyNSRightMouseUp NSEventTypeRightMouseUp
#define MyNSLeftMouseUp NSEventTypeLeftMouseUp
#define MyNSOtherMouseUp NSEventTypeOtherMouseUp
#define MyNSMouseMoved NSEventTypeMouseMoved
#define MyNSLeftMouseDragged NSEventTypeLeftMouseDragged
#define MyNSRightMouseDragged NSEventTypeRightMouseDragged
#define MyNSOtherMouseDragged NSEventTypeOtherMouseDragged
#define MyNSScrollWheel NSEventTypeScrollWheel
#else
#define MyNSTitledWindowMask NSTitledWindowMask
#define MyNSResizableWindowMask NSResizableWindowMask
#define MyNSClosableWindowMask NSClosableWindowMask
#define MyNSMiniaturizableWindowMask NSMiniaturizableWindowMask
#define MyNSAnyEventMask NSAnyEventMask
#define MyNSEventTypeFlagsChanged NSFlagsChanged
#define MyNSEventModifierFlagShift NSShiftKeyMask
#define MyNSEventModifierFlagControl NSControlKeyMask
#define MyNSEventModifierFlagOption NSAlternateKeyMask
#define MyNSKeyUp NSKeyUp
#define MyNSKeyDown NSKeyDown
#define NSRightMouseDown NSRightMouseDown
#define MyNSLeftMouseDown LeftMouseDown
#define MyNSOtherMouseDown NSOtherMouseDown
#define MyNSLeftMouseUp NSLeftMouseUp
#define MyNSRightMouseUp NSRightMouseUp
#define MyNSOtherMouseUp NSOtherMouseUp
#define MyNSMouseMoved NSMouseMoved
#define MyNSLeftMouseDragged NSLeftMouseDragged
#define MyNSRightMouseDragged NSRightMouseDragged
#define MyNSOtherMouseDragged NSOtherMouseDragged
#define MyNSScrollWheel NSScrollWheel
#endif
enum
{
@@ -368,7 +423,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame
styleMask:NSTitledWindowMask |NSResizableWindowMask| NSClosableWindowMask | NSMiniaturizableWindowMask
styleMask:MyNSTitledWindowMask |MyNSResizableWindowMask| MyNSClosableWindowMask | MyNSMiniaturizableWindowMask
backing:NSBackingStoreBuffered
defer:false];
@@ -771,7 +826,7 @@ void MacOpenGLWindow::startRendering()
[pool release];
pool = [[NSAutoreleasePool alloc] init];
event = [m_internalData->m_myApp
nextEventMatchingMask:NSAnyEventMask
nextEventMatchingMask:MyNSAnyEventMask
untilDate:[NSDate distantPast]
inMode:NSDefaultRunLoopMode
// inMode:NSEventTrackingRunLoopMode
@@ -780,12 +835,12 @@ void MacOpenGLWindow::startRendering()
//NSShiftKeyMask = 1 << 17,
//NSControlKeyMask
if ([event type] == NSFlagsChanged)
if ([event type] == MyNSEventTypeFlagsChanged)
{
int modifiers = [event modifierFlags];
if (m_keyboardCallback)
{
if ((modifiers & NSShiftKeyMask))
if ((modifiers & MyNSEventModifierFlagShift))
{
m_keyboardCallback(B3G_SHIFT,1);
m_modifierFlags |= MY_MAC_SHIFTKEY;
@@ -797,7 +852,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_SHIFTKEY;
}
}
if (modifiers & NSControlKeyMask)
if (modifiers & MyNSEventModifierFlagControl)
{
m_keyboardCallback(B3G_CONTROL,1);
m_modifierFlags |= MY_MAC_CONTROL_KEY;
@@ -809,7 +864,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_CONTROL_KEY;
}
}
if (modifiers & NSAlternateKeyMask)
if (modifiers & MyNSEventModifierFlagOption)
{
m_keyboardCallback(B3G_ALT,1);
m_modifierFlags |= MY_MAC_ALTKEY;
@@ -826,7 +881,7 @@ void MacOpenGLWindow::startRendering()
}
}
if ([event type] == NSKeyUp)
if ([event type] == MyNSKeyUp)
{
handledEvent = true;
@@ -841,7 +896,9 @@ void MacOpenGLWindow::startRendering()
m_keyboardCallback(keycode,state);
}
}
if ([event type] == NSKeyDown)
if ([event type] == MyNSKeyDown)
{
handledEvent = true;
@@ -861,7 +918,8 @@ void MacOpenGLWindow::startRendering()
}
if (([event type]== NSRightMouseDown) || ([ event type]==NSLeftMouseDown)||([event type]==NSOtherMouseDown))
if (([event type]== MyNSRightMouseDown) || ([ event type]==MyNSLeftMouseDown)||([event type]==MyNSOtherMouseDown))
{
// printf("right mouse!");
// float mouseX,mouseY;
@@ -873,17 +931,17 @@ void MacOpenGLWindow::startRendering()
int button=0;
switch ([event type])
{
case NSLeftMouseDown:
case MyNSLeftMouseDown:
{
button=0;
break;
}
case NSOtherMouseDown:
case MyNSOtherMouseDown:
{
button=1;
break;
}
case NSRightMouseDown:
case MyNSRightMouseDown:
{
button=2;
break;
@@ -902,7 +960,7 @@ void MacOpenGLWindow::startRendering()
}
if (([event type]== NSRightMouseUp) || ([ event type]==NSLeftMouseUp)||([event type]==NSOtherMouseUp))
if (([event type]== MyNSRightMouseUp) || ([ event type]==MyNSLeftMouseUp)||([event type]==MyNSOtherMouseUp))
{
// printf("right mouse!");
// float mouseX,mouseY;
@@ -915,17 +973,17 @@ void MacOpenGLWindow::startRendering()
int button=0;
switch ([event type])
{
case NSLeftMouseUp:
case MyNSLeftMouseUp:
{
button=0;
break;
}
case NSOtherMouseUp:
case MyNSOtherMouseUp:
{
button=1;
break;
}
case NSRightMouseUp:
case MyNSRightMouseUp:
{
button=2;
break;
@@ -943,7 +1001,7 @@ void MacOpenGLWindow::startRendering()
}
if ([event type] == NSMouseMoved)
if ([event type] == MyNSMouseMoved)
{
NSPoint eventLocation = [event locationInWindow];
@@ -960,7 +1018,8 @@ void MacOpenGLWindow::startRendering()
}
}
if (([event type] == NSLeftMouseDragged) || ([event type] == NSRightMouseDragged) || ([event type] == NSOtherMouseDragged))
if (([event type] == MyNSLeftMouseDragged) || ([event type] == MyNSRightMouseDragged) || ([event type] == MyNSOtherMouseDragged))
{
int dx1, dy1;
CGGetLastMouseDelta (&dx1, &dy1);
@@ -979,7 +1038,7 @@ void MacOpenGLWindow::startRendering()
// printf("mouse coord = %f, %f\n",m_mouseX,m_mouseY);
}
if ([event type] == NSScrollWheel)
if ([event type] == MyNSScrollWheel)
{
float dy, dx;
dy = [ event deltaY ];

4
examples/SharedMemory/PhysicsClient.h
View File

@@ -34,6 +34,10 @@ public:
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;

25
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1045,6 +1045,31 @@ int b3GetNumBodies(b3PhysicsClientHandle physClient)
return cl->getNumBodies();
}
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumUserConstraints();
}
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* infoPtr)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3UserConstraint constraintInfo1;
b3Assert(physClient);
b3Assert(infoPtr);
b3Assert(constraintUniqueId>=0);
if (infoPtr==0)
return 0;
if (cl->getUserConstraintInfo(constraintUniqueId, constraintInfo1))
{
*infoPtr = constraintInfo1;
return 1;
}
return 0;
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -57,7 +57,7 @@ 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
///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
///return the total number of bodies in the simulation
@@ -76,15 +76,21 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
///change parameters of an existing user constraint
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
@@ -320,7 +326,7 @@ void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastI
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
///experiments of robots interacting with non-rigid objects (such as btSoftBody)
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);

197
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -28,6 +28,7 @@ struct PhysicsClientSharedMemoryInternalData {
SharedMemoryBlock* m_testBlock1;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
@@ -46,6 +47,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus;
@@ -138,6 +141,22 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
return false;
}
int PhysicsClientSharedMemory::getNumUserConstraints() const
{
return m_data->m_userConstraintInfoMap.size();
}
int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const
{
b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId];
if (constraintPtr)
{
info = *constraintPtr;
return 1;
}
return 0;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{
@@ -156,6 +175,8 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
if (m_data->m_isConnected) {
disconnectSharedMemory();
}
resetData();
if (m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
@@ -163,6 +184,34 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
delete m_data;
}
void PhysicsClientSharedMemory::resetData()
{
m_data->m_debugLinesFrom.clear();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
@@ -398,7 +447,72 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
if (userConstraintPtr)
{
const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
}
break;
}
case CMD_USER_CONSTRAINT_FAILED:
{
b3Warning("createConstraint failed");
break;
}
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{
b3Warning("removeConstraint failed");
break;
}
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:
{
b3Warning("request actual state failed");
break;
}
case CMD_BODY_INFO_COMPLETED:
{
if (m_data->m_verboseOutput) {
@@ -497,30 +611,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
}
m_data->m_debugLinesFrom.clear();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
resetData();
break;
}
case CMD_DEBUG_LINES_COMPLETED: {
@@ -802,20 +894,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("User debug draw failed");
break;
}
case CMD_USER_CONSTRAINT_COMPLETED:
{
break;
}
case CMD_USER_CONSTRAINT_FAILED:
{
b3Warning("createConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:
{
b3Warning("request actual state failed");
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
{
break;
@@ -841,6 +920,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
{
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i=0;i<numConstraints;i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0)
{
@@ -863,6 +948,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
{
if (m_data->m_constraintIdsRequestInfo.size())
{
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
m_data->m_constraintIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_USER_CONSTRAINT;
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
submitClientCommand(command);
return 0;
}
else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{
//are there any bodies left to be processed?
@@ -879,7 +983,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
return 0;
} else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
if (m_data->m_constraintIdsRequestInfo.size())
{
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
m_data->m_constraintIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_USER_CONSTRAINT;
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
submitClientCommand(command);
return 0;
} else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
}

8
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -12,8 +12,8 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
@@ -44,6 +44,10 @@ public:
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual void setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len);

99
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -19,6 +19,8 @@ struct BodyJointInfoCache2
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
struct PhysicsDirectInternalData
{
DummyGUIHelper m_noGfx;
@@ -34,6 +36,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
@@ -112,6 +115,7 @@ void PhysicsDirect::resetData()
}
}
m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
}
// return true if connection succesfull, can also check 'isConnected'
@@ -670,6 +674,47 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break;
}
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
case CMD_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
if (userConstraintPtr)
{
const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
}
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:
@@ -677,6 +722,29 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
//we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i=0;i<numConstraints;i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
SharedMemoryCommand infoRequestCommand;
infoRequestCommand.m_type = CMD_USER_CONSTRAINT;
infoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
SharedMemoryStatus infoStatus;
bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
if (hasStatus)
{
int cid = infoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,infoStatus.m_userConstraintResultArgs);
}
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i = 0; i<numBodies; i++)
@@ -721,10 +789,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
break;
}
case CMD_USER_CONSTRAINT_COMPLETED:
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{
b3Warning("removeConstraint failed");
break;
}
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
break;
}
case CMD_USER_CONSTRAINT_FAILED:
{
b3Warning("createConstraint failed");
@@ -765,10 +841,12 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
/*if (hasStatus)
{
postProcessStatus(m_data->m_serverStatus);
m_data->m_hasStatus = false;
}
*/
return hasStatus;
}
@@ -777,6 +855,23 @@ int PhysicsDirect::getNumBodies() const
return m_data->m_bodyJointMap.size();
}
int PhysicsDirect::getNumUserConstraints() const
{
return m_data->m_userConstraintInfoMap.size();
}
int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const
{
b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId];
if (constraintPtr)
{
info = *constraintPtr;
return 1;
}
return 0;
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{

6
examples/SharedMemory/PhysicsDirect.h
View File

@@ -63,6 +63,10 @@ public:
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);
@@ -86,7 +90,7 @@ public:
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
//those 2 APIs are for internal use for visualization
//the following APIs are for internal use for visualization:
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();
virtual void debugDraw(int debugDrawMode);

11
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -99,7 +99,16 @@ bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3Joint
return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
}
///todo: move this out of the
int PhysicsLoopBack::getNumUserConstraints() const
{
return m_data->m_physicsClient->getNumUserConstraints();
}
int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const
{
return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
}
///todo: move this out of the interface
void PhysicsLoopBack::setSharedMemoryKey(int key)
{
m_data->m_physicsServer->setSharedMemoryKey(key);

4
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -48,6 +48,10 @@ public:
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

362
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -135,6 +135,9 @@ struct InteralUserConstraintData
{
btTypedConstraint* m_rbConstraint;
btMultiBodyConstraint* m_mbConstraint;
b3UserConstraint m_userConstraintData;
InteralUserConstraintData()
:m_rbConstraint(0),
m_mbConstraint(0)
@@ -834,6 +837,9 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
deleteCachedInverseDynamicsBodies();
deleteCachedInverseKinematicsBodies();
m_data->m_userConstraints.clear();
m_data->m_saveWorldBodyData.clear();
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{
delete m_data->m_multiBodyJointFeedbacks[i];
@@ -1760,6 +1766,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies;
int usz = m_data->m_userConstraints.size();
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;
for (int i=0;i<usz;i++)
{
int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId;
serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key;
}
serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
hasStatus = true;
break;
@@ -1785,7 +1802,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
int constraintCount = 0;
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
if (f)
{
@@ -1900,6 +1917,93 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
};
}
//user constraints
{
for (int i=0;i<m_data->m_userConstraints.size();i++)
{
InteralUserConstraintData* ucptr = m_data->m_userConstraints.getAtIndex(i);
b3UserConstraint& uc = ucptr->m_userConstraintData;
int parentBodyIndex=uc.m_parentBodyIndex;
int parentJointIndex=uc.m_parentJointIndex;
int childBodyIndex=uc.m_childBodyIndex;
int childJointIndex=uc.m_childJointIndex;
btVector3 jointAxis(uc.m_jointAxis[0],uc.m_jointAxis[1],uc.m_jointAxis[2]);
btVector3 pivotParent(uc.m_parentFrame[0],uc.m_parentFrame[1],uc.m_parentFrame[2]);
btVector3 pivotChild(uc.m_childFrame[0],uc.m_childFrame[1],uc.m_childFrame[2]);
btQuaternion ornFrameParent(uc.m_parentFrame[3],uc.m_parentFrame[4],uc.m_parentFrame[5],uc.m_parentFrame[6]);
btQuaternion ornFrameChild(uc.m_childFrame[3],uc.m_childFrame[4],uc.m_childFrame[5],uc.m_childFrame[6]);
{
char jointTypeStr[1024]="FIXED";
bool hasKnownJointType = true;
switch (uc.m_jointType)
{
case eRevoluteType:
{
sprintf(jointTypeStr,"p.JOINT_REVOLUTE");
break;
}
case ePrismaticType:
{
sprintf(jointTypeStr,"p.JOINT_PRISMATIC");
break;
}
case eSphericalType:
{
sprintf(jointTypeStr,"p.JOINT_SPHERICAL");
break;
}
case ePlanarType:
{
sprintf(jointTypeStr,"p.JOINT_PLANAR");
break;
}
case eFixedType :
{
sprintf(jointTypeStr,"p.JOINT_FIXED");
break;
}
case ePoint2PointType:
{
sprintf(jointTypeStr,"p.JOINT_POINT2POINT");
break; }
default:
{
hasKnownJointType = false;
b3Warning("unknown constraint type in SAVE_WORLD");
}
};
if (hasKnownJointType)
{
{
sprintf(line,"cid%d = p.createConstraint(%d,%d,%d,%d,%s,[%f,%f,%f],[%f,%f,%f],[%f,%f,%f],[%f,%f,%f,%f],[%f,%f,%f,%f])\n",
constraintCount,
parentBodyIndex,
parentJointIndex,
childBodyIndex,
childJointIndex,
jointTypeStr,
jointAxis[0],jointAxis[1],jointAxis[2],
pivotParent[0],pivotParent[1],pivotParent[2],
pivotChild[0],pivotChild[1],pivotChild[2],
ornFrameParent[0],ornFrameParent[1],ornFrameParent[2],ornFrameParent[3],
ornFrameChild[0],ornFrameChild[1],ornFrameChild[2],ornFrameChild[3]
);
int len = strlen(line);
fwrite(line,len,1,f);
}
{
sprintf(line,"p.changeConstraint(cid%d,maxForce=%f)\n",constraintCount,uc.m_maxAppliedForce);
int len = strlen(line);
fwrite(line,len,1,f);
constraintCount++;
}
}
}
}
}
{
btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
@@ -1942,6 +2046,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++)
{
@@ -3545,126 +3650,159 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true;
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO)
{
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
if (userConstraintPtr)
{
serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData;
serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
{
btScalar defaultMaxForce = 500.0;
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody)
{
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
//also create a constraint with just a single multibody/rigid body without child
//if (childBody)
if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
{
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
//also create a constraint with just a single multibody/rigid body without child
//if (childBody)
{
if (childBody && childBody->m_multiBody)
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
{
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
multibodyFixed->setMaxAppliedImpulse(500.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
if (childBody && childBody->m_multiBody)
{
if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex <childBody->m_multiBody->getNumLinks()))
{
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
multibodyFixed->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
else
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(500.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
else
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
{
if (childBody && childBody->m_multiBody)
{
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
multibodySlider->setMaxAppliedImpulse(500.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodySlider;
int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
else
else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
if (childBody && childBody->m_multiBody)
{
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
multibodySlider->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodySlider;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
else
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(500.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodySlider;
int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodySlider;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; }
} else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
{
if (childBody && childBody->m_multiBody)
} else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
{
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500);
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
else
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
if (childBody && childBody->m_multiBody)
{
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
else
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
} else
{
b3Warning("unknown constraint type");
}
} else
{
b3Warning("unknown constraint type");
}
}
}
}
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
{
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED;
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
if (userConstraintPtr)
{
if (userConstraintPtr->m_mbConstraint)
@@ -3674,7 +3812,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]);
userConstraintPtr->m_userConstraintData.m_childFrame[0] = clientCmd.m_userConstraintArguments.m_childFrame[0];
userConstraintPtr->m_userConstraintData.m_childFrame[1] = clientCmd.m_userConstraintArguments.m_childFrame[1];
userConstraintPtr->m_userConstraintData.m_childFrame[2] = clientCmd.m_userConstraintArguments.m_childFrame[2];
userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
@@ -3683,13 +3823,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_userConstraintArguments.m_childFrame[4],
clientCmd.m_userConstraintArguments.m_childFrame[5],
clientCmd.m_userConstraintArguments.m_childFrame[6]);
userConstraintPtr->m_userConstraintData.m_childFrame[3] = clientCmd.m_userConstraintArguments.m_childFrame[3];
userConstraintPtr->m_userConstraintData.m_childFrame[4] = clientCmd.m_userConstraintArguments.m_childFrame[4];
userConstraintPtr->m_userConstraintData.m_childFrame[5] = clientCmd.m_userConstraintArguments.m_childFrame[5];
userConstraintPtr->m_userConstraintData.m_childFrame[6] = clientCmd.m_userConstraintArguments.m_childFrame[6];
btMatrix3x3 childFrameBasis(childFrameOrn);
userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime;
userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp);
}
}
@@ -3697,12 +3841,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
//todo
}
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange;
serverCmd.m_updateFlags = clientCmd.m_updateFlags;
serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED;
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
{
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED;
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
if (userConstraintPtr)
@@ -3717,8 +3864,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
}
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;
}
@@ -3974,7 +4123,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int numRb = importer->getNumRigidBodies();
serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
for( int i=0;i<numRb;i++)
{
btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
@@ -4044,6 +4194,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++)
{
@@ -4539,7 +4690,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_gripperRigidbodyFixed = 0;
}
//todo: move this to Python/scripting
//todo: move this to Python/scripting (it is almost ready to be removed!)
void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
static btAlignedObjectArray<char> gBufferServerToClient;

30
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -432,6 +432,8 @@ struct SdfLoadedArgs
{
int m_numBodies;
int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_numUserConstraints;
int m_userConstraintUniqueIds[MAX_SDF_BODIES];
///@todo(erwincoumans) load cameras, lights etc
//int m_numCameras;
@@ -541,27 +543,13 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16,
USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
USER_CONSTRAINT_REQUEST_INFO=64,
};
struct UserConstraintArgs
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
};
struct UserConstraintResultArgs
{
int m_userConstraintUniqueId;
};
enum EnumUserDebugDrawFlags
{
@@ -659,7 +647,7 @@ struct SharedMemoryCommand
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct UserConstraintArgs m_userConstraintArguments;
struct b3UserConstraint m_userConstraintArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
@@ -708,6 +696,10 @@ struct SharedMemoryStatus
int m_numDataStreamBytes;
char* m_dataStream;
//m_updateFlags is a bit fields to tell which parameters were updated,
//m_updateFlags is ignored for most status messages
int m_updateFlags;
union
{
struct BulletDataStreamArgs m_dataStreamArguments;
@@ -723,7 +715,7 @@ struct SharedMemoryStatus
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct UserConstraintResultArgs m_userConstraintResultArgs;
struct b3UserConstraint m_userConstraintResultArgs;
struct SendVREvents m_sendVREvents;
struct SendRaycastHits m_raycastHits;
};

19
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -112,6 +112,11 @@ enum EnumSharedMemoryServerStatus
CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED,
CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED,
CMD_USER_CONSTRAINT_INFO_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_FAILED,
CMD_CHANGE_USER_CONSTRAINT_FAILED,
CMD_USER_CONSTRAINT_FAILED,
CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
@@ -164,6 +169,20 @@ struct b3JointInfo
double m_jointAxis[3]; // joint axis in parent local frame
};
struct b3UserConstraint
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
};
struct b3BodyInfo
{
const char* m_baseName;

134
examples/pybullet/pybullet.c
View File

@@ -1578,6 +1578,7 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
}
}
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
@@ -1650,6 +1651,122 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject*
}
static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int constraintUniqueId= -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char *kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&constraintUniqueId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(sm,constraintUniqueId, &constraintInfo))
{
PyObject* pyListConstraintInfo = PyTuple_New(11);
PyTuple_SetItem(pyListConstraintInfo,0,PyLong_FromLong(constraintInfo.m_parentBodyIndex));
PyTuple_SetItem(pyListConstraintInfo,1,PyLong_FromLong(constraintInfo.m_parentJointIndex));
PyTuple_SetItem(pyListConstraintInfo,2,PyLong_FromLong(constraintInfo.m_childBodyIndex));
PyTuple_SetItem(pyListConstraintInfo,3,PyLong_FromLong(constraintInfo.m_childJointIndex));
PyTuple_SetItem(pyListConstraintInfo,4,PyLong_FromLong(constraintInfo.m_jointType));
{
PyObject* axisObj = PyTuple_New(3);
PyTuple_SetItem(axisObj,0,PyFloat_FromDouble(constraintInfo.m_jointAxis[0]));
PyTuple_SetItem(axisObj,1,PyFloat_FromDouble(constraintInfo.m_jointAxis[1]));
PyTuple_SetItem(axisObj,2,PyFloat_FromDouble(constraintInfo.m_jointAxis[2]));
PyTuple_SetItem(pyListConstraintInfo,5,axisObj);
}
{
PyObject* parentFramePositionObj = PyTuple_New(3);
PyTuple_SetItem(parentFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[0]));
PyTuple_SetItem(parentFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[1]));
PyTuple_SetItem(parentFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[2]));
PyTuple_SetItem(pyListConstraintInfo,6,parentFramePositionObj);
}
{
PyObject* childFramePositionObj = PyTuple_New(3);
PyTuple_SetItem(childFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_childFrame[0]));
PyTuple_SetItem(childFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_childFrame[1]));
PyTuple_SetItem(childFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_childFrame[2]));
PyTuple_SetItem(pyListConstraintInfo,7,childFramePositionObj);
}
{
PyObject* parentFrameOrientationObj = PyTuple_New(4);
PyTuple_SetItem(parentFrameOrientationObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[3]));
PyTuple_SetItem(parentFrameOrientationObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[4]));
PyTuple_SetItem(parentFrameOrientationObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[5]));
PyTuple_SetItem(parentFrameOrientationObj,3,PyFloat_FromDouble(constraintInfo.m_parentFrame[6]));
PyTuple_SetItem(pyListConstraintInfo,8,parentFrameOrientationObj);
}
{
PyObject* childFrameOrientation = PyTuple_New(4);
PyTuple_SetItem(childFrameOrientation,0,PyFloat_FromDouble(constraintInfo.m_childFrame[3]));
PyTuple_SetItem(childFrameOrientation,1,PyFloat_FromDouble(constraintInfo.m_childFrame[4]));
PyTuple_SetItem(childFrameOrientation,2,PyFloat_FromDouble(constraintInfo.m_childFrame[5]));
PyTuple_SetItem(childFrameOrientation,3,PyFloat_FromDouble(constraintInfo.m_childFrame[6]));
PyTuple_SetItem(pyListConstraintInfo,9,childFrameOrientation);
}
PyTuple_SetItem(pyListConstraintInfo,10,PyFloat_FromDouble(constraintInfo.m_maxAppliedForce));
return pyListConstraintInfo;
} else
{
PyErr_SetString(SpamError, "Couldn't get user constraint info");
return NULL;
}
}
}
PyErr_SetString(SpamError, "error in getConstraintInfo.");
return NULL;
}
static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds)
{
int numConstraints = 0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char *kwlist[] = { "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
numConstraints = b3GetNumUserConstraints(sm);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(numConstraints);
#else
return PyInt_FromLong(numConstraints);
#endif
}
// Return the number of joints in an object based on
// body index; body index is based on order of sequence
// the object is loaded into simulation
@@ -2221,13 +2338,10 @@ static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args,
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int res = 0;
char* text;
double colorRGB[3]={1,1,1};
PyObject* textPositionObj=0;
double rangeMin = 0.f;
double rangeMax = 1.f;
double startValue = 0.f;
@@ -4541,7 +4655,7 @@ static PyMethodDef SpamMethods[] = {
"Load multibodies from an MJCF file." },
{"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS,
"Create a constraint between two bodies. Returns a (int) unique id, if successfull."
"Create a constraint between two bodies. Returns a (int) unique id, if successfull."
},
{"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS,
@@ -4561,11 +4675,21 @@ static PyMethodDef SpamMethods[] = {
"Get the number of bodies in the simulation."},
{"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS,
"Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."},
"getBodyUniqueId is used after connecting to server with existing bodies."
"Get the unique id of the body, given a integer range [0.. number of bodies)."},
{"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS,
"Get the body info, given a body unique id."},
{"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS| METH_KEYWORDS,
"Get the number of user-created constraints in the simulation."},
{"getConstraintInfo",(PyCFunction) pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
"Get the user-created constraint info, given a constraint unique id."},
{"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS,
"Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
{"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation,
METH_VARARGS | METH_KEYWORDS,
"Get the world position and orientation of the base of the object. "