From 754dbd5fda8fb5ed2ff367ff3f22c56885edd5b2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 17 May 2020 13:46:11 -0700 Subject: [PATCH] add pybullet.unsupportedChangeScaling feature for some small experiments, this is not general and has many limitations that are not likely are going to be resolved, so unless it does what you want, ignore this api, it is unsupported! --- .../CommonGUIHelperInterface.h | 2 + examples/ExampleBrowser/OpenGLGuiHelper.cpp | 8 +++ examples/ExampleBrowser/OpenGLGuiHelper.h | 1 + .../SharedMemory/GraphicsServerExample.cpp | 20 ++++++ .../GraphicsSharedMemoryCommands.h | 9 +++ .../SharedMemory/GraphicsSharedMemoryPublic.h | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 16 +++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 14 ++++ .../SharedMemory/PhysicsServerExample.cpp | 24 +++++++ examples/SharedMemory/RemoteGUIHelper.cpp | 5 ++ examples/SharedMemory/RemoteGUIHelper.h | 1 + examples/SharedMemory/SharedMemoryCommands.h | 2 + examples/pybullet/pybullet.c | 67 +++++++++++++++++++ 14 files changed, 172 insertions(+) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index d65cb6581..d5b72a0c8 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -45,6 +45,7 @@ struct GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeInstanceFlags(int instanceUid, int flags) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} + virtual void changeScaling(int instanceUid, const double scaling[3]) {} virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {} virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height) {} virtual void updateShape(int shapeIndex, float* vertices) {} @@ -151,6 +152,7 @@ struct DummyGUIHelper : public GUIHelperInterface virtual void removeAllGraphicsInstances() {} virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} + virtual void changeScaling(int instanceUid, const double scaling[3]) {} virtual Common2dCanvasInterface* get2dCanvasInterface() { diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 610329425..8afdad793 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -388,6 +388,14 @@ void OpenGLGuiHelper::changeInstanceFlags(int instanceUid, int flags) m_data->m_glApp->m_renderer->writeSingleInstanceFlagsToCPU( flags, instanceUid); } } +void OpenGLGuiHelper::changeScaling(int instanceUid, const double scaling[3]) +{ + if (instanceUid >= 0) + { + m_data->m_glApp->m_renderer->writeSingleInstanceScaleToCPU(scaling, instanceUid); + }; +} + void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { if (instanceUid >= 0) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index bdaa22772..8c9b0e70e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -29,6 +29,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid); virtual void changeInstanceFlags(int instanceUid, int flags); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); + virtual void changeScaling(int instanceUid, const double scaling[3]); virtual void changeSpecularColor(int instanceUid, const double specularColor[3]); virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height); virtual void removeTexture(int textureUid); diff --git a/examples/SharedMemory/GraphicsServerExample.cpp b/examples/SharedMemory/GraphicsServerExample.cpp index b63d36c65..3e92b75c4 100644 --- a/examples/SharedMemory/GraphicsServerExample.cpp +++ b/examples/SharedMemory/GraphicsServerExample.cpp @@ -488,6 +488,18 @@ void TCPThreadFunc(void* userPtr, void* lsMemory) printf("GFX_CMD_CHANGE_RGBA_COLOR\n"); break; } + case GFX_CMD_CHANGE_SCALING: + { + args->submitCommand(); + while (args->isCommandOutstanding()) + { + clock.usleep(0); + } + if (gVerboseNetworkMessagesServer) + printf("GFX_CMD_CHANGE_SCALING\n"); + break; + } + case GFX_CMD_GET_CAMERA_INFO: { args->submitCommand(); @@ -833,6 +845,14 @@ public: m_args.processCommand(); break; } + + case GFX_CMD_CHANGE_SCALING: + { + m_guiHelper->changeScaling(clientCmd.m_changeScalingCommand.m_graphicsUid, clientCmd.m_changeScalingCommand.m_scaling); + m_args.processCommand(); + break; + } + case GFX_CMD_GET_CAMERA_INFO: { serverStatusOut.m_type = GFX_CMD_GET_CAMERA_INFO_FAILED; diff --git a/examples/SharedMemory/GraphicsSharedMemoryCommands.h b/examples/SharedMemory/GraphicsSharedMemoryCommands.h index c612d9b96..2fbaf459f 100644 --- a/examples/SharedMemory/GraphicsSharedMemoryCommands.h +++ b/examples/SharedMemory/GraphicsSharedMemoryCommands.h @@ -112,6 +112,14 @@ struct GraphicsChangeRGBAColorCommand double m_rgbaColor[4]; }; +struct GraphicsChangeScalingCommand +{ + int m_graphicsUid; + double m_scaling[3]; +}; + + + struct GraphicsGetCameraInfoStatus { int width; @@ -150,6 +158,7 @@ struct GraphicsSharedMemoryCommand struct GraphicsSyncTransformsCommand m_syncTransformsCommand; struct GraphicsRemoveInstanceCommand m_removeGraphicsInstanceCommand; struct GraphicsChangeRGBAColorCommand m_changeRGBAColorCommand; + struct GraphicsChangeScalingCommand m_changeScalingCommand; }; }; diff --git a/examples/SharedMemory/GraphicsSharedMemoryPublic.h b/examples/SharedMemory/GraphicsSharedMemoryPublic.h index 486721c72..4a80736bd 100644 --- a/examples/SharedMemory/GraphicsSharedMemoryPublic.h +++ b/examples/SharedMemory/GraphicsSharedMemoryPublic.h @@ -22,6 +22,7 @@ enum EnumGraphicsSharedMemoryClientCommand GFX_CMD_REMOVE_SINGLE_GRAPHICS_INSTANCE, GFX_CMD_CHANGE_RGBA_COLOR, GFX_CMD_GET_CAMERA_INFO, + GFX_CMD_CHANGE_SCALING, //don't go beyond this command! GFX_CMD_MAX_CLIENT_COMMANDS, }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e65a7be93..b3f4d4552 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2240,6 +2240,22 @@ B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle return 0; } +B3_SHARED_API int b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle, double scaling[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_SCALING; + + command->m_initPoseArgs.m_scaling[0] = scaling[0]; + command->m_initPoseArgs.m_scaling[1] = scaling[1]; + command->m_initPoseArgs.m_scaling[2] = scaling[2]; + + return 0; +} + + + B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 41d1a9fba..560546736 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -574,6 +574,8 @@ extern "C" B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, const double linVel[/*3*/]); B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, const double angVel[/*3*/]); + B3_SHARED_API int b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle, double scaling[/* 3*/]); + B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 309fbc574..17dac827e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10032,6 +10032,20 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe { btMultiBody* mb = body->m_multiBody; + if (clientCmd.m_updateFlags & INIT_POSE_HAS_SCALING) + { + btVector3 scaling(clientCmd.m_initPoseArgs.m_scaling[0], clientCmd.m_initPoseArgs.m_scaling[1], clientCmd.m_initPoseArgs.m_scaling[2]); + + mb->getBaseCollider()->getCollisionShape()->setLocalScaling(scaling); + //refresh broadphase + m_data->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs( + mb->getBaseCollider()->getBroadphaseHandle(), + m_data->m_dynamicsWorld->getDispatcher()); + //also visuals + int graphicsIndex = mb->getBaseCollider()->getUserIndex(); + m_data->m_guiHelper->changeScaling(graphicsIndex, clientCmd.m_initPoseArgs.m_scaling); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) { mb->setBaseVel(baseLinVel); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 7cde642a2..3a159f581 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -128,6 +128,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperRemoveTexture, eGUIHelperSetVisualizerFlagCheckRenderedFrame, eGUIHelperUpdateShape, + eGUIHelperChangeGraphicsInstanceScaling, eGUIUserDebugRemoveAllParameters, }; @@ -1035,6 +1036,19 @@ public: workerThreadWait(); } + int m_graphicsInstanceChangeScaling; + double m_baseScaling[3]; + virtual void changeScaling(int instanceUid, const double scaling[3]) + { + m_graphicsInstanceChangeScaling = instanceUid; + m_baseScaling[0] = scaling[0]; + m_baseScaling[1] = scaling[1]; + m_baseScaling[2] = scaling[2]; + m_cs->lock(); + m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceScaling); + workerThreadWait(); + } + double m_specularColor[3]; int m_graphicsInstanceChangeSpecular; virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) @@ -2181,6 +2195,16 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->mainThreadRelease(); break; } + + case eGUIHelperChangeGraphicsInstanceScaling: + { + B3_PROFILE("eGUIHelperChangeGraphicsInstanceScaling"); + + m_multiThreadedHelper->m_childGuiHelper->changeScaling(m_multiThreadedHelper->m_graphicsInstanceChangeScaling, m_multiThreadedHelper->m_baseScaling); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + case eGUIHelperChangeGraphicsInstanceSpecularColor: { B3_PROFILE("eGUIHelperChangeGraphicsInstanceSpecularColor"); diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp index a7bed235d..b5754e185 100644 --- a/examples/SharedMemory/RemoteGUIHelper.cpp +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -529,6 +529,11 @@ void RemoteGUIHelper::removeGraphicsInstance(int graphicsUid) } } } + +void RemoteGUIHelper::changeScaling(int instanceUid, const double scaling[3]) +{ + +} void RemoteGUIHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); diff --git a/examples/SharedMemory/RemoteGUIHelper.h b/examples/SharedMemory/RemoteGUIHelper.h index 1931091e5..3d74b03fd 100644 --- a/examples/SharedMemory/RemoteGUIHelper.h +++ b/examples/SharedMemory/RemoteGUIHelper.h @@ -38,6 +38,7 @@ struct RemoteGUIHelper : public GUIHelperInterface virtual void removeAllGraphicsInstances(); virtual void removeGraphicsInstance(int graphicsUid); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); + virtual void changeScaling(int instanceUid, const double scaling[3]); virtual Common2dCanvasInterface* get2dCanvasInterface(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 3ab8776c9..b892df048 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -215,6 +215,7 @@ enum EnumInitPoseFlags INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8, INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16, INIT_POSE_HAS_JOINT_VELOCITY = 32, + INIT_POSE_HAS_SCALING=64, }; ///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position @@ -228,6 +229,7 @@ struct InitPoseArgs double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_scaling[3]; }; struct RequestDebugLinesArgs diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e5c97d7fd..f0789b258 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4891,6 +4891,67 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, return Py_None; } +static PyObject* pybullet_changeScaling(PyObject* self, + PyObject* args, PyObject* keywds) +{ + { + int bodyUniqueId; + PyObject* scalingObj; + double scaling[3]; + + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "scaling", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist, &bodyUniqueId, &scalingObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(scalingObj, "expected a sequence"); + len = PySequence_Size(scalingObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + scaling[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "scaling needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + + + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + b3CreatePoseCommandSetBaseScaling(commandHandle, scaling); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + } + Py_INCREF(Py_None); + return Py_None; +} + + // Get the a single joint info for a specific bodyUniqueId // // Args: @@ -12218,6 +12279,12 @@ static PyMethodDef SpamMethods[] = { "Reset the world position and orientation of the base of the object " "instantaneously, not through physics simulation. (x,y,z) position vector " "and (x,y,z,w) quaternion orientation."}, + + { "unsupportedChangeScaling", + (PyCFunction)pybullet_changeScaling, METH_VARARGS | METH_KEYWORDS, + "Change the scaling of the base of an object." + "Warning: unsupported rudimentary feature that has many limitations." + }, {"getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, METH_VARARGS | METH_KEYWORDS,