From 15e6ee1a0424490e7edd11513a08e21076e4baa5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 21 Mar 2017 20:43:23 -0700 Subject: [PATCH 1/2] add missing define in pybullet to start/stop MP4 video, pybullet.startStateLogging(STATE_LOGGING_VIDEO_MP4,"filename.mp4") --- examples/pybullet/pybullet.c | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 36ceb794c..61417868c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5368,6 +5368,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR); PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT); PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS); + PyModule_AddIntConstant(m, "STATE_LOGGING_VIDEO_MP4", STATE_LOGGING_VIDEO_MP4); PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI); PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS); From 0a654c2d58161a99825aa43c5d5bc1ec969e1a1d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 23 Mar 2017 10:29:16 -0700 Subject: [PATCH 2/2] expose optional targetVelocity to pybullet.resetJointState add C-API: b3CreatePoseCommandSetJointVelocities and b3CreatePoseCommandSetJointVelocity --- examples/SharedMemory/PhysicsClientC_API.cpp | 44 +++++++++++++++++-- examples/SharedMemory/PhysicsClientC_API.h | 3 ++ .../PhysicsServerCommandProcessor.cpp | 19 +++++--- examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/pybullet/pybullet.c | 11 +++-- 5 files changed, 66 insertions(+), 12 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5ac6ea74a..0e2cf9839 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -561,7 +561,7 @@ int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle if (bodyIndex>=0) { b3JointInfo info; - bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info); + bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info)!=0; if (result) { if ((info.m_qIndex>=0) && (info.m_uIndex>=0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM)) @@ -735,6 +735,7 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl for (int i=0;im_initPoseArgs.m_hasInitialStateQ[i] = 0; + command->m_initPoseArgs.m_hasInitialStateQdot[i] = 0; } return (b3SharedMemoryCommandHandle) command; } @@ -817,8 +818,11 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE; for (int i=0;im_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i]; - command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1; + if ((i+7)m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i]; + command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1; + } } return 0; } @@ -842,6 +846,40 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar return 0; } +int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + + command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY; + for (int i=0;im_initPoseArgs.m_initialStateQdot[i+6] = jointVelocities[i]; + command->m_initPoseArgs.m_hasInitialStateQdot[i+6] = 1; + } + } + return 0; +} + +int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY; + b3JointInfo info; + b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info); + btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0); + if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >=0) && (info.m_uIndexm_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity; + command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex] = 1; + } + return 0; +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 01eb55cc1..7aef18f86 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -301,6 +301,9 @@ int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle comman int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); +int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities); +int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity); + ///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors. ///This is rather inconsistent, to mix programmatical creation with loading from file. b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4061cea95..22469e7e6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3362,15 +3362,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) { - int dofIndex = 7; + int uDofIndex = 6; + int posVarCountIndex = 7; for (int i=0;igetNumLinks();i++) { - if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1)) + if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount==1)) { - mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]); - mb->setJointVel(i,0); + mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]); + mb->setJointVel(i,0);//backwards compatibility } - dofIndex += mb->getLink(i).m_dofCount; + if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount==1)) + { + btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]; + mb->setJointVel(i,vel); + } + + posVarCountIndex += mb->getLink(i).m_posVarCount; + uDofIndex += mb->getLink(i).m_dofCount; + } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 1c119c4d1..0f413791f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -117,6 +117,7 @@ enum EnumInitPoseFlags INIT_POSE_HAS_JOINT_STATE=4, INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8, INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16, + INIT_POSE_HAS_JOINT_VELOCITY=32, }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 61417868c..b3b462d90 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1864,11 +1864,13 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje int bodyUniqueId; int jointIndex; double targetValue; + double targetVelocity = 0; + b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "jointIndex", "targetValue", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|i", kwlist, &bodyUniqueId, &jointIndex, &targetValue, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "targetValue", "targetVelocity", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|di", kwlist, &bodyUniqueId, &jointIndex, &targetValue, &targetVelocity, &physicsClientId)) { return NULL; } @@ -1896,6 +1898,9 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); + b3CreatePoseCommandSetJointVelocity(sm, commandHandle, jointIndex, + targetVelocity); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); } } @@ -2912,8 +2917,6 @@ static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObj static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; struct b3KeyboardEventsData keyboardEventsData;