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

Merge pull request #926 from erwincoumans/master

Browse Source 
more improvements to pybullet (bodies/constraints are up-to-date after reconnection), pybullet quickstart guide
This commit is contained in:
erwincoumans
2017-01-22 21:22:10 -08:00
committed by GitHub
parent d834bc72af 73e83d6e84
commit d2c7d0b57d
18 changed files with 886 additions and 202 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

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) 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; btMatrix4x4 nodeTrans;
nodeTrans.setIdentity(); nodeTrans.setIdentity();

97
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -10,6 +10,61 @@
#include <stddef.h> #include <stddef.h>
#include <string.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 MyNSRightMouseDown NSRightMouseDown
#define MyNSLeftMouseDown NSLeftMouseDown
#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 enum
{ {
@@ -368,7 +423,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_internalData->m_window = [NSWindow alloc]; m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame [m_internalData->m_window initWithContentRect:frame
styleMask:NSTitledWindowMask |NSResizableWindowMask| NSClosableWindowMask | NSMiniaturizableWindowMask styleMask:MyNSTitledWindowMask |MyNSResizableWindowMask| MyNSClosableWindowMask | MyNSMiniaturizableWindowMask
backing:NSBackingStoreBuffered backing:NSBackingStoreBuffered
defer:false]; defer:false];
@@ -771,7 +826,7 @@ void MacOpenGLWindow::startRendering()
[pool release]; [pool release];
pool = [[NSAutoreleasePool alloc] init]; pool = [[NSAutoreleasePool alloc] init];
event = [m_internalData->m_myApp event = [m_internalData->m_myApp
nextEventMatchingMask:NSAnyEventMask nextEventMatchingMask:MyNSAnyEventMask
untilDate:[NSDate distantPast] untilDate:[NSDate distantPast]
inMode:NSDefaultRunLoopMode inMode:NSDefaultRunLoopMode
// inMode:NSEventTrackingRunLoopMode // inMode:NSEventTrackingRunLoopMode
@@ -780,12 +835,12 @@ void MacOpenGLWindow::startRendering()
//NSShiftKeyMask = 1 << 17, //NSShiftKeyMask = 1 << 17,
//NSControlKeyMask //NSControlKeyMask
if ([event type] == NSFlagsChanged) if ([event type] == MyNSEventTypeFlagsChanged)
{ {
int modifiers = [event modifierFlags]; int modifiers = [event modifierFlags];
if (m_keyboardCallback) if (m_keyboardCallback)
{ {
if ((modifiers & NSShiftKeyMask)) if ((modifiers & MyNSEventModifierFlagShift))
{ {
m_keyboardCallback(B3G_SHIFT,1); m_keyboardCallback(B3G_SHIFT,1);
m_modifierFlags |= MY_MAC_SHIFTKEY; m_modifierFlags |= MY_MAC_SHIFTKEY;
@@ -797,7 +852,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_SHIFTKEY; m_modifierFlags &= ~MY_MAC_SHIFTKEY;
} }
} }
if (modifiers & NSControlKeyMask) if (modifiers & MyNSEventModifierFlagControl)
{ {
m_keyboardCallback(B3G_CONTROL,1); m_keyboardCallback(B3G_CONTROL,1);
m_modifierFlags |= MY_MAC_CONTROL_KEY; m_modifierFlags |= MY_MAC_CONTROL_KEY;
@@ -809,7 +864,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_CONTROL_KEY; m_modifierFlags &= ~MY_MAC_CONTROL_KEY;
} }
} }
if (modifiers & NSAlternateKeyMask) if (modifiers & MyNSEventModifierFlagOption)
{ {
m_keyboardCallback(B3G_ALT,1); m_keyboardCallback(B3G_ALT,1);
m_modifierFlags |= MY_MAC_ALTKEY; m_modifierFlags |= MY_MAC_ALTKEY;
@@ -826,7 +881,7 @@ void MacOpenGLWindow::startRendering()
} }
} }
if ([event type] == NSKeyUp) if ([event type] == MyNSKeyUp)
{ {
handledEvent = true; handledEvent = true;
@@ -841,7 +896,9 @@ void MacOpenGLWindow::startRendering()
m_keyboardCallback(keycode,state); m_keyboardCallback(keycode,state);
} }
} }
if ([event type] == NSKeyDown)
if ([event type] == MyNSKeyDown)
{ {
handledEvent = true; 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!"); // printf("right mouse!");
// float mouseX,mouseY; // float mouseX,mouseY;
@@ -873,17 +931,17 @@ void MacOpenGLWindow::startRendering()
int button=0; int button=0;
switch ([event type]) switch ([event type])
{ {
case NSLeftMouseDown: case MyNSLeftMouseDown:
{ {
button=0; button=0;
break; break;
} }
case NSOtherMouseDown: case MyNSOtherMouseDown:
{ {
button=1; button=1;
break; break;
} }
case NSRightMouseDown: case MyNSRightMouseDown:
{ {
button=2; button=2;
break; 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!"); // printf("right mouse!");
// float mouseX,mouseY; // float mouseX,mouseY;
@@ -915,17 +973,17 @@ void MacOpenGLWindow::startRendering()
int button=0; int button=0;
switch ([event type]) switch ([event type])
{ {
case NSLeftMouseUp: case MyNSLeftMouseUp:
{ {
button=0; button=0;
break; break;
} }
case NSOtherMouseUp: case MyNSOtherMouseUp:
{ {
button=1; button=1;
break; break;
} }
case NSRightMouseUp: case MyNSRightMouseUp:
{ {
button=2; button=2;
break; break;
@@ -943,7 +1001,7 @@ void MacOpenGLWindow::startRendering()
} }
if ([event type] == NSMouseMoved) if ([event type] == MyNSMouseMoved)
{ {
NSPoint eventLocation = [event locationInWindow]; 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; int dx1, dy1;
CGGetLastMouseDelta (&dx1, &dy1); CGGetLastMouseDelta (&dx1, &dy1);
@@ -979,7 +1038,7 @@ void MacOpenGLWindow::startRendering()
// printf("mouse coord = %f, %f\n",m_mouseX,m_mouseY); // printf("mouse coord = %f, %f\n",m_mouseX,m_mouseY);
} }
if ([event type] == NSScrollWheel) if ([event type] == MyNSScrollWheel)
{ {
float dy, dx; float dy, dx;
dy = [ event deltaY ]; 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 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 setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 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(); 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() ) /// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) 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* actualStateQdot[],
const double* jointReactionForces[]); 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); b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
///return the total number of bodies in the simulation ///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); 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); 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); int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
///change parameters of an existing user constraint
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); 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 ///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 ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
@@ -320,7 +326,7 @@ void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastI
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); 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); 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) ///experiments of robots interacting with non-rigid objects (such as btSoftBody)
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale); int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);

197
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -28,6 +28,7 @@ struct PhysicsClientSharedMemoryInternalData {
SharedMemoryBlock* m_testBlock1; SharedMemoryBlock* m_testBlock1;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap; btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom; btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo; btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
@@ -46,6 +47,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3RayHitInfo> m_raycastHits; btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btAlignedObjectArray<int> m_bodyIdsRequestInfo; btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus; SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus; SharedMemoryStatus m_lastServerStatus;
@@ -138,6 +141,22 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
return false; 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() PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{ {
@@ -156,6 +175,8 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
if (m_data->m_isConnected) { if (m_data->m_isConnected) {
disconnectSharedMemory(); disconnectSharedMemory();
} }
resetData();
if (m_data->m_ownsSharedMemory) if (m_data->m_ownsSharedMemory)
{ {
delete m_data->m_sharedMemory; delete m_data->m_sharedMemory;
@@ -163,6 +184,34 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
delete m_data; 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; } void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
@@ -398,7 +447,72 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break; 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: case CMD_BODY_INFO_COMPLETED:
{ {
if (m_data->m_verboseOutput) { if (m_data->m_verboseOutput) {
@@ -497,30 +611,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) { if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
} }
m_data->m_debugLinesFrom.clear(); resetData();
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();
break; break;
} }
case CMD_DEBUG_LINES_COMPLETED: { case CMD_DEBUG_LINES_COMPLETED: {
@@ -802,20 +894,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("User debug draw failed"); b3Warning("User debug draw failed");
break; 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: case CMD_SYNC_BODY_INFO_COMPLETED:
{ {
break; 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)) 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; int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0) 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) if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{ {
//are there any bodies left to be processed? //are there any bodies left to be processed?
@@ -879,7 +983,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
return 0; return 0;
} else } 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: protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
public: public:
PhysicsClientSharedMemory(); PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory(); virtual ~PhysicsClientSharedMemory();
@@ -44,6 +44,10 @@ public:
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const; 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 setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len); virtual void uploadBulletFileToSharedMemory(const char* data, int len);

100
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -19,6 +19,8 @@ struct BodyJointInfoCache2
btAlignedObjectArray<b3JointInfo> m_jointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
}; };
struct PhysicsDirectInternalData struct PhysicsDirectInternalData
{ {
DummyGUIHelper m_noGfx; DummyGUIHelper m_noGfx;
@@ -34,6 +36,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<TmpFloat3> m_debugLinesColor; btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap; btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
@@ -112,6 +115,7 @@ void PhysicsDirect::resetData()
} }
} }
m_data->m_bodyJointMap.clear(); m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
} }
// return true if connection succesfull, can also check 'isConnected' // return true if connection succesfull, can also check 'isConnected'
@@ -670,6 +674,47 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break; 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_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED: case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED:
@@ -677,6 +722,30 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
//we'll stream further info from the physics server //we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy //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;
infoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid;
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; int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i = 0; i<numBodies; i++) for (int i = 0; i<numBodies; i++)
@@ -721,10 +790,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{ {
break; break;
} }
case CMD_USER_CONSTRAINT_COMPLETED:
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{ {
b3Warning("removeConstraint failed");
break; break;
} }
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
break;
}
case CMD_USER_CONSTRAINT_FAILED: case CMD_USER_CONSTRAINT_FAILED:
{ {
b3Warning("createConstraint failed"); b3Warning("createConstraint failed");
@@ -765,10 +842,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); 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; m_data->m_hasStatus = hasStatus;
if (hasStatus) /*if (hasStatus)
{ {
postProcessStatus(m_data->m_serverStatus); postProcessStatus(m_data->m_serverStatus);
m_data->m_hasStatus = false;
} }
*/
return hasStatus; return hasStatus;
} }
@@ -777,6 +856,23 @@ int PhysicsDirect::getNumBodies() const
return m_data->m_bodyJointMap.size(); 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 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 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 ///todo: move this out of the
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
@@ -86,7 +90,7 @@ public:
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); 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 bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene(); virtual void renderScene();
virtual void debugDraw(int debugDrawMode); 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); 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) void PhysicsLoopBack::setSharedMemoryKey(int key)
{ {
m_data->m_physicsServer->setSharedMemoryKey(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 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 ///todo: move this out of the
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);

362
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -135,6 +135,9 @@ struct InteralUserConstraintData
{ {
btTypedConstraint* m_rbConstraint; btTypedConstraint* m_rbConstraint;
btMultiBodyConstraint* m_mbConstraint; btMultiBodyConstraint* m_mbConstraint;
b3UserConstraint m_userConstraintData;
InteralUserConstraintData() InteralUserConstraintData()
:m_rbConstraint(0), :m_rbConstraint(0),
m_mbConstraint(0) m_mbConstraint(0)
@@ -834,6 +837,9 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
deleteCachedInverseDynamicsBodies(); deleteCachedInverseDynamicsBodies();
deleteCachedInverseKinematicsBodies(); deleteCachedInverseKinematicsBodies();
m_data->m_userConstraints.clear();
m_data->m_saveWorldBodyData.clear();
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++) for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{ {
delete m_data->m_multiBodyJointFeedbacks[i]; delete m_data->m_multiBodyJointFeedbacks[i];
@@ -1760,6 +1766,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies; 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; serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
hasStatus = true; hasStatus = true;
break; break;
@@ -1785,7 +1802,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName); //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
int constraintCount = 0;
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w"); FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
if (f) 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(); btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]); 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_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); 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); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++) for (int i=0;i<maxBodies;i++)
{ {
@@ -3545,126 +3650,159 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut; SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true; 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) if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
{ {
btScalar defaultMaxForce = 500.0;
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody) if (parentBody && parentBody->m_multiBody)
{ {
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
//also create a constraint with just a single multibody/rigid body without child
//if (childBody)
{ {
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); //also create a constraint with just a single multibody/rigid body without child
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])); //if (childBody)
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)
{ {
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); if (childBody && childBody->m_multiBody)
multibodyFixed->setMaxAppliedImpulse(500.0); {
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex <childBody->m_multiBody->getNumLinks()))
InteralUserConstraintData userConstraintData; {
userConstraintData.m_mbConstraint = multibodyFixed; btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
int uid = m_data->m_userConstraintUIDGenerator++; multibodyFixed->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_userConstraints.insert(uid,userConstraintData); m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; InteralUserConstraintData userConstraintData;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; 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 else
{ {
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(500.0); rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed); world->addMultiBodyConstraint(rigidbodyFixed);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodyFixed; userConstraintData.m_mbConstraint = rigidbodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; 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); btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(500.0); rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider); world->addMultiBodyConstraint(rigidbodySlider);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodySlider; userConstraintData.m_mbConstraint = rigidbodySlider;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; 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) } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
{
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); if (childBody && childBody->m_multiBody)
p2p->setMaxAppliedImpulse(500); {
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
InteralUserConstraintData userConstraintData; p2p->setMaxAppliedImpulse(defaultMaxForce);
userConstraintData.m_mbConstraint = p2p; m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
int uid = m_data->m_userConstraintUIDGenerator++; InteralUserConstraintData userConstraintData;
m_data->m_userConstraints.insert(uid,userConstraintData); userConstraintData.m_mbConstraint = p2p;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
} serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
else serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
{ userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; 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); btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500); p2p->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p); world->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p; userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; 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 } else
{ {
b3Warning("unknown constraint type"); b3Warning("unknown constraint type");
} }
}
}
} }
}
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
{ {
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
if (userConstraintPtr) if (userConstraintPtr)
{ {
if (userConstraintPtr->m_mbConstraint) if (userConstraintPtr->m_mbConstraint)
@@ -3674,7 +3812,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]); 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); userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB);
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) 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[4],
clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[5],
clientCmd.m_userConstraintArguments.m_childFrame[6]); 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); btMatrix3x3 childFrameBasis(childFrameOrn);
userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis);
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{ {
btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; 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); userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp);
} }
} }
@@ -3697,12 +3841,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
//todo //todo
} }
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; 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) if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
{ {
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED;
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
if (userConstraintPtr) if (userConstraintPtr)
@@ -3717,8 +3864,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
} }
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;
} }
@@ -3974,7 +4123,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int numRb = importer->getNumRigidBodies(); int numRb = importer->getNumRigidBodies();
serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
for( int i=0;i<numRb;i++) for( int i=0;i<numRb;i++)
{ {
btCollisionObject* colObj = importer->getRigidBodyByIndex(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); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); 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); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++) for (int i=0;i<maxBodies;i++)
{ {
@@ -4539,7 +4690,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_gripperRigidbodyFixed = 0; 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() void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{ {
static btAlignedObjectArray<char> gBufferServerToClient; static btAlignedObjectArray<char> gBufferServerToClient;

30
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -432,6 +432,8 @@ struct SdfLoadedArgs
{ {
int m_numBodies; int m_numBodies;
int m_bodyUniqueIds[MAX_SDF_BODIES]; int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_numUserConstraints;
int m_userConstraintUniqueIds[MAX_SDF_BODIES];
///@todo(erwincoumans) load cameras, lights etc ///@todo(erwincoumans) load cameras, lights etc
//int m_numCameras; //int m_numCameras;
@@ -541,27 +543,13 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8, USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16,
USER_CONSTRAINT_CHANGE_MAX_FORCE=32, 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 enum EnumUserDebugDrawFlags
{ {
@@ -659,7 +647,7 @@ struct SharedMemoryCommand
struct ExternalForceArgs m_externalForceArguments; struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CalculateJacobianArgs m_calculateJacobianArguments; struct CalculateJacobianArgs m_calculateJacobianArguments;
struct UserConstraintArgs m_userConstraintArguments; struct b3UserConstraint m_userConstraintArguments;
struct RequestContactDataArgs m_requestContactPointArguments; struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs; struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments; struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
@@ -708,6 +696,10 @@ struct SharedMemoryStatus
int m_numDataStreamBytes; int m_numDataStreamBytes;
char* m_dataStream; 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 union
{ {
struct BulletDataStreamArgs m_dataStreamArguments; struct BulletDataStreamArgs m_dataStreamArguments;
@@ -723,7 +715,7 @@ struct SharedMemoryStatus
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs; struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs; struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct UserConstraintResultArgs m_userConstraintResultArgs; struct b3UserConstraint m_userConstraintResultArgs;
struct SendVREvents m_sendVREvents; struct SendVREvents m_sendVREvents;
struct SendRaycastHits m_raycastHits; 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_PARAMETER_COMPLETED,
CMD_USER_DEBUG_DRAW_FAILED, CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED, 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_USER_CONSTRAINT_FAILED,
CMD_REQUEST_VR_EVENTS_DATA_COMPLETED, CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
@@ -164,6 +169,20 @@ struct b3JointInfo
double m_jointAxis[3]; // joint axis in parent local frame 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 struct b3BodyInfo
{ {
const char* m_baseName; const char* m_baseName;

158
examples/pybullet/pybullet.c
View File

@@ -31,6 +31,7 @@ static PyObject* SpamError;
#define MAX_PHYSICS_CLIENTS 1024 #define MAX_PHYSICS_CLIENTS 1024
static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0}; static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0};
static int sPhysicsClientsGUI[MAX_PHYSICS_CLIENTS] = {0};
static int sNumPhysicsClients=0; static int sNumPhysicsClients=0;
b3PhysicsClientHandle getPhysicsClient(int physicsClientId) b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
@@ -50,6 +51,8 @@ b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
//broken connection? //broken connection?
b3DisconnectSharedMemory(sm); b3DisconnectSharedMemory(sm);
sPhysicsClients1[physicsClientId] = 0; sPhysicsClients1[physicsClientId] = 0;
sPhysicsClientsGUI[physicsClientId] = 0;
sNumPhysicsClients--; sNumPhysicsClients--;
} }
return 0; return 0;
@@ -209,6 +212,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
int freeIndex = -1; int freeIndex = -1;
int method = eCONNECT_GUI;
int i; int i;
b3PhysicsClientHandle sm=0; b3PhysicsClientHandle sm=0;
@@ -221,7 +225,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
{ {
int method = eCONNECT_GUI;
int key = SHARED_MEMORY_KEY; int key = SHARED_MEMORY_KEY;
int port = 1234; int port = 1234;
const char* hostName = "localhost"; const char* hostName = "localhost";
@@ -237,6 +241,21 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
} }
} }
//Only one local in-process GUI connection allowed.
if (method == eCONNECT_GUI)
{
int i;
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
{
if (sPhysicsClientsGUI[i] ==eCONNECT_GUI)
{
PyErr_SetString(SpamError,
"Only one local in-process GUI connection allowed. Use DIRECT connection mode or start a separate GUI physics server (ExampleBrowser, App_SharedMemoryPhysics_GUI, App_SharedMemoryPhysics_VR) and connect over SHARED_MEMORY or UDP instead.");
return NULL;
}
}
}
if (size == 2) if (size == 2)
{ {
if (!PyArg_ParseTuple(args, "ii", &method, &key)) if (!PyArg_ParseTuple(args, "ii", &method, &key))
@@ -321,6 +340,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
sPhysicsClients1[freeIndex] = sm; sPhysicsClients1[freeIndex] = sm;
sPhysicsClientsGUI[freeIndex] = method;
sNumPhysicsClients++; sNumPhysicsClients++;
command = b3InitSyncBodyInfoCommand(sm); command = b3InitSyncBodyInfoCommand(sm);
@@ -363,6 +383,7 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
} }
sPhysicsClients1[physicsClientId] = 0; sPhysicsClients1[physicsClientId] = 0;
sPhysicsClientsGUI[physicsClientId] = 0;
sNumPhysicsClients--; sNumPhysicsClients--;
Py_INCREF(Py_None); Py_INCREF(Py_None);
@@ -1557,6 +1578,7 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
} }
} }
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int physicsClientId = 0; int physicsClientId = 0;
@@ -1629,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 // Return the number of joints in an object based on
// body index; body index is based on order of sequence // body index; body index is based on order of sequence
// the object is loaded into simulation // the object is loaded into simulation
@@ -2200,14 +2338,10 @@ static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args,
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
int res = 0;
char* text; char* text;
double posXYZ[3];
double colorRGB[3]={1,1,1};
PyObject* textPositionObj=0;
double rangeMin = 0.f; double rangeMin = 0.f;
double rangeMax = 1.f; double rangeMax = 1.f;
double startValue = 0.f; double startValue = 0.f;
@@ -4521,7 +4655,7 @@ static PyMethodDef SpamMethods[] = {
"Load multibodies from an MJCF file." }, "Load multibodies from an MJCF file." },
{"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, {"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, {"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS,
@@ -4541,11 +4675,21 @@ static PyMethodDef SpamMethods[] = {
"Get the number of bodies in the simulation."}, "Get the number of bodies in the simulation."},
{"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, {"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, {"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS,
"Get the body info, given a body unique id."}, "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, {"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation,
METH_VARARGS | METH_KEYWORDS, METH_VARARGS | METH_KEYWORDS,
"Get the world position and orientation of the base of the object. " "Get the world position and orientation of the base of the object. "

22
examples/pybullet/vr_kuka_pr2_move.py Normal file
View File

@@ -0,0 +1,22 @@
#python script with hardcoded values, assumes that you run the vr_kuka_setup.py first
import pybullet as p
p.connect(p.SHARED_MEMORY)
pr2_gripper = 2
pr2_cid = 1
CONTROLLER_ID = 0
POSITION=1
ORIENTATION=2
BUTTONS=6
while True:
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]&p.VR_BUTTON_IS_DOWN):
p.changeConstraint(pr2_cid,e[POSITION],e[ORIENTATION], maxForce=50)
#todo
#p.setJointMotorControl2(pr2_gripper,0)

29
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -430,6 +430,13 @@ const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
btVector3 result = local_pos; btVector3 result = local_pos;
while (i != -1) { while (i != -1) {
// 'result' is in frame i. transform it to frame parent(i) // 'result' is in frame i. transform it to frame parent(i)
@@ -447,6 +454,13 @@ btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
if (i == -1) { if (i == -1) {
// world to base // world to base
return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
@@ -458,6 +472,14 @@ btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
btVector3 result = local_dir; btVector3 result = local_dir;
while (i != -1) { while (i != -1) {
result = quatRotate(getParentToLocalRot(i).inverse() , result); result = quatRotate(getParentToLocalRot(i).inverse() , result);
@@ -469,6 +491,13 @@ btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
if (i == -1) { if (i == -1) {
return quatRotate(getWorldToBaseRot(), world_dir); return quatRotate(getWorldToBaseRot(), world_dir);
} else { } else {