diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 39ea58041..642a2a655 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -572,7 +572,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)) @@ -746,6 +746,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; } @@ -828,8 +829,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; } @@ -853,6 +857,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 0333d4dc4..aa64dccf3 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -304,6 +304,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 5a694bdf1..2a0f36fd5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3365,15 +3365,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 1c5ecb0b1..ebb56998f 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 0807f80af..a841939d1 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); } } @@ -2975,8 +2980,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; @@ -5434,6 +5437,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);