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!
This commit is contained in:
Erwin Coumans 2020-05-17 13:46:11 -07:00
parent 004dcc3404
commit 754dbd5fda
14 changed files with 172 additions and 0 deletions

View File

@ -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()
{

View File

@ -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)

View File

@ -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);

View File

@ -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;

View File

@ -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;
};
};

View File

@ -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,
};

View File

@ -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;

View File

@ -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);

View File

@ -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);

View File

@ -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");

View File

@ -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();

View File

@ -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();

View File

@ -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

View File

@ -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,