From 013dbda023eb728646c98d4d666c3dbba8a67a3c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 26 Jun 2016 18:18:30 -0700 Subject: [PATCH] implement a few more pybullet methods: pybullet_applyExternalForce, pybullet_applyExternalTorque, pybullet_setTimeStep, pybullet_resetBasePositionAndOrientation, pybullet_getQuaternionFromEuler, pybullet_getEulerFromQuaternion --- examples/SharedMemory/PhysicsClientC_API.cpp | 47 ++ examples/SharedMemory/PhysicsClientC_API.h | 5 + .../PhysicsServerCommandProcessor.cpp | 64 ++ examples/SharedMemory/SharedMemoryCommands.h | 22 + examples/SharedMemory/SharedMemoryPublic.h | 7 + examples/SharedMemory/premake4.lua | 2 + examples/pybullet/pybullet.c | 549 +++++++++++++----- 7 files changed, 547 insertions(+), 149 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 203e0ebbe..e67a2c144 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -903,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage } } +b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_APPLY_EXTERNAL_FORCE; + command->m_updateFlags = 0; + command->m_externalForceArguments.m_numForcesAndTorques = 0; + return (b3SharedMemoryCommandHandle) command; +} + +void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE); + int index = command->m_externalForceArguments.m_numForcesAndTorques; + command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId; + command->m_externalForceArguments.m_linkIds[index] = linkId; + command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag; + for (int i = 0; i < 3; ++i) { + command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i]; + command->m_externalForceArguments.m_positions[index+i] = position[i]; + } + + command->m_externalForceArguments.m_numForcesAndTorques++; +} + +void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE); + int index = command->m_externalForceArguments.m_numForcesAndTorques; + command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId; + command->m_externalForceArguments.m_linkIds[index] = linkId; + command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag; + + for (int i = 0; i < 3; ++i) { + command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i]; + } + command->m_externalForceArguments.m_numForcesAndTorques++; +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index aee561d8f..8f57f4db9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -159,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d double rayToWorldZ); b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient); +/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates. +b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); +void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); +void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); + #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 99b883454..6ead18363 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2056,6 +2056,70 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_APPLY_EXTERNAL_FORCE: + { + for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) + { + InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0); + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i*3+0], + clientCmd.m_externalForceArguments.m_positions[i*3+1], + clientCmd.m_externalForceArguments.m_positions[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal; + mb->addBaseForce(forceWorld); + mb->addBaseTorque(relPosWorld.cross(forceWorld)); + //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal; + btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal; + mb->addLinkForce(link, forceWorld); + mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); + //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); + } + } + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; + mb->addBaseTorque(torqueWorld); + //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal; + mb->addLinkTorque(link, torqueWorld); + //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + } default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9745b8eb6..3ddb11e91 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -319,6 +319,27 @@ struct SdfRequestInfoArgs int m_bodyUniqueId; }; +///flags for b3ApplyExternalTorque and b3ApplyExternalForce +enum EnumExternalForcePrivateFlags +{ +// EF_LINK_FRAME=1, +// EF_WORLD_FRAME=2, + EF_TORQUE=4, + EF_FORCE=8, +}; + + + +struct ExternalForceArgs +{ + int m_numForcesAndTorques; + int m_bodyUniqueIds[MAX_SDF_BODIES]; + int m_linkIds[MAX_SDF_BODIES]; + double m_forcesAndTorques[3*MAX_SDF_BODIES]; + double m_positions[3*MAX_SDF_BODIES]; + int m_forceFlags[MAX_SDF_BODIES]; +}; + enum EnumSdfRequestInfoFlags { SDF_REQUEST_INFO_BODY=1, @@ -350,6 +371,7 @@ struct SharedMemoryCommand struct RequestDebugLinesArgs m_requestDebugLinesArguments; struct RequestPixelDataArgs m_requestPixelDataArguments; struct PickBodyArgs m_pickBodyArguments; + struct ExternalForceArgs m_externalForceArguments; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 5e6da5c50..3aeeb8f11 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -26,6 +26,7 @@ enum EnumSharedMemoryClientCommand CMD_MOVE_PICKED_BODY, CMD_REMOVE_PICKING_CONSTRAINT_BODY, CMD_REQUEST_CAMERA_IMAGE_DATA, + CMD_APPLY_EXTERNAL_FORCE, CMD_MAX_CLIENT_COMMANDS }; @@ -143,5 +144,11 @@ enum { CONTROL_MODE_POSITION_VELOCITY_PD, }; +///flags for b3ApplyExternalTorque and b3ApplyExternalForce +enum EnumExternalForceFlags +{ + EF_LINK_FRAME=1, + EF_WORLD_FRAME=2, +}; #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index e923b412f..34aea2de5 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -26,6 +26,8 @@ files { "PhysicsServer.h", "main.cpp", "PhysicsClientC_API.cpp", + "SharedMemoryCommands.h", + "SharedMemoryPublic.h", "PhysicsServer.cpp", "PosixSharedMemory.cpp", "Win32SharedMemory.cpp", diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 78e770dbe..5cdf39a59 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -437,6 +437,36 @@ pybullet_setGravity(PyObject* self, PyObject* args) return Py_None; } +static PyObject * +pybullet_setTimeStep(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double timeStep=0.001; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "d", &timeStep)) + { + PyErr_SetString(SpamError, "setTimeStep expected a single value (double)."); + return NULL; + } + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + // Internal function used to get the base position and orientation @@ -632,140 +662,88 @@ pybullet_resetJointState(PyObject* self, PyObject* args) return NULL; } - - -// CURRENTLY NOT SUPPORTED -// Initalize a single joint position for a specific body index -// -// This method skips any physics simulation and -// teleports all joints to the new positions. - -// TODO(hellojas): initializing one joint currently not supported -// static PyObject* -// pybullet_initializeJointPosition(PyObject* self, PyObject* args) -// { -// if (0==sm) -// { -// PyErr_SetString(SpamError, "Not connected to physics server."); -// return NULL; -// } -// -// int i; -// int bodyIndex = -1; -// int jointIndex = -1; -// double jointPos = 0.0; -// -// int size= PySequence_Size(args); -// -// if (size==3) // get body index, joint index, and joint position value -// { -// if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos)) -// { -// b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); -// -// // printf("initializing joint %d at %f\n", jointIndex, jointPos); -// b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); -// -// b3SharedMemoryStatusHandle status_handle = -// b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); -// -// const int status_type = b3GetStatusType(status_handle); -// -// } -// } -// -// Py_INCREF(Py_None); -// return Py_None; -// } -#if 0 -static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) { - int i, j; - int numDegreeQ; - int numDegreeU; - int arrSizeOfPosAndOrn = 7; - - for (i =0;i euler yaw/pitch/roll convention from URDF/SDF, see Gazebo +///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc +static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) +{ + + double squ; + double sqx; + double sqy; + double sqz; + + double quat[4]; + + PyObject* quatObj; + + if (PyArg_ParseTuple(args, "O", &quatObj)) + { + PyObject* seq; + int len,i; + seq = PySequence_Fast(quatObj, "expected a sequence"); + len = PySequence_Size(quatObj); + if (len==4) + { + for (i = 0; i < 4; i++) + { + quat[i] = pybullet_internalGetFloatFromSequence(seq,i); + } + } else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + return NULL; + } + + { + double rpy[3]; + sqx = quat[0] * quat[0]; + sqy = quat[1] * quat[1]; + sqz = quat[2] * quat[2]; + squ = quat[3] * quat[3]; + rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz); + double sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]); + rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg)); + rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz); + { + PyObject *pylist; + int i; + pylist = PyTuple_New(3); + for (i=0;i<3;i++) + PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i])); + return pylist; + } + } + Py_INCREF(Py_None); + return Py_None; +} + static PyMethodDef SpamMethods[] = { @@ -1109,6 +1343,9 @@ static PyMethodDef SpamMethods[] = { {"setGravity", pybullet_setGravity, METH_VARARGS, "Set the gravity acceleration (x,y,z)."}, + {"setTimeStep", pybullet_setTimeStep, METH_VARARGS, + "Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"}, + {"loadURDF", pybullet_loadURDF, METH_VARARGS, "Create a multibody by loading a URDF file."}, @@ -1118,8 +1355,8 @@ static PyMethodDef SpamMethods[] = { {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, -// {"resetBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, -// "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."}, + {"resetBasePositionAndOrientation", pybullet_resetBasePositionAndOrientation, METH_VARARGS, + "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."}, {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."}, @@ -1136,18 +1373,29 @@ static PyMethodDef SpamMethods[] = { {"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS, "Set a single joint motor control mode and desired target value. There is no immediate state change, stepSimulation will process the motors."}, + {"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS, + "for objectUniqueId, linkIndex (-1 for base/root link), apply a force [x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or FORCE_IN_WORLD_FRAME coordinates"}, + + {"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS, + "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque [x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or TORQUE_IN_WORLD_FRAME coordinates"}, + + {"renderImage", pybullet_renderImage, METH_VARARGS, + "Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"}, + + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, + "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"}, + + {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS, + "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"}, + + //todo(erwincoumans) //saveSnapshot //loadSnapshot + ////todo(erwincoumans) //collision info //raycast info - - //applyBaseForce - //applyLinkForce - - {"renderImage", pybullet_renderImage, METH_VARARGS, - "Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"}, - + {NULL, NULL, 0, NULL} /* Sentinel */ }; @@ -1155,7 +1403,7 @@ static PyMethodDef SpamMethods[] = { static struct PyModuleDef moduledef = { PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ - "Python bindings for Bullet", /* m_doc */ + "Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)", /* m_doc */ -1, /* m_size */ SpamMethods, /* m_methods */ NULL, /* m_reload */ @@ -1198,6 +1446,9 @@ initpybullet(void) PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read + PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME); + PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); PyModule_AddObject(m, "error", SpamError);