diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e60953750..24d7e298c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -672,6 +672,40 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan return 0; } +int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY; + command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1; + + command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0]; + command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1]; + command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2]; + + return 0; +} + +int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY; + command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1; + + command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0]; + command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1]; + command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2]; + + return 0; +} + int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -686,6 +720,8 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand return 0; } + + int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b9da28cd5..f8377c089 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -256,6 +256,9 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]); +int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]); + int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 18231566f..d0a3dca38 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2342,6 +2342,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; + btVector3 baseLinVel(0, 0, 0); + btVector3 baseAngVel(0, 0, 0); + + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], + clientCmd.m_initPoseArgs.m_initialStateQdot[1], + clientCmd.m_initPoseArgs.m_initialStateQdot[2]); + mb->setBaseVel(baseLinVel); + + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], + clientCmd.m_initPoseArgs.m_initialStateQdot[4], + clientCmd.m_initPoseArgs.m_initialStateQdot[5]); + mb->setBaseOmega(baseAngVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) { btVector3 zero(0,0,0); @@ -2349,7 +2371,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); - mb->setBaseVel(zero); + mb->setBaseVel(baseLinVel); mb->setBasePos(btVector3( clientCmd.m_initPoseArgs.m_initialStateQ[0], clientCmd.m_initPoseArgs.m_initialStateQ[1], @@ -2362,7 +2384,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); - mb->setBaseOmega(btVector3(0,0,0)); + mb->setBaseOmega(baseAngVel); btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3], clientCmd.m_initPoseArgs.m_initialStateQ[4], clientCmd.m_initPoseArgs.m_initialStateQ[5], diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5b3346433..5d25059c2 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -109,7 +109,9 @@ enum EnumInitPoseFlags { INIT_POSE_HAS_INITIAL_POSITION=1, INIT_POSE_HAS_INITIAL_ORIENTATION=2, - INIT_POSE_HAS_JOINT_STATE=4 + INIT_POSE_HAS_JOINT_STATE=4, + INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8, + INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16, }; @@ -122,6 +124,8 @@ struct InitPoseArgs int m_bodyUniqueId; int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; + int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM]; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 61b57c083..7aeaab7e0 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -30,6 +30,123 @@ enum eCONNECT_METHOD { static PyObject* SpamError; static b3PhysicsClientHandle sm = 0; + +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { + double v = 0.0; + PyObject* item; + + if (PyList_Check(seq)) { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + else { + item = PyTuple_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + return v; +} + + +// internal function to set a float matrix[16] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// matrix - float[16] which will be set by values from objMat +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { + int i, len; + PyObject* seq; + + seq = PySequence_Fast(objMat, "expected a sequence"); + if (seq) + { + len = PySequence_Size(objMat); + if (len == 16) { + for (i = 0; i < len; i++) { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// internal function to set a float vector[3] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// vector - float[3] which will be set by values from objMat +static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { + int i, len; + PyObject* seq = 0; + if (objVec == NULL) + return 0; + + seq = PySequence_Fast(objVec, "expected a sequence"); + if (seq) + { + + len = PySequence_Size(objVec); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// vector - double[3] which will be set by values from obVec +static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + if (seq) + { + len = PySequence_Size(obVec); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// vector - double[3] which will be set by values from obVec +static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + len = PySequence_Size(obVec); + if (len == 4) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + return 0; +} + + + // Step through one timestep of the simulation static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { if (0 == sm) { @@ -371,19 +488,6 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { return PyLong_FromLong(bodyIndex); } -static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { - double v = 0.0; - PyObject* item; - - if (PyList_Check(seq)) { - item = PyList_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } else { - item = PyTuple_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } - return v; -} @@ -768,11 +872,68 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args) return Py_None; } +static int pybullet_internalGetBaseVelocity( + int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) { + baseLinearVelocity[0] = 0.; + baseLinearVelocity[1] = 0.; + baseLinearVelocity[2] = 0.; + + baseAngularVelocity[0] = 0.; + baseAngularVelocity[1] = 0.; + baseAngularVelocity[2] = 0.; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return 0; + } + + { + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQdot; + // const double* jointReactionForces[]; + + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBaseVelocity failed."); + return 0; + } + + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); + + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } + // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] + // and orientation x,y,z,w = + // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] + baseLinearVelocity[0] = actualStateQdot[0]; + baseLinearVelocity[1] = actualStateQdot[1]; + baseLinearVelocity[2] = actualStateQdot[2]; + + baseAngularVelocity[0] = actualStateQdot[3]; + baseAngularVelocity[1] = actualStateQdot[4]; + baseAngularVelocity[2] = actualStateQdot[5]; + + } + } + return 1; +} // Internal function used to get the base position and orientation // Orientation is returned in quaternions static int pybullet_internalGetBasePositionAndOrientation( - int bodyIndex, double basePosition[3], double baseOrientation[3]) { + int bodyIndex, double basePosition[3], double baseOrientation[4]) { basePosition[0] = 0.; basePosition[1] = 0.; basePosition[2] = 0.; @@ -855,8 +1016,7 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, if (0 == pybullet_internalGetBasePositionAndOrientation( bodyIndex, basePosition, baseOrientation)) { PyErr_SetString(SpamError, - "GetBasePositionAndOrientation failed (#joints/links " - "exceeds maximum?)."); + "GetBasePositionAndOrientation failed."); return NULL; } @@ -891,6 +1051,62 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, } } + +static PyObject* pybullet_getBaseVelocity(PyObject* self, + PyObject* args) { + int bodyIndex = -1; + double baseLinearVelocity[3]; + double baseAngularVelocity[3]; + PyObject* pylistLinVel=0; + PyObject* pylistAngVel=0; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + + if (0 == pybullet_internalGetBaseVelocity( + bodyIndex, baseLinearVelocity, baseAngularVelocity)) { + PyErr_SetString(SpamError, + "getBaseVelocity failed."); + return NULL; + } + + { + PyObject* item; + int i; + int num = 3; + pylistLinVel = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseLinearVelocity[i]); + PyTuple_SetItem(pylistLinVel, i, item); + } + } + + { + PyObject* item; + int i; + int num = 3; + pylistAngVel = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseAngularVelocity[i]); + PyTuple_SetItem(pylistAngVel, i, item); + } + } + + { + PyObject* pylist; + pylist = PyTuple_New(2); + PyTuple_SetItem(pylist, 0, pylistLinVel); + PyTuple_SetItem(pylist, 1, pylistAngVel); + return pylist; + } +} static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args) { if (0 == sm) { @@ -1035,6 +1251,66 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { return NULL; } + + + +static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds) +{ + static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL }; + + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int bodyIndex=0; + PyObject* linVelObj=0; + PyObject* angVelObj=0; + double linVel[3] = { 0, 0, 0 }; + double angVel[3] = { 0, 0, 0 }; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj)) + { + return NULL; + } + if (linVelObj || angVelObj) + { + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + if (linVelObj) + { + pybullet_internalSetVectord(linVelObj, linVel); + b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVel); + } + + if (angVelObj) + { + pybullet_internalSetVectord(angVelObj, angVel); + b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVel); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + else + { + PyErr_SetString(SpamError, "expected at least linearVelocity and/or angularVelocity."); + return NULL; + } + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} + + + // Reset the position and orientation of the base/root link, position [x,y,z] // and orientation quaternion [x,y,z,w] static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, @@ -1366,105 +1642,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) { return Py_None; } -// internal function to set a float matrix[16] -// used to initialize camera position with -// a view and projection matrix in renderImage() -// -// // Args: -// matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { - int i, len; - PyObject* seq; - - seq = PySequence_Fast(objMat, "expected a sequence"); - if (seq) - { - len = PySequence_Size(objMat); - if (len == 16) { - for (i = 0; i < len; i++) { - matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// internal function to set a float vector[3] -// used to initialize camera position with -// a view and projection matrix in renderImage() -// -// // Args: -// vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { - int i, len; - PyObject* seq=0; - if (objVec==NULL) - return 0; - - seq = PySequence_Fast(objVec, "expected a sequence"); - if (seq) - { - - len = PySequence_Size(objVec); - if (len == 3) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { - int i, len; - PyObject* seq; - if (obVec==NULL) - return 0; - - seq = PySequence_Fast(obVec, "expected a sequence"); - if (seq) - { - len = PySequence_Size(obVec); - if (len == 3) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { - int i, len; - PyObject* seq; - if (obVec==NULL) - return 0; - - seq = PySequence_Fast(obVec, "expected a sequence"); - len = PySequence_Size(obVec); - if (len == 4) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - return 0; -} - - static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) { @@ -3361,6 +3538,19 @@ static PyMethodDef SpamMethods[] = { "instantaneously, not through physics simulation. (x,y,z) position vector " "and (x,y,z,w) quaternion orientation."}, + { "getBaseVelocity", pybullet_getBaseVelocity, + METH_VARARGS, + "Get the linear and angular velocity of the base of the object " + " in world space coordinates. " + "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." }, + + { "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS, + "Reset the linear and/or angular velocity of the base of the object " + " in world space coordinates. " + "linearVelocity (x,y,z) and angularVelocity (x,y,z)." }, + + + {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."},